public bool ClosestPtPointTriangle(FPVector p, FPVector a, FPVector b, FPVector c,
ref SubSimplexClosestResult result)
{
result.UsedVertices.Reset();
FP v, w;
// Check if P in vertex region outside A
FPVector ab = b - a;
FPVector ac = c - a;
FPVector ap = p - a;
FP d1 = FPVector.Dot(ab, ap);
FP d2 = FPVector.Dot(ac, ap);
if (d1 <= FP.Zero && d2 <= FP.Zero)
{
result.ClosestPointOnSimplex = a;
result.UsedVertices.UsedVertexA = true;
result.SetBarycentricCoordinates(1, 0, 0, 0);
return(true); // a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
FPVector bp = p - b;
FP d3 = FPVector.Dot(ab, bp);
FP d4 = FPVector.Dot(ac, bp);
if (d3 >= FP.Zero && d4 <= d3)
{
result.ClosestPointOnSimplex = b;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(0, 1, 0, 0);
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(1 - v, v, 0, 0);
return(true);
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
FPVector cp = p - c;
FP d5 = FPVector.Dot(ab, cp);
FP d6 = FPVector.Dot(ac, cp);
if (d6 >= FP.Zero && d5 <= d6)
{
result.ClosestPointOnSimplex = c;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 0, 1, 0);
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(1 - w, 0, w, 0);
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;
if (va <= FP.Zero && (d4 - d3) >= FP.Zero && (d5 - d6) >= FP.Zero)
{
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result.ClosestPointOnSimplex = b + w * (c - b);
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 1 - w, w, 0);
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(1 - v - w, v, w, 0);
return(true);
}
public bool ClosestPtPointTetrahedron(FPVector p, FPVector a, FPVector b, FPVector c, FPVector d,
ref SubSimplexClosestResult finalResult)
{
tempResult.Reset();
// 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)
{
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(p, a, b, c, ref tempResult);
FPVector q = tempResult.ClosestPointOnSimplex;
FP sqDist = ((FPVector)(q - p)).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],
0);
}
}
// Repeat test for face acd
if (pointOutsideACD != 0)
{
ClosestPtPointTriangle(p, a, c, d, ref tempResult);
FPVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
FP sqDist = ((FPVector)(q - p)).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],
0,
tempResult.BarycentricCoords[VertexB],
tempResult.BarycentricCoords[VertexC]);
}
}
// Repeat test for face adb
if (pointOutsideADB != 0)
{
ClosestPtPointTriangle(p, a, d, b, ref tempResult);
FPVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
FP sqDist = ((FPVector)(q - p)).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],
0,
tempResult.BarycentricCoords[VertexB]);
}
}
// Repeat test for face bdc
if (pointOutsideBDC != 0)
{
ClosestPtPointTriangle(p, b, d, c, ref tempResult);
FPVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
FP sqDist = ((FPVector)(q - p)).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(
0,
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 UpdateClosestVectorAndPoints()
{
if (_needsUpdate)
{
_cachedBC.Reset();
_needsUpdate = false;
FPVector p, a, b, c, d;
switch (NumVertices)
{
case 0:
_cachedValidClosest = false;
break;
case 1:
_cachedPA = _simplexPointsP[0];
_cachedPB = _simplexPointsQ[0];
_cachedV = _cachedPA - _cachedPB;
_cachedBC.Reset();
_cachedBC.SetBarycentricCoordinates(1f, FP.Zero, FP.Zero, FP.Zero);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 2:
//closest point origin from line segment
FPVector from = _simplexVectorW[0];
FPVector to = _simplexVectorW[1];
//FPVector nearest;
FPVector diff = from * (-1);
FPVector v = to - from;
FP t = FPVector.Dot(v, diff);
if (t > 0)
{
FP dotVV = v.sqrMagnitude;
if (t < dotVV)
{
t /= dotVV;
diff -= t * v;
_cachedBC.UsedVertices.UsedVertexA = true;
_cachedBC.UsedVertices.UsedVertexB = true;
}
else
{
t = 1;
diff -= v;
//reduce to 1 point
_cachedBC.UsedVertices.UsedVertexB = true;
}
}
else
{
t = 0;
//reduce to 1 point
_cachedBC.UsedVertices.UsedVertexA = true;
}
_cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
//nearest = from + t * v;
_cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
_cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 3:
//closest point origin from triangle
p = new FPVector();
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 4:
p = new FPVector();
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
d = _simplexVectorW[3];
bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);
if (hasSeperation)
{
_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
}
else
{
if (_cachedBC.Degenerate)
{
_cachedValidClosest = false;
}
else
{
_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
_cachedV.x = _cachedV.y = _cachedV.z = FP.Zero;
}
break; // !!!!!!!!!!!! proverit na vsakiy sluchai
}
_cachedValidClosest = _cachedBC.IsValid;
//closest point origin from tetrahedron
break;
default:
_cachedValidClosest = false;
break;
}
}
return(_cachedValidClosest);
}