public bool ClosestPtPointTetrahedron(TSVector a, TSVector b, TSVector c, TSVector d,
ref SubSimplexClosestResult finalResult)
{
tempResult.Reset();
// Start out assuming point inside all halfspaces, so closest to itself
finalResult.closestPointOnSimplex = TSVector.zero;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = true;
finalResult.usedVertices.UsedVertexB = true;
finalResult.usedVertices.UsedVertexC = true;
finalResult.usedVertices.UsedVertexD = true;
int pointOutsideABC = PointOutsideOfPlane(a, b, c, d);
int pointOutsideACD = PointOutsideOfPlane(a, c, d, b);
int pointOutsideADB = PointOutsideOfPlane(a, d, b, c);
int pointOutsideBDC = PointOutsideOfPlane(b, d, c, a);
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
{
finalResult.degenerate = true;
return(false);
}
if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
{
return(false);
}
FP bestSqDist = FP.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
ClosestPtPointTriangle(a, b, c, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
FP sqDist = q.sqrMagnitude;
// Update best closest point if (squared) distance is less than current best
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
//convert result bitmask!
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
tempResult.barycentricCoords[VertexA],
tempResult.barycentricCoords[VertexB],
tempResult.barycentricCoords[VertexC],
FP.Zero);
}
}
// Repeat test for face acd
if (pointOutsideACD != 0)
{
ClosestPtPointTriangle(a, c, d, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
//convert result bitmask!
FP sqDist = q.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
tempResult.barycentricCoords[VertexA],
FP.Zero,
tempResult.barycentricCoords[VertexB],
tempResult.barycentricCoords[VertexC]);
}
}
// Repeat test for face adb
if (pointOutsideADB != 0)
{
ClosestPtPointTriangle(a, d, b, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
//convert result bitmask!
FP sqDist = q.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
tempResult.barycentricCoords[VertexA],
tempResult.barycentricCoords[VertexC],
FP.Zero,
tempResult.barycentricCoords[VertexB]);
}
}
// Repeat test for face bdc
if (pointOutsideBDC != 0)
{
ClosestPtPointTriangle(b, d, c, ref tempResult);
TSVector q = tempResult.closestPointOnSimplex;
//convert result bitmask!
FP sqDist = q.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.Reset();
finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexA;
finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexB;
finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexC;
finalResult.SetBarycentricCoordinates(
FP.Zero,
tempResult.barycentricCoords[VertexA],
tempResult.barycentricCoords[VertexC],
tempResult.barycentricCoords[VertexB]);
}
}
//help! we ended up full !
if (finalResult.usedVertices.UsedVertexA &&
finalResult.usedVertices.UsedVertexB &&
finalResult.usedVertices.UsedVertexC &&
finalResult.usedVertices.UsedVertexD)
{
return(true);
}
return(true);
}
public bool ClosestPtPointTriangle(TSVector a, TSVector b, TSVector c,
ref SubSimplexClosestResult result)
{
result.usedVertices.Reset();
FP v, w;
// Check if P in vertex region outside A
TSVector ab = b - a;
TSVector ac = c - a;
FP d1 = TSVector.Dot(ab, a);
FP d2 = TSVector.Dot(ac, a);
if (d1 >= FP.Zero && d2 >= FP.Zero)
{
result.closestPointOnSimplex = a;
result.usedVertices.UsedVertexA = true;
result.SetBarycentricCoordinates(FP.One, FP.Zero, FP.Zero, FP.Zero);
return(true); // a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
FP d3 = TSVector.Dot(ab, b);
FP d4 = TSVector.Dot(ac, b);
if (d3 <= FP.Zero && d4 >= d3)
{
result.closestPointOnSimplex = b;
result.usedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(FP.Zero, FP.One, FP.Zero, FP.Zero);
return(true); // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
FP vc = d1 * d4 - d3 * d2;
if (vc <= FP.Zero && d1 >= FP.Zero && d3 <= FP.Zero)
{
v = d1 / (d1 - d3);
result.closestPointOnSimplex = a + v * ab;
result.usedVertices.UsedVertexA = true;
result.usedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(FP.One - v, v, FP.Zero, FP.Zero);
return(true);
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
FP d5 = TSVector.Dot(ab, c);
FP d6 = TSVector.Dot(ac, c);
if (d6 <= FP.Zero && d5 >= d6)
{
result.closestPointOnSimplex = c;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.Zero, FP.Zero, FP.One, FP.Zero);
return(true);//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
FP vb = d5 * d2 - d1 * d6;
if (vb <= FP.Zero && d2 >= FP.Zero && d6 <= FP.Zero)
{
w = d2 / (d2 - d6);
result.closestPointOnSimplex = a + w * ac;
result.usedVertices.UsedVertexA = true;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.One - w, FP.Zero, w, FP.Zero);
return(true);
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
FP va = d3 * d6 - d5 * d4;
FP d34 = d4 - d3;
FP d65 = d5 - d6;
if (va <= FP.Zero && d34 >= FP.Zero && d65 >= FP.Zero)
{
w = d34 / (d34 + d65);
result.closestPointOnSimplex = b + w * (c - b);
result.usedVertices.UsedVertexB = true;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.Zero, FP.One - w, w, FP.Zero);
return(true);
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
FP denom = FP.One / (va + vb + vc);
v = vb * denom;
w = vc * denom;
result.closestPointOnSimplex = a + ab * v + ac * w;
result.usedVertices.UsedVertexA = true;
result.usedVertices.UsedVertexB = true;
result.usedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(FP.One - v - w, v, w, FP.Zero);
return(true);
}