public bool ClosestPtPointTetrahedron(JVector p, JVector a, JVector b, JVector c, JVector d,
ref SubSimplexClosestResult finalResult)
{
tempResult.Reset();
// Start out assuming point inside all halfspaces, so closest to itself
finalResult.ClosestPointOnSimplex = p;
finalResult.UsedVertices.Reset();
finalResult.UsedVertices.UsedVertexA = true;
finalResult.UsedVertices.UsedVertexB = true;
finalResult.UsedVertices.UsedVertexC = true;
finalResult.UsedVertices.UsedVertexD = true;
int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
int pointOutsideBDC = PointOutsideOfPlane(p, 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;
float bestSqDist = float.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
ClosestPtPointTriangle(p, a, b, c, ref tempResult);
JVector q = tempResult.ClosestPointOnSimplex;
float sqDist = ((JVector)(q - p)).LengthSquared();
// 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],
0);
}
}
// Repeat test for face acd
if (pointOutsideACD != 0)
{
ClosestPtPointTriangle(p, a, c, d, ref tempResult);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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],
0,
tempResult.BarycentricCoords[VertexB],
tempResult.BarycentricCoords[VertexC]);
}
}
// Repeat test for face adb
if (pointOutsideADB != 0)
{
ClosestPtPointTriangle(p, a, d, b, ref tempResult);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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],
0,
tempResult.BarycentricCoords[VertexB]);
}
}
// Repeat test for face bdc
if (pointOutsideBDC != 0)
{
ClosestPtPointTriangle(p, b, d, c, ref tempResult);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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(
0,
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;
}
}