public void GetClosestPoints(out Vector3 closestPointA, out Vector3 closestPointB)
{
//A * U + B * V + C * W
switch (State)
{
case SimplexState.Point:
closestPointA = SimplexA.A;
closestPointB = SimplexB.A;
return;
case SimplexState.Segment:
Vector3 temp;
Vector3.Multiply(ref SimplexA.A, U, out closestPointA);
Vector3.Multiply(ref SimplexA.B, V, out temp);
Vector3.Add(ref closestPointA, ref temp, out closestPointA);
Vector3.Multiply(ref SimplexB.A, U, out closestPointB);
Vector3.Multiply(ref SimplexB.B, V, out temp);
Vector3.Add(ref closestPointB, ref temp, out closestPointB);
return;
case SimplexState.Triangle:
Vector3.Multiply(ref SimplexA.A, U, out closestPointA);
Vector3.Multiply(ref SimplexA.B, V, out temp);
Vector3.Add(ref closestPointA, ref temp, out closestPointA);
Vector3.Multiply(ref SimplexA.C, W, out temp);
Vector3.Add(ref closestPointA, ref temp, out closestPointA);
Vector3.Multiply(ref SimplexB.A, U, out closestPointB);
Vector3.Multiply(ref SimplexB.B, V, out temp);
Vector3.Add(ref closestPointB, ref temp, out closestPointB);
Vector3.Multiply(ref SimplexB.C, W, out temp);
Vector3.Add(ref closestPointB, ref temp, out closestPointB);
return;
}
closestPointA = Toolbox.ZeroVector;
closestPointB = Toolbox.ZeroVector;
}