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 updateClosestVectorAndPoints() { if (needsUpdate) { cachedBC.reset(); needsUpdate = false; switch (numVertices()) { case 0: return(false); case 1: { cachedP1 = simplexPointsP [0]; cachedP2 = simplexPointsQ [0]; cachedV = cachedP1 - cachedP2; cachedBC.reset(); cachedBC.setBarycentricCoordinates(VFixedPoint.One, VFixedPoint.Zero, VFixedPoint.Zero, VFixedPoint.Zero); if (cachedV.sqrMagnitude < Globals.EPS2) { return(true); } else { return(false); } } case 2: { VInt3 tmp = new VInt3(); //closest point origin from line segment VInt3 from = simplexVectorW[0]; VInt3 to = simplexVectorW[1]; VInt3 p = VInt3.zero; VInt3 diff = p - from; VInt3 v = to - from; VFixedPoint t = VInt3.Dot(v, diff); if (t > VFixedPoint.Zero) { VFixedPoint dotVV = v.sqrMagnitude; if (t < dotVV) { t /= dotVV; cachedBC.usedVertices.usedVertexA = true; cachedBC.usedVertices.usedVertexB = true; } else { t = VFixedPoint.One; // reduce to 1 point cachedBC.usedVertices.usedVertexB = true; } } else { t = VFixedPoint.Zero; //reduce to 1 point cachedBC.usedVertices.usedVertexA = true; } cachedBC.setBarycentricCoordinates(VFixedPoint.One - t, t, VFixedPoint.Zero, VFixedPoint.Zero); tmp = simplexPointsP[1] - simplexPointsP[0]; tmp *= t; cachedP1 = simplexPointsP[0] + tmp; tmp = simplexPointsQ[1] - simplexPointsQ[0]; tmp *= t; cachedP2 = simplexPointsQ[0] + tmp; cachedV = cachedP1 - cachedP2; reduceVertices(cachedBC.usedVertices); if (cachedV.sqrMagnitude < Globals.EPS2) { return(true); } else { return(false); } } case 3: { // closest point origin from triangle VInt3 p = VInt3.zero; VInt3 a = simplexVectorW[0]; VInt3 b = simplexVectorW[1]; VInt3 c = simplexVectorW[2]; closestPtPointTriangle(p, a, b, c, cachedBC); cachedP1 = simplexPointsP[0] * cachedBC.barycentricCoords[0] + simplexPointsP[1] * cachedBC.barycentricCoords[1] + simplexPointsP[2] * cachedBC.barycentricCoords[2]; cachedP2 = simplexPointsQ[0] * cachedBC.barycentricCoords[0] + simplexPointsQ[1] * cachedBC.barycentricCoords[1] + simplexPointsQ[2] * cachedBC.barycentricCoords[2]; cachedV = cachedP1 - cachedP2; reduceVertices(cachedBC.usedVertices); if (cachedV.sqrMagnitude < Globals.EPS2) { return(true); } else { return(false); } } case 4: { VInt3 p = VInt3.zero; VInt3 a = simplexVectorW[0]; VInt3 b = simplexVectorW[1]; VInt3 c = simplexVectorW[2]; VInt3 d = simplexVectorW[3]; bool hasSeperation = closestPtPointTetrahedron(p, a, b, c, d, cachedBC); if (!hasSeperation) { cachedP1 = simplexPointsP[0] * cachedBC.barycentricCoords[0] + simplexPointsP[1] * cachedBC.barycentricCoords[1] + simplexPointsP[2] * cachedBC.barycentricCoords[2] + simplexPointsP[3] * cachedBC.barycentricCoords[3]; cachedP2 = simplexPointsQ[0] * cachedBC.barycentricCoords[0] + simplexPointsQ[1] * cachedBC.barycentricCoords[1] + simplexPointsQ[2] * cachedBC.barycentricCoords[2] + simplexPointsQ[3] * cachedBC.barycentricCoords[3]; cachedV = cachedP1 - cachedP2; reduceVertices(cachedBC.usedVertices); return(true); } else { return(false); } } } } return(false); }
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); }