public bool closestPtPointTetrahedron(VInt3 p, VInt3 a, VInt3 b, VInt3 c, VInt3 d, SubSimplexClosestResult finalResult)
{
SubSimplexClosestResult tempResult = subsimplexResultsPool;
tempResult.reset();
VInt3 tmp = new VInt3();
VInt3 q = new VInt3();
// 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)
{
//point is inside tethahedron, no need to update
return(false);
}
VFixedPoint bestSqDist = VFixedPoint.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
closestPtPointTriangle(p, a, b, c, tempResult);
q = tempResult.closestPointOnSimplex;
tmp = q - p;
VFixedPoint sqDist = tmp.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[VERTA],
tempResult.barycentricCoords[VERTB],
tempResult.barycentricCoords[VERTC],
VFixedPoint.Zero
);
}
}
// Repeat test for face acd
if (pointOutsideACD != 0)
{
closestPtPointTriangle(p, a, c, d, tempResult);
q = tempResult.closestPointOnSimplex;
//convert result bitmask!
tmp = q - p;
VFixedPoint sqDist = tmp.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[VERTA],
VFixedPoint.Zero,
tempResult.barycentricCoords[VERTB],
tempResult.barycentricCoords[VERTC]
);
}
}
if (pointOutsideADB != 0)
{
closestPtPointTriangle(p, a, d, b, tempResult);
q = tempResult.closestPointOnSimplex;
//convert result bitmask!
tmp = q - p;
VFixedPoint sqDist = tmp.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.reset();
finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexC;
finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB;
finalResult.setBarycentricCoordinates(
tempResult.barycentricCoords[VERTA],
tempResult.barycentricCoords[VERTC],
VFixedPoint.Zero,
tempResult.barycentricCoords[VERTB]
);
}
}
// Repeat test for face bdc
if (pointOutsideBDC != 0)
{
closestPtPointTriangle(p, b, d, c, tempResult);
q = tempResult.closestPointOnSimplex;
//convert result bitmask!
tmp = q - p;
VFixedPoint sqDist = tmp.sqrMagnitude;
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.closestPointOnSimplex = q;
finalResult.usedVertices.reset();
finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexA;
finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC;
finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB;
finalResult.setBarycentricCoordinates(
VFixedPoint.Zero,
tempResult.barycentricCoords[VERTA],
tempResult.barycentricCoords[VERTC],
tempResult.barycentricCoords[VERTB]
);
}
}
return(true);
}
public bool closestPtPointTriangle(VInt3 p, VInt3 a, VInt3 b, VInt3 c, SubSimplexClosestResult result)
{
result.usedVertices.reset();
// Check if P in vertex region outside A
VInt3 ab = b - a;
VInt3 ac = c - a;
VInt3 ap = p - a;
VFixedPoint d1 = VInt3.Dot(ab, ap);
VFixedPoint d2 = VInt3.Dot(ac, ap);
if (d1 <= VFixedPoint.Zero && d2 <= VFixedPoint.Zero)
{
result.closestPointOnSimplex = a;
result.usedVertices.usedVertexA = true;
result.setBarycentricCoordinates(VFixedPoint.One, VFixedPoint.Zero, VFixedPoint.Zero, VFixedPoint.Zero);
return(true); // a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
VInt3 bp = p - b;
VFixedPoint d3 = VInt3.Dot(ab, bp);
VFixedPoint d4 = VInt3.Dot(ac, bp);
if (d3 >= VFixedPoint.Zero && d4 <= d3)
{
result.closestPointOnSimplex = b;
result.usedVertices.usedVertexB = true;
result.setBarycentricCoordinates(VFixedPoint.Zero, VFixedPoint.One, VFixedPoint.Zero, VFixedPoint.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
VFixedPoint vc = d1 * d4 - d3 * d2;
if (vc <= VFixedPoint.Zero && d1 >= VFixedPoint.Zero && d3 <= VFixedPoint.Zero)
{
VFixedPoint v1 = d1 / (d1 - d3);
result.closestPointOnSimplex = ab * v1 + a;
result.usedVertices.usedVertexA = true;
result.usedVertices.usedVertexB = true;
result.setBarycentricCoordinates(VFixedPoint.One - v1, v1, VFixedPoint.Zero, VFixedPoint.Zero);
return(true);
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
VInt3 cp = p - c;
VFixedPoint d5 = VInt3.Dot(ab, cp);
VFixedPoint d6 = VInt3.Dot(ac, cp);
if (d6 >= VFixedPoint.Zero && d5 <= d6)
{
result.closestPointOnSimplex = c;
result.usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(VFixedPoint.Zero, VFixedPoint.Zero, VFixedPoint.One, VFixedPoint.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
VFixedPoint vb = d5 * d2 - d1 * d6;
if (vb <= VFixedPoint.Zero && d2 >= VFixedPoint.Zero && d6 <= VFixedPoint.Zero)
{
VFixedPoint w1 = d2 / (d2 - d6);
result.closestPointOnSimplex = ac * w1 + a;
result.usedVertices.usedVertexA = true;
result.usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(VFixedPoint.One - w1, VFixedPoint.Zero, w1, VFixedPoint.Zero);
return(true);
}
// Check if P in edge region of BC, if so return projection of P onto BC
VFixedPoint va = d3 * d6 - d5 * d4;
if (va <= VFixedPoint.Zero && (d4 - d3) >= VFixedPoint.Zero && (d5 - d6) >= VFixedPoint.Zero)
{
VFixedPoint w1 = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result.closestPointOnSimplex = (c - b) * w1 + b;
result.usedVertices.usedVertexB = true;
result.usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(VFixedPoint.Zero, VFixedPoint.One - w1, w1, VFixedPoint.Zero);
return(true);
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
VFixedPoint denom = VFixedPoint.One / (va + vb + vc);
VFixedPoint v = vb * denom;
VFixedPoint w = vc * denom;
result.closestPointOnSimplex = a + ab * v + ac * w;
result.usedVertices.usedVertexA = true;
result.usedVertices.usedVertexB = true;
result.usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(VFixedPoint.One - v - w, v, w, VFixedPoint.Zero);
return(true);
}
