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); }