public void GetPointOnSegmentClosestToOrigin(out Vector3 point)
{
Vector3 segmentDisplacement;
Vector3.Subtract(ref B, ref A, out segmentDisplacement);
float dotA;
Vector3.Dot(ref segmentDisplacement, ref A, out dotA);
if (dotA > 0)
{
//'Behind' segment. This can't happen in a boolean version,
//but with closest points warmstarting or raycasts, it will.
State = SimplexState.Point;
U = 1;
point = A;
return;
}
float dotB;
Vector3.Dot(ref segmentDisplacement, ref B, out dotB);
if (dotB > 0)
{
//Inside segment.
U = dotB / segmentDisplacement.LengthSquared();
V = 1 - U;
Vector3.Multiply(ref segmentDisplacement, V, out point);
Vector3.Add(ref point, ref A, out point);
return;
}
//It should be possible in the warmstarted closest point calculation/raycasting to be outside B.
//It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found.
//Outside B.
//Remove current A; we're becoming a point.
A = B;
SimplexA.A = SimplexA.B;
SimplexB.A = SimplexB.B;
State = SimplexState.Point;
U = 1;
point = A;
}
