src/PointTriangle.h
#define vecdot(a,b) ((a).x*(b).x + (a).y*(b).y + (a).z*(b).z)
#define vecdotproduct(a,b) ((coord){(a).y*(b).z - (a).z*(b).y, \
(a).z*(b).x - (a).x*(b).z, \
(a).x*(b).y - (a).y*(b).x})
#define vecdiff(a,b) ((coord){(a).x - (b).x, (a).y - (b).y, (a).z - (b).z})
#define vecdist2(a,b) (sq((a).x - (b).x) + sq((a).y - (b).y) + sq((a).z - (b).z))- Returns the squared distance between point P and triangle P0, P1,
- P2. Squared distance is returned in d2. s and t returns the
- closest point in parametric form in terms of edges P0P1 and P0P2.
double PointTriangleDistance (const coord * P,
const coord * P0,
const coord * P1,
const coord * P2,
double * s, double * t)
{
// From http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
double d2;
coord diff = vecdiff (*P0, *P);
coord edge0 = vecdiff(*P1, *P0);
coord edge1 = vecdiff(*P2, *P0);
double a00 = vecdot(edge0, edge0);
double a01 = vecdot(edge0, edge1);
double a11 = vecdot(edge1, edge1);
double b0 = vecdot(diff, edge0);
double b1 = vecdot(diff, edge1);
double c = vecdot(diff, diff);
double det = fabs(a00*a11 - a01*a01);
*s = a01*b1 - a11*b0;
*t = a01*b0 - a00*b1;
if (*s + *t <= det)
{
if (*s < 0.0)
{
if (*t < 0.0) // region 4
{
if (b0 < 0.0)
{
*t = 0.0;
if (-b0 >= a00)
{
*s = 1.0;
d2 = a00 + 2.0*b0 + c;
}
else
{
*s = -b0/a00;
d2 = b0**s + c;
}
}
else
{
*s = 0.0;
if (b1 >= 0.0)
{
*t = 0.0;
d2 = c;
}
else if (-b1 >= a11)
{
*t = 1.0;
d2 = a11 + 2.0*b1 + c;
}
else
{
*t = -b1/a11;
d2 = b1**t + c;
}
}
}
else // region 3
{
*s = 0.0;
if (b1 >= 0.0)
{
*t = 0.0;
d2 = c;
}
else if (-b1 >= a11)
{
*t = 1.0;
d2 = a11 + 2.0*b1 + c;
}
else
{
*t = -b1/a11;
d2 = b1**t + c;
}
}
}
else if (*t < 0.0) // region 5
{
*t = 0.0;
if (b0 >= 0.0)
{
*s = 0.0;
d2 = c;
}
else if (-b0 >= a00)
{
*s = 1.0;
d2 = a00 + 2.0*b0 + c;
}
else
{
*s = -b0/a00;
d2 = b0**s + c;
}
}
else // region 0
{
if (det == 0.) // degenerate triangle
d2 = HUGE;
else {
double invDet = 1.0/det;
*s *= invDet;
*t *= invDet;
d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
}
}
}
else
{
if (*s < 0.0) // region 2
{
double tmp0 = a01 + b0;
double tmp1 = a11 + b1;
if (tmp1 > tmp0)
{
double numer = tmp1 - tmp0;
double denom = a00 - 2.0*a01 + a11;
if (numer >= denom)
{
*s = 1.0;
*t = 0.0;
d2 = a00 + 2.0*b0 + c;
}
else
{
*s = numer/denom;
*t = 1.0 - *s;
d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
}
}
else
{
*s = 0.0;
if (tmp1 <= 0.0)
{
*t = 1.0;
d2 = a11 + 2.0*b1 + c;
}
else if (b1 >= 0.0)
{
*t = 0.0;
d2 = c;
}
else
{
*t = -b1/a11;
d2 = b1**t + c;
}
}
}
else if (*t < 0.0) // region 6
{
double tmp0 = a01 + b1;
double tmp1 = a00 + b0;
if (tmp1 > tmp0)
{
double numer = tmp1 - tmp0;
double denom = a00 - 2.0*a01 + a11;
if (numer >= denom)
{
*t = 1.0;
*s = 0.0;
d2 = a11 + 2.0*b1 + c;
}
else
{
*t = numer/denom;
*s = 1.0 - *t;
d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
}
}
else
{
*t = 0.0;
if (tmp1 <= 0.0)
{
*s = 1.0;
d2 = a00 + 2.0*b0 + c;
}
else if (b0 >= 0.0)
{
*s = 0.0;
d2 = c;
}
else
{
*s = -b0/a00;
d2 = b0**s + c;
}
}
}
else // region 1
{
double numer = a11 + b1 - a01 - b0;
if (numer <= 0.0)
{
*s = 0.0;
*t = 1.0;
d2 = a11 + 2.0*b1 + c;
}
else
{
double denom = a00 - 2.0*a01 + a11;
if (numer >= denom)
{
*s = 1.0;
*t = 0.0;
d2 = a00 + 2.0*b0 + c;
}
else
{
*s = numer/denom;
*t = 1.0 - *s;
d2 = *s*(a00**s + a01**t + 2.0*b0) + *t*(a01**s + a11**t + 2.0*b1) + c;
}
}
}
}
// Account for numerical round-off error
if (d2 < 0.0)
{
d2 = 0.0;
}
return d2;
}
int PointTriangleOrientation (const coord * P,
const coord * P0,
const coord * P1,
const coord * P2)
{
coord diff = vecdiff (*P0, *P);
coord edge0 = vecdiff (*P1, *P0);
coord edge1 = vecdiff (*P2, *P0);
coord n = vecdotproduct (edge0, edge1);
return sign (vecdot (diff, n));
}Returns the squared distance between p and [p0:p1].
double PointSegmentDistance (const coord * p, const coord * p0, const coord * p1,
coord * segmentClosest, double * segmentParameter)
{
// The direction vector is not unit length. The normalization is deferred
// until it is needed.
coord direction = vecdiff (*p1, *p0);
coord diff = vecdiff (*p, *p1);
double t = vecdot(direction, diff);
if (t >= (double)0)
{
*segmentParameter = (double)1;
*segmentClosest = *p1;
}
else
{
diff = vecdiff (*p, *p0);
t = vecdot(direction, diff);
if (t <= (double)0)
{
*segmentParameter = (double)0;
*segmentClosest = *p0;
}
else
{
double sqrLength = vecdot(direction, direction);
if (sqrLength > (double)0)
{
t /= sqrLength;
*segmentParameter = t;
(*segmentClosest).z = 0.;
foreach_dimension()
(*segmentClosest).x = (*p0).x + t * direction.x;
}
else
{
*segmentParameter = (double)0;
*segmentClosest = *p0;
}
}
}
diff = vecdiff (*p, *segmentClosest);
return vecdot(diff, diff);
}
int PointSegmentOrientation (const coord * P,
const coord * P0,
const coord * P1)
{
coord diff = vecdiff (*P0, *P);
coord edge0 = vecdiff (*P0, *P1);
coord n = vecdotproduct (diff, edge0);
return sign(n.z);
}