1.1 --- a/src/geom.cc	Tue Aug 25 00:38:00 2015 +0300
     1.2 +++ b/src/geom.cc	Tue Aug 25 02:26:01 2015 +0300
     1.3 @@ -256,6 +256,33 @@
     1.4  	return (pt - cent).length() - rad;
     1.5  }
     1.6  
     1.7 +// TODO version which takes both radii into account
     1.8 +float capsule_distance(const Vector3 &a, float ra, const Vector3 &b, float rb, const Vector3 &pt)
     1.9 +{
    1.10 +	Vector3 ab_dir = b - a;
    1.11 +	float ab_len_sq = ab_dir.length_sq();
    1.12 +
    1.13 +	if(fabs(ab_len_sq) < 1e-5) {
    1.14 +		// if a == b, the capsule is a sphere with radius the maximum of the capsule radii
    1.15 +		return sphere_distance(a, std::max(ra, rb), pt);
    1.16 +	}
    1.17 +	float ab_len = sqrt(ab_len_sq);
    1.18 +
    1.19 +	Vector3 ap_dir = pt - a;
    1.20 +
    1.21 +	float t = dot_product(ap_dir, ab_dir / ab_len) / ab_len;
    1.22 +	if(t < 0.0) {
    1.23 +		return sphere_distance(a, ra, pt);
    1.24 +	}
    1.25 +	if(t >= 1.0) {
    1.26 +		return sphere_distance(b, rb, pt);
    1.27 +	}
    1.28 +
    1.29 +	Vector3 pproj = a + ab_dir * t;
    1.30 +	return (pproj - pt).length() - ra;
    1.31 +}
    1.32 +
    1.33 +#if 0
    1.34  float capsule_distance(const Vector3 &a, float ra, const Vector3 &b, float rb, const Vector3 &pt)
    1.35  {
    1.36  	Vector3 ab_dir = b - a;
    1.37 @@ -304,3 +331,4 @@
    1.38  	}
    1.39  	return dist;
    1.40  }
    1.41 +#endif