ld33_umonster
diff src/geom.cc @ 7:92d662deb66e
capsule distance seems broken
author | John Tsiombikas <nuclear@member.fsf.org> |
---|---|
date | Tue, 25 Aug 2015 00:38:00 +0300 |
parents | 4a6683050e29 |
children | bed39534d471 |
line diff
1.1 --- a/src/geom.cc Sun Aug 23 05:37:09 2015 +0300 1.2 +++ b/src/geom.cc Tue Aug 25 00:38:00 2015 +0300 1.3 @@ -1,5 +1,6 @@ 1.4 +#include <assert.h> 1.5 +#include <float.h> 1.6 #include <algorithm> 1.7 -#include <float.h> 1.8 #include "geom.h" 1.9 1.10 GeomObject::~GeomObject() 1.11 @@ -249,3 +250,57 @@ 1.12 } 1.13 return true; 1.14 } 1.15 + 1.16 +float sphere_distance(const Vector3 ¢, float rad, const Vector3 &pt) 1.17 +{ 1.18 + return (pt - cent).length() - rad; 1.19 +} 1.20 + 1.21 +float capsule_distance(const Vector3 &a, float ra, const Vector3 &b, float rb, const Vector3 &pt) 1.22 +{ 1.23 + Vector3 ab_dir = b - a; 1.24 + 1.25 + if(fabs(ab_dir.length_sq()) < 1e-5) { 1.26 + // if a == b, the capsule is a sphere with radius the maximum of the capsule radii 1.27 + return sphere_distance(a, std::max(ra, rb), pt); 1.28 + } 1.29 + float ab_len = ab_dir.length(); 1.30 + 1.31 + Vector3 ap_dir = pt - a; 1.32 + Vector3 rotaxis = cross_product(ab_dir, ap_dir).normalized(); 1.33 + 1.34 + Matrix4x4 rmat; 1.35 + rmat.set_rotation(rotaxis, M_PI / 2.0); 1.36 + Vector3 right = ab_dir.transformed(rmat) / ab_len; 1.37 + 1.38 + // XXX I think this check is redundant, always false, due to the cross product order 1.39 + //assert(dot_product(right, ab_dir) >= 0.0); 1.40 + if(dot_product(right, ab_dir) < 0.0) { 1.41 + right = -right; 1.42 + } 1.43 + Vector3 aa = a + right * ra; 1.44 + Vector3 bb = b + right * rb; 1.45 + 1.46 + // project pt to the line segment bb-aa, see if the projection lies within the interval [0, 1) 1.47 + Vector3 aabb_dir = bb - aa; 1.48 + float aabb_len = aabb_dir.length(); 1.49 + Vector3 aap_dir = pt - aa; 1.50 + 1.51 + float t = dot_product(aap_dir, aabb_dir / aabb_len) / aabb_len; 1.52 + if(t < 0.0) { 1.53 + return sphere_distance(a, ra, pt); 1.54 + } 1.55 + if(t >= 1.0) { 1.56 + return sphere_distance(b, rb, pt); 1.57 + } 1.58 + 1.59 + Vector3 ppt = aa + aabb_dir * t; 1.60 + Vector3 norm = ppt - pt; 1.61 + float dist = norm.length(); 1.62 + 1.63 + if(dot_product(norm, right) < 0.0) { 1.64 + // inside the cone 1.65 + dist = -dist; 1.66 + } 1.67 + return dist; 1.68 +}