public void GetPointOnTriangleClosestToOrigin(ref RaySimplex simplex, 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->P, C->P...
//Check to see if it's outside A.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
float AdotAB, AdotAC;
Vector3.Dot(ref ab, ref A, out AdotAB);
Vector3.Dot(ref ac, ref A, out AdotAC);
AdotAB = -AdotAB;
AdotAC = -AdotAC;
if (AdotAC <= 0f && AdotAB <= 0)
{
//It is A!
simplex.State = SimplexState.Point;
point = A;
return;
}
//Check to see if it's outside B.
//TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
float BdotAB, BdotAC;
Vector3.Dot(ref ab, ref B, out BdotAB);
Vector3.Dot(ref ac, ref B, out BdotAC);
BdotAB = -BdotAB;
BdotAC = -BdotAC;
if (BdotAB >= 0f && BdotAC <= BdotAB)
{
//It is B!
simplex.State = SimplexState.Point;
simplex.A = simplex.B;
point = B;
return;
}
//Check to see if it's outside AB.
float vc = AdotAB * BdotAC - BdotAB * AdotAC;
if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero
{
simplex.State = SimplexState.Segment;
float V = AdotAB / (AdotAB - BdotAB);
Vector3.Multiply(ref ab, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//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 CdotAB, CdotAC;
Vector3.Dot(ref ab, ref C, out CdotAB);
Vector3.Dot(ref ac, ref C, out CdotAC);
CdotAB = -CdotAB;
CdotAC = -CdotAC;
if (CdotAC >= 0f && CdotAB <= CdotAC)
{
//It is C!
simplex.State = SimplexState.Point;
simplex.A = simplex.C;
point = A;
return;
}
//Check if it's outside AC.
//float AdotAB, AdotAC;
//Vector3.Dot(ref ab, ref A, out AdotAB);
//Vector3.Dot(ref ac, ref A, out AdotAC);
//AdotAB = -AdotAB;
//AdotAC = -AdotAC;
float vb = CdotAB * AdotAC - AdotAB * CdotAC;
if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Get rid of B. Compress C into B.
simplex.State = SimplexState.Segment;
simplex.B = simplex.C;
float V = AdotAC / (AdotAC - CdotAC);
Vector3.Multiply(ref ac, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//Check if it's outside BC.
//float BdotAB, BdotAC;
//Vector3.Dot(ref ab, ref B, out BdotAB);
//Vector3.Dot(ref ac, ref B, out BdotAC);
//BdotAB = -BdotAB;
//BdotAC = -BdotAC;
float va = BdotAB * CdotAC - CdotAB * BdotAC;
float d3d4;
float d6d5;
if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
{
//Throw away A. C->A.
//TODO: Does B->A, C->B work better?
simplex.State = SimplexState.Segment;
simplex.A = simplex.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 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);
}