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