public static void GetClosestPointOnTetrahedronToPoint(RawList<Vector3> tetrahedron, ref Vector3 p, RawList<int> subsimplex, RawList<float> baryCoords, out Vector3 closestPoint)
{
var subsimplexCandidate = CommonResources.GetIntList();
var baryCoordsCandidate = CommonResources.GetFloatList();
Vector3 a = tetrahedron[0];
Vector3 b = tetrahedron[1];
Vector3 c = tetrahedron[2];
Vector3 d = tetrahedron[3];
closestPoint = p;
Vector3 pq;
float bestSqDist = float.MaxValue;
subsimplex.Clear();
subsimplex.Add(0); //Provides a baseline; if the object is not outside of any planes, then it's inside and the subsimplex is the tetrahedron itself.
subsimplex.Add(1);
subsimplex.Add(2);
subsimplex.Add(3);
baryCoords.Clear();
Vector3 q;
bool baryCoordsFound = false;
// If point outside face abc then compute closest point on abc
if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 0, 1, 2, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
// Update best closest point if (squared) distance is less than current best
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
// Repeat test for face acd
if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 0, 2, 3, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
// Repeat test for face adb
if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 0, 3, 1, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
// Repeat test for face bdc
if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c))
{
GetClosestPointOnTriangleToPoint(tetrahedron, 1, 3, 2, ref p, subsimplexCandidate, baryCoordsCandidate, out q);
Vector3.Subtract(ref q, ref p, out pq);
float sqDist = pq.LengthSquared();
if (sqDist < bestSqDist)
{
closestPoint = q;
subsimplex.Clear();
baryCoords.Clear();
for (int k = 0; k < subsimplexCandidate.Count; k++)
{
subsimplex.Add(subsimplexCandidate[k]);
baryCoords.Add(baryCoordsCandidate[k]);
}
//subsimplex.AddRange(subsimplexCandidate);
//baryCoords.AddRange(baryCoordsCandidate);
baryCoordsFound = true;
}
}
if (!baryCoordsFound)
{
//subsimplex is the entire tetrahedron, can only occur when objects intersect! Determinants of each of the tetrahedrons based on triangles composing the sides and the point itself.
//This is basically computing the volume of parallelepipeds (triple scalar product).
//Could be quicker just to do it directly.
float abcd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1,
tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1,
tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
float pbcd = (new Matrix(p.X, p.Y, p.Z, 1,
tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1,
tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
float apcd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1,
p.X, p.Y, p.Z, 1,
tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
float abpd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1,
tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1,
p.X, p.Y, p.Z, 1,
tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant();
abcd = 1 / abcd;
baryCoords.Add(pbcd * abcd); //u
baryCoords.Add(apcd * abcd); //v
baryCoords.Add(abpd * abcd); //w
baryCoords.Add(1 - baryCoords[0] - baryCoords[1] - baryCoords[2]); //x = 1-u-v-w
}
CommonResources.GiveBack(subsimplexCandidate);
CommonResources.GiveBack(baryCoordsCandidate);
}