```/*
Distance.cpp
Written by Matthew Fisher

Code for computing geometric object distances.
http://www.geometrictools.com/LibMathematics/Distance/Distance.html
*/

float Math::DistanceLineSegmentSq(const Vec3f &l0, const Vec3f &l1, const Vec3f &seg0, const Vec3f &seg1, DistanceLineSegmentResult *result)
{
Vec3f segmentCenter = (seg0 + seg1) * 0.5f;
Vec3f diff = l0 - segmentCenter;
Vec3f lineDirection = Vec3f::Normalize(l1 - l0);
Vec3f segmentDirection = Vec3f::Normalize(seg1 - seg0);
float segmentExtent = (seg1 - seg0).Length() * 0.5f;
float a01 = -Vec3f::Dot(lineDirection, segmentDirection);
float b0 = Vec3f::Dot(diff, lineDirection);
float c = diff.LengthSq();
float det = Math::Abs(1.0f - a01*a01);
float b1, s0, s1, sqrDist, extDet;

if (det >= 1e-7f)
{
// The line and segment are not parallel.
b1 = -Vec3f::Dot(diff, segmentDirection);
s1 = a01*b0 - b1;
extDet = segmentExtent*det;

if (s1 >= -extDet)
{
if (s1 <= extDet)
{
// Two interior points are closest, one on the line and one
// on the segment.
float invDet = ((float)1)/det;
s0 = (a01*b1 - b0)*invDet;
s1 *= invDet;
sqrDist = s0*(s0 + a01*s1 + ((float)2)*b0) +
s1*(a01*s0 + s1 + ((float)2)*b1) + c;
}
else
{
// The endpoint e1 of the segment and an interior point of
// the line are closest.
s1 = segmentExtent;
s0 = -(a01*s1 + b0);
sqrDist = -s0*s0 + s1*(s1 + ((float)2)*b1) + c;
}
}
else
{
// The end point e0 of the segment and an interior point of the
// line are closest.
s1 = -segmentExtent;
s0 = -(a01*s1 + b0);
sqrDist = -s0*s0 + s1*(s1 + ((float)2)*b1) + c;
}
}
else
{
// The line and segment are parallel.  Choose the closest pair so that
// one point is at segment center.
s1 = (float)0;
s0 = -b0;
sqrDist = b0*s0 + c;
}

if(result != NULL)
{
result->lineParameter = s0;
result->segmentParameter = s1;
}
//mClosestPoint0 = l0 + s0*lineDirection;
//mClosestPoint1 = segmentCenter + s1*segmentDirection;

// Account for numerical round-off errors.
if (sqrDist < (float)0)
{
sqrDist = (float)0;
}
return sqrDist;

}

float Math::DistancePointTriangleSq(const Vec3f &t0, const Vec3f &t1, const Vec3f &t2, const Vec3f &p)
{
Vec3f diff = t0 - p;
Vec3f edge0 = t1 - t0;
Vec3f edge1 = t2 - t0;
float a00 = edge0.LengthSq();
float a01 = Vec3f::Dot(edge0, edge1);
float a11 = edge1.LengthSq();
float b0 = Vec3f::Dot(diff, edge0);
float b1 = Vec3f::Dot(diff, edge1);
float c = diff.LengthSq();
float det = Math::Abs(a00*a11 - a01*a01);
float s = a01*b1 - a11*b0;
float t = a01*b0 - a00*b1;
float sqrDistance;

if (s + t <= det)
{
if (s < (float)0)
{
if (t < (float)0)  // region 4
{
if (b0 < (float)0)
{
t = (float)0;
if (-b0 >= a00)
{
s = (float)1;
sqrDistance = a00 + ((float)2)*b0 + c;
}
else
{
s = -b0/a00;
sqrDistance = b0*s + c;
}
}
else
{
s = (float)0;
if (b1 >= (float)0)
{
t = (float)0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = (float)1;
sqrDistance = a11 + ((float)2)*b1 + c;
}
else
{
t = -b1/a11;
sqrDistance = b1*t + c;
}
}
}
else  // region 3
{
s = (float)0;
if (b1 >= (float)0)
{
t = (float)0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = (float)1;
sqrDistance = a11 + ((float)2)*b1 + c;
}
else
{
t = -b1/a11;
sqrDistance = b1*t + c;
}
}
}
else if (t < (float)0)  // region 5
{
t = (float)0;
if (b0 >= (float)0)
{
s = (float)0;
sqrDistance = c;
}
else if (-b0 >= a00)
{
s = (float)1;
sqrDistance = a00 + ((float)2)*b0 + c;
}
else
{
s = -b0/a00;
sqrDistance = b0*s + c;
}
}
else  // region 0
{
// minimum at interior point
float invDet = ((float)1)/det;
s *= invDet;
t *= invDet;
sqrDistance = s*(a00*s + a01*t + ((float)2)*b0) +
t*(a01*s + a11*t + ((float)2)*b1) + c;
}
}
else
{
float tmp0, tmp1, numer, denom;

if (s < (float)0)  // region 2
{
tmp0 = a01 + b0;
tmp1 = a11 + b1;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - ((float)2)*a01 + a11;
if (numer >= denom)
{
s = (float)1;
t = (float)0;
sqrDistance = a00 + ((float)2)*b0 + c;
}
else
{
s = numer/denom;
t = (float)1 - s;
sqrDistance = s*(a00*s + a01*t + ((float)2)*b0) +
t*(a01*s + a11*t + ((float)2)*b1) + c;
}
}
else
{
s = (float)0;
if (tmp1 <= (float)0)
{
t = (float)1;
sqrDistance = a11 + ((float)2)*b1 + c;
}
else if (b1 >= (float)0)
{
t = (float)0;
sqrDistance = c;
}
else
{
t = -b1/a11;
sqrDistance = b1*t + c;
}
}
}
else if (t < (float)0)  // region 6
{
tmp0 = a01 + b1;
tmp1 = a00 + b0;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - ((float)2)*a01 + a11;
if (numer >= denom)
{
t = (float)1;
s = (float)0;
sqrDistance = a11 + ((float)2)*b1 + c;
}
else
{
t = numer/denom;
s = (float)1 - t;
sqrDistance = s*(a00*s + a01*t + ((float)2)*b0) +
t*(a01*s + a11*t + ((float)2)*b1) + c;
}
}
else
{
t = (float)0;
if (tmp1 <= (float)0)
{
s = (float)1;
sqrDistance = a00 + ((float)2)*b0 + c;
}
else if (b0 >= (float)0)
{
s = (float)0;
sqrDistance = c;
}
else
{
s = -b0/a00;
sqrDistance = b0*s + c;
}
}
}
else  // region 1
{
numer = a11 + b1 - a01 - b0;
if (numer <= (float)0)
{
s = (float)0;
t = (float)1;
sqrDistance = a11 + ((float)2)*b1 + c;
}
else
{
denom = a00 - ((float)2)*a01 + a11;
if (numer >= denom)
{
s = (float)1;
t = (float)0;
sqrDistance = a00 + ((float)2)*b0 + c;
}
else
{
s = numer/denom;
t = (float)1 - s;
sqrDistance = s*(a00*s + a01*t + ((float)2)*b0) +
t*(a01*s + a11*t + ((float)2)*b1) + c;
}
}
}
}

// Account for numerical round-off error.
if (sqrDistance < 0.0f)
{
sqrDistance = 0.0f;
}

//mClosestPoint0 = *mPoint;
//mClosestPoint1 = mTriangle->V[0] + s*edge0 + t*edge1;
//mTriangleBary[1] = s;
//mTriangleBary[2] = t;
//mTriangleBary[0] = (float)1 - s - t;
return sqrDistance;
}

float Math::DistanceLineTriangleSq(const Vec3f &t0, const Vec3f &t1, const Vec3f &t2, const Vec3f &l0, const Vec3f &l1, DistanceLineTriangleResult *result)
{
// Test if line intersects triangle.  If so, the squared distance is zero.
Vec3f edge0 = t1 - t0;
Vec3f edge1 = t2 - t0;
Vec3f normal = Vec3f::Cross(edge0, edge1);
Vec3f lineDirection = Vec3f::Normalize(l1 - l0);
float NdD = Vec3f::Dot(normal, lineDirection);
if (Math::Abs(NdD) > 1e-7f)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vec3f diff = l0 - t0;
Vec3f U, V;
Vec3f::CompleteOrthonormalBasis(lineDirection, U, V);
float UdE0 = Vec3f::Dot(U, edge0);
float UdE1 = Vec3f::Dot(U, edge1);
float UdDiff = Vec3f::Dot(U, diff);
float VdE0 = Vec3f::Dot(V, edge0);
float VdE1 = Vec3f::Dot(V, edge1);
float VdDiff = Vec3f::Dot(V, diff);
float invDet = ((float)1)/(UdE0*VdE1 - UdE1*VdE0);

// Barycentric coordinates for the point of intersection.
float b1 = (VdE1*UdDiff - UdE1*VdDiff)*invDet;
float b2 = (UdE0*VdDiff - VdE0*UdDiff)*invDet;
float b0 = (float)1 - b1 - b2;

if (b0 >= (float)0 && b1 >= (float)0 && b2 >= (float)0)
{
// Line parameter for the point of intersection.
float DdE0 = Vec3f::Dot(lineDirection, edge0);
float DdE1 = Vec3f::Dot(lineDirection, edge1);
float DdDiff = Vec3f::Dot(lineDirection, diff);

if(result != NULL)
{
result->lineParameter = b1*DdE0 + b2*DdE1 - DdDiff;
}

// Barycentric coordinates for the point of intersection.
//mTriangleBary[0] = b0;
//mTriangleBary[1] = b1;
//mTriangleBary[2] = b2;

// The intersection point is inside or on the triangle.
//mClosestPoint0 = mLine->Origin + mLineParameter*mLine->Direction;

//mClosestPoint1 = t0 + b1*edge0 + b2*edge1;

return (float)0;
}
}

const Vec3f trianglePoints[3] = {t0, t1, t2};
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel.  Regardless, the
// closest point on the triangle is on an edge of the triangle.  Compare
// the line to all three edges of the triangle.
float sqrDist = numeric_limits<float>::max();
for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
DistanceLineSegmentResult lineSegmentResult;
float sqrDistTmp = DistanceLineSegmentSq(l0, l1, trianglePoints[i0], trianglePoints[i1], &lineSegmentResult);
if (sqrDistTmp < sqrDist)
{
//mClosestPoint0 = queryLS.GetClosestPoint0();
//mClosestPoint1 = queryLS.GetClosestPoint1();
sqrDist = sqrDistTmp;

if(result != NULL)
{
result->lineParameter = lineSegmentResult.lineParameter;
}
//mLineParameter = queryLS.GetLineParameter();
//float ratio = queryLS.GetSegmentParameter()/segment.Extent;
//mTriangleBary[i0] = ((float)0.5)*((float)1 - ratio);
//mTriangleBary[i1] = (float)1 - mTriangleBary[i0];
//mTriangleBary[3-i0-i1] = (float)0;
}
}

return sqrDist;
}

float Math::DistanceSegmentTriangleSq(const Vec3f &t0, const Vec3f &t1, const Vec3f &t2, const Vec3f &seg0, const Vec3f &seg1)
{
float segmentExtent = (seg1 - seg0).Length() * 0.5f;
Vec3f segmentCenter = (seg0 + seg1) * 0.5f;

DistanceLineTriangleResult lineTriangleResult;
float sqrDist = DistanceLineTriangleSq(t0, t1, t2, segmentCenter, seg1, &lineTriangleResult);
float segmentParameter = lineTriangleResult.lineParameter;

if (segmentParameter >= -segmentExtent)
{
if (segmentParameter <= segmentExtent)
{
//mClosestPoint0 = queryLT.GetClosestPoint0();
//mClosestPoint1 = queryLT.GetClosestPoint1();
//mTriangleBary[0] = queryLT.GetTriangleBary(0);
//mTriangleBary[1] = queryLT.GetTriangleBary(1);
//mTriangleBary[2] = queryLT.GetTriangleBary(2);
}
else
{
//mClosestPoint0 = mSegment->P1;
sqrDist = DistancePointTriangleSq(t0, t1, t2, seg1);
//mClosestPoint1 = queryPT.GetClosestPoint1();
//mSegmentParameter = mSegment->Extent;
//mTriangleBary[0] = queryPT.GetTriangleBary(0);
//mTriangleBary[1] = queryPT.GetTriangleBary(1);
//mTriangleBary[2] = queryPT.GetTriangleBary(2);
}
}
else
{
//mClosestPoint0 = mSegment->P0;
sqrDist = DistancePointTriangleSq(t0, t1, t2, seg0);
//mClosestPoint1 = queryPT.GetClosestPoint1();
//mSegmentParameter = -mSegment->Extent;
//mTriangleBary[0] = queryPT.GetTriangleBary(0);
//mTriangleBary[1] = queryPT.GetTriangleBary(1);
//mTriangleBary[2] = queryPT.GetTriangleBary(2);
}

return sqrDist;

}

float Math::DistanceTriangleTriangleSq(const Vec3f &t0v0, const Vec3f &t0v1, const Vec3f &t0v2, const Vec3f &t1v0, const Vec3f &t1v1, const Vec3f &t1v2)
{
// Compare edges of triangle0 to the interior of triangle1.
float sqrDist = numeric_limits<float>::max(), sqrDistTmp;

const Vec3f triangle0Points[3] = {t0v0, t0v1, t0v2};
const Vec3f triangle1Points[3] = {t1v0, t1v1, t1v2};

int i0, i1;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
sqrDistTmp = DistanceSegmentTriangleSq(triangle1Points[0], triangle1Points[1], triangle1Points[2], triangle0Points[i0], triangle0Points[i1]);
if (sqrDistTmp < sqrDist)
{
//mClosestPoint0 = queryST.GetClosestPoint0();
//mClosestPoint1 = queryST.GetClosestPoint1();
sqrDist = sqrDistTmp;

//ratio = queryST.GetSegmentParameter()/edge.Extent;
//mTriangleBary0[i0] = ((float)0.5)*((float)1 - ratio);
//mTriangleBary0[i1] = (float)1 - mTriangleBary0[i0];
//mTriangleBary0[3-i0-i1] = (float)0;
//mTriangleBary1[0] = queryST.GetTriangleBary(0);
//mTriangleBary1[1] = queryST.GetTriangleBary(1);
//mTriangleBary1[2] = queryST.GetTriangleBary(2);

if (sqrDist <= 1e-7f)
{
return (float)0;
}
}
}

// Compare edges of triangle1 to the interior of triangle0.
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
sqrDistTmp = DistanceSegmentTriangleSq(triangle0Points[0], triangle0Points[1], triangle0Points[2], triangle1Points[i0], triangle1Points[i1]);
if (sqrDistTmp < sqrDist)
{
//mClosestPoint0 = queryST.GetClosestPoint0();
//mClosestPoint1 = queryST.GetClosestPoint1();
sqrDist = sqrDistTmp;

//ratio = queryST.GetSegmentParameter()/edge.Extent;
//mTriangleBary1[i0] = ((float)0.5)*((float)1 - ratio);
//mTriangleBary1[i1] = (float)1 - mTriangleBary1[i0];
//mTriangleBary1[3-i0-i1] = (float)0;
//mTriangleBary0[0] = queryST.GetTriangleBary(0);
//mTriangleBary0[1] = queryST.GetTriangleBary(1);
//mTriangleBary0[2] = queryST.GetTriangleBary(2);

if (sqrDist <= 1e-7f)
{
return (float)0;
}
}
}

return sqrDist;

}
```