- Vec3 ab_dir = b - a;
-
- if(fabs(length_sq(ab_dir)) < 1e-5) {
- // if a == b, the capsule is a sphere with radius the maximum of the capsule radii
- return sphere_distance(a, std::max(ra, rb), pt);
- }
- float ab_len = length(ab_dir);
-
- Vec3 ap_dir = pt - a;
- Vec3 rotaxis = normalize(cross(ab_dir, ap_dir));
-
- Mat4 rmat;
- rmat.set_rotation(rotaxis, M_PI / 2.0);
- Vec3 right = rmat * ab_dir / ab_len;
-
- // XXX I think this check is redundant, always false, due to the cross product order
- //assert(dot(right, ab_dir) >= 0.0);
- if(dot(right, ab_dir) < 0.0) {
- right = -right;
- }
- Vec3 aa = a + right * ra;
- Vec3 bb = b + right * rb;
-
- // project pt to the line segment bb-aa, see if the projection lies within the interval [0, 1)
- Vec3 aabb_dir = bb - aa;
- float aabb_len = length(aabb_dir);
- Vec3 aap_dir = pt - aa;
-
- float t = dot(aap_dir, aabb_dir / aabb_len) / aabb_len;
- if(t < 0.0) {
- return sphere_distance(a, ra, pt);
- }
- if(t >= 1.0) {
- return sphere_distance(b, rb, pt);
- }
-
- Vec3 ppt = aa + aabb_dir * t;
- Vec3 norm = ppt - pt;
- float dist = length(norm);
-
- if(dot(norm, right) < 0.0) {
- // inside the cone
- dist = -dist;
- }
- return dist;