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