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 &cent, 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 +}