// find distance x0 is from triangle x1-x2-x3
float point_triangle_distance(
const Vec3f &x0,
const Vec3f &x1, const Vec3f &x2, const Vec3f &x3,
float * baryCentricCoordinates)
{
// first find barycentric coordinates of closest point on infinite plane
Vec3f x13(x1-x3), x23(x2-x3), x03(x0-x3);
float m13=mag2(x13), m23=mag2(x23), d=dot(x13,x23);
float invdet=1.f/max(m13*m23-d*d,1e-30f);
float a=dot(x13,x03), b=dot(x23,x03);
// the barycentric coordinates themselves
float w23=invdet*(m23*a-d*b);
float w31=invdet*(m13*b-d*a);
float w12=1-w23-w31;
if(baryCentricCoordinates!=0){
baryCentricCoordinates[0] = w23;
baryCentricCoordinates[1] = w31;
baryCentricCoordinates[2] = w12;
}
if(w23>=0 && w31>=0 && w12>=0){ // if we're inside the triangle
return dist(x0, w23*x1+w31*x2+w12*x3);
}else{ // we have to clamp to one of the edges
if(w23>0) // this rules out edge 2-3 for us
return min(point_segment_distance(x0,x1,x2), point_segment_distance(x0,x1,x3));
else if(w31>0) // this rules out edge 1-3
return min(point_segment_distance(x0,x1,x2), point_segment_distance(x0,x2,x3));
else // w12 must be >0, ruling out edge 1-2
return min(point_segment_distance(x0,x1,x3), point_segment_distance(x0,x2,x3));
}
}
// find distance x0 is from triangle x1-x2-x3
static FLOAT64 point_triangle_distance(const vec3d &x0, const vec3d &x1, const vec3d &x2, const vec3d &x3, vec3d &out ) {
// first find barycentric coordinates of closest point on infinite plane
vec3d x13(x1-x3), x23(x2-x3), x03(x0-x3);
FLOAT64 m13=x13.len2(), m23=x23.len2(), d=x13*x23;
FLOAT64 invdet=1.0/fmax(m13*m23-d*d,1e-30);
FLOAT64 a=x13*x03, b=x23*x03;
// the barycentric coordinates themselves
FLOAT64 w23=invdet*(m23*a-d*b);
FLOAT64 w31=invdet*(m13*b-d*a);
FLOAT64 w12=1-w23-w31;
if(w23>=0 && w31>=0 && w12>=0){ // if we're inside the triangle
out = w23*x1+w31*x2+w12*x3;
return (x0-out).len();
}else{ // we have to clamp to one of the edges
if(w23>0) // this rules out edge 2-3 for us
return fmin(point_segment_distance(x0,x1,x2,out), point_segment_distance(x0,x1,x3,out));
else if(w31>0) // this rules out edge 1-3
return fmin(point_segment_distance(x0,x1,x2,out), point_segment_distance(x0,x2,x3,out));
else // w12 must be >0, ruling out edge 1-2
return fmin(point_segment_distance(x0,x1,x3,out), point_segment_distance(x0,x2,x3,out));
}
}
