public bool ClosestPtPointTriangle(JVector p, JVector a, JVector b, JVector c,
ref SubSimplexClosestResult result)
{
result.UsedVertices.Reset();
float v, w;
// Check if P in vertex region outside A
JVector ab = b - a;
JVector ac = c - a;
JVector ap = p - a;
float d1 = JVector.Dot(ab, ap);
float d2 = JVector.Dot(ac, ap);
if (d1 <= 0f && d2 <= 0f)
{
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
JVector bp = p - b;
float d3 = JVector.Dot(ab, bp);
float d4 = JVector.Dot(ac, bp);
if (d3 >= 0f && 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
float vc = d1 * d4 - d3 * d2;
if (vc <= 0f && d1 >= 0f && d3 <= 0f)
{
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
JVector cp = p - c;
float d5 = JVector.Dot(ab, cp);
float d6 = JVector.Dot(ac, cp);
if (d6 >= 0f && 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
float vb = d5 * d2 - d1 * d6;
if (vb <= 0f && d2 >= 0f && d6 <= 0f)
{
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
float va = d3 * d6 - d5 * d4;
if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
{
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)
float denom = 1.0f / (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(JVector p, JVector a, JVector b, JVector c, JVector 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;
float bestSqDist = float.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
ClosestPtPointTriangle(p, a, b, c, ref tempResult);
JVector q = tempResult.ClosestPointOnSimplex;
float sqDist = ((JVector)(q - p)).LengthSquared();
// 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);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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 ClosestPtPointTetrahedron(JVector p, JVector a, JVector b, JVector c, JVector 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);
}
float bestSqDist = float.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
ClosestPtPointTriangle(p, a, b, c, ref tempResult);
JVector q = tempResult.ClosestPointOnSimplex;
float sqDist = ((JVector)(q - p)).LengthSquared();
// 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);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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);
JVector q = tempResult.ClosestPointOnSimplex;
//convert result bitmask!
float sqDist = ((JVector)(q - p)).LengthSquared();
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 ClosestPtPointTriangle(JVector p, JVector a, JVector b, JVector c,
ref SubSimplexClosestResult result)
{
result.UsedVertices.Reset();
float v, w;
// Check if P in vertex region outside A
JVector ab = b - a;
JVector ac = c - a;
JVector ap = p - a;
float d1 = JVector.Dot(ab, ap);
float d2 = JVector.Dot(ac, ap);
if (d1 <= 0f && d2 <= 0f)
{
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
JVector bp = p - b;
float d3 = JVector.Dot(ab, bp);
float d4 = JVector.Dot(ac, bp);
if (d3 >= 0f && 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
float vc = d1 * d4 - d3 * d2;
if (vc <= 0f && d1 >= 0f && d3 <= 0f)
{
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
JVector cp = p - c;
float d5 = JVector.Dot(ab, cp);
float d6 = JVector.Dot(ac, cp);
if (d6 >= 0f && 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
float vb = d5 * d2 - d1 * d6;
if (vb <= 0f && d2 >= 0f && d6 <= 0f)
{
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
float va = d3 * d6 - d5 * d4;
if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
{
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)
float denom = 1.0f / (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);
}
bool closestPtPointTetrahedron(ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d, SubSimplexClosestResult finalResult)
{
tempResult.reset();
// Start out assuming point inside all halfspaces, so closest to itself
finalResult.m_closestPointOnSimplex = p;
finalResult.m_usedVertices.reset();
finalResult.m_usedVertices.usedVertexA = true;
finalResult.m_usedVertices.usedVertexB = true;
finalResult.m_usedVertices.usedVertexC = true;
finalResult.m_usedVertices.usedVertexD = true;
int pointOutsideABC = pointOutsideOfPlane(ref p, ref a, ref b, ref c, ref d);
int pointOutsideACD = pointOutsideOfPlane(ref p, ref a, ref c, ref d, ref b);
int pointOutsideADB = pointOutsideOfPlane(ref p, ref a, ref d, ref b, ref c);
int pointOutsideBDC = pointOutsideOfPlane(ref p, ref b, ref d, ref c, ref a);
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
{
finalResult.m_degenerate = true;
return false;
}
if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
{
return false;
}
float bestSqDist = float.MaxValue;
// If point outside face abc then compute closest point on abc
if (pointOutsideABC != 0)
{
closestPtPointTriangle(ref p, ref a, ref b, ref c, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
float sqDist = (q - p).dot(q - p);
// Update best closest point if (squared) distance is less than current best
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = q;
//convert result bitmask!
finalResult.m_usedVertices.reset();
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
finalResult.setBarycentricCoordinates(
tempResult.m_barycentricCoords[VERTA],
tempResult.m_barycentricCoords[VERTB],
tempResult.m_barycentricCoords[VERTC],
0
);
}
}
// Repeat test for face acd
if (pointOutsideACD != 0)
{
closestPtPointTriangle(ref p, ref a, ref c, ref d, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
float sqDist = (q - p).dot(q - p);
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = q;
finalResult.m_usedVertices.reset();
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
finalResult.setBarycentricCoordinates(
tempResult.m_barycentricCoords[VERTA],
0,
tempResult.m_barycentricCoords[VERTB],
tempResult.m_barycentricCoords[VERTC]
);
}
}
// Repeat test for face adb
if (pointOutsideADB != 0)
{
closestPtPointTriangle(ref p, ref a, ref d, ref b, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
float sqDist = (q - p).dot(q - p);
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = q;
finalResult.m_usedVertices.reset();
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
finalResult.setBarycentricCoordinates(
tempResult.m_barycentricCoords[VERTA],
tempResult.m_barycentricCoords[VERTC],
0,
tempResult.m_barycentricCoords[VERTB]
);
}
}
// Repeat test for face bdc
if (pointOutsideBDC != 0)
{
closestPtPointTriangle(ref p, ref b, ref d, ref c, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
float sqDist = (q - p).dot(q - p);
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = q;
finalResult.m_usedVertices.reset();
//
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
finalResult.setBarycentricCoordinates(
0,
tempResult.m_barycentricCoords[VERTA],
tempResult.m_barycentricCoords[VERTC],
tempResult.m_barycentricCoords[VERTB]
);
}
}
//help! we ended up full !
if (finalResult.m_usedVertices.usedVertexA &&
finalResult.m_usedVertices.usedVertexB &&
finalResult.m_usedVertices.usedVertexC &&
finalResult.m_usedVertices.usedVertexD)
{
return true;
}
return true;
}
bool closestPtPointTriangle(ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, SubSimplexClosestResult result)
{
result.m_usedVertices.reset();
// Check if P in vertex region outside A
btVector3 ab = b - a;
btVector3 ac = c - a;
btVector3 ap = p - a;
float d1 = ab.dot(ap);
float d2 = ac.dot(ap);
if (d1 <= 0.0f && d2 <= 0.0f)
{
result.m_closestPointOnSimplex = a;
result.m_usedVertices.usedVertexA = true;
result.setBarycentricCoordinates(1, 0, 0);
return true;// a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
btVector3 bp = p - b;
float d3 = ab.dot(bp);
float d4 = ac.dot(bp);
if (d3 >= 0.0f && d4 <= d3)
{
result.m_closestPointOnSimplex = b;
result.m_usedVertices.usedVertexB = true;
result.setBarycentricCoordinates(0, 1, 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
float vc = d1 * d4 - d3 * d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
{
float v = d1 / (d1 - d3);
result.m_closestPointOnSimplex = a + v * ab;
result.m_usedVertices.usedVertexA = true;
result.m_usedVertices.usedVertexB = true;
result.setBarycentricCoordinates(1 - v, v, 0);
return true;
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
btVector3 cp = p - c;
float d5 = ab.dot(cp);
float d6 = ac.dot(cp);
if (d6 >= 0.0f && d5 <= d6)
{
result.m_closestPointOnSimplex = c;
result.m_usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(0, 0, 1);
return true;//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
float vb = d5 * d2 - d1 * d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
{
float w = d2 / (d2 - d6);
result.m_closestPointOnSimplex = a + w * ac;
result.m_usedVertices.usedVertexA = true;
result.m_usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(1 - w, 0, w);
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
float va = d3 * d6 - d5 * d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
{
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result.m_closestPointOnSimplex = b + w * (c - b);
result.m_usedVertices.usedVertexB = true;
result.m_usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(0, 1 - w, w);
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)
float denom = 1.0f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
result.m_closestPointOnSimplex = a + ab * v + ac * w;
result.m_usedVertices.usedVertexA = true;
result.m_usedVertices.usedVertexB = true;
result.m_usedVertices.usedVertexC = true;
result.setBarycentricCoordinates(1 - v - w, v, w);
}
return true;
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
}
