public void GetPointOnTriangleClosestToOrigin(out Vector3 point)
{
Vector3 ab, ac;
Vector3.Subtract(ref B, ref A, out ab);
Vector3.Subtract(ref C, ref A, out ac);
//The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
//just use -A.
//Same for B->, C->P...
//CAN'T BE IN A'S REGION.
//CAN'T BE IN B'S REGION.
//CAN'T BE IN AB'S REGION.
//Check to see if it's outside C.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
float d5, d6;
Vector3.Dot(ref ab, ref C, out d5);
Vector3.Dot(ref ac, ref C, out d6);
d5 = -d5;
d6 = -d6;
if (d6 >= 0f && d5 <= d6)
{
//It is C!
State = SimplexState.Point;
A = C;
point = A;
return;
}
//Check if it's outside AC.
float d1, d2;
Vector3.Dot(ref ab, ref A, out d1);
Vector3.Dot(ref ac, ref A, out d2);
d1 = -d1;
d2 = -d2;
float vb = d5 * d2 - d1 * d6;
if (vb <= 0f && d2 > 0f && d6 < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
{
//Get rid of B. Compress C into B.
State = SimplexState.Segment;
B = C;
float V = d2 / (d2 - d6);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check if it's outside BC.
float d3, d4;
Vector3.Dot(ref ab, ref B, out d3);
Vector3.Dot(ref ac, ref B, out d4);
d3 = -d3;
d4 = -d4;
float va = d3 * d6 - d5 * d4;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = d4 - d3) > 0f && (d6d5 = d5 - d6) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
State = SimplexState.Segment;
A = C;
float U = d3d4 / (d3d4 + d6d5);
Vector3 bc;
Vector3.Subtract(ref C, ref B, out bc);
Vector3.Multiply(ref bc, U, out point);
Vector3.Add(ref point, ref B, out point);
return;
}
//On the face of the triangle.
float vc = d1 * d4 - d3 * d2;
float denom = 1f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
Vector3.Multiply(ref ab, v, out point);
Vector3 acw;
Vector3.Multiply(ref ac, w, out acw);
Vector3.Add(ref A, ref point, out point);
Vector3.Add(ref point, ref acw, out point);
}