public void GetPointOnSegmentClosestToOrigin(ref RaySimplex simplex, 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.
simplex.State = SimplexState.Point;
point = A;
return;
}
float dotB;
Vector3.Dot(ref segmentDisplacement, ref B, out dotB);
if (dotB > 0)
{
//Inside segment.
float V = -dotA / segmentDisplacement.LengthSquared();
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.
simplex.A = simplex.B;
simplex.State = SimplexState.Point;
point = A;
}
