/// <summary>
/// Find the closest point to the origin.
/// </summary>
/// <param name="W">Simplex</param>
/// <returns>Closest point on simplex to origin.</returns>
public Vector ClosestPoint(Simplex W)
{
Vector d = new Vector();
if (W.count >= 2)
{
d = W.B.MinkowskiPoint - W.A.MinkowskiPoint;
}
switch (W.count)
{
case 0:
return(new Vector(0, 0));
case 1:
return(W.A.MinkowskiPoint);
case 2:
return(W.A.MinkowskiPoint - (((d * W.A.MinkowskiPoint) / (d * d)) * d));
case 3: {
Vector3 W1 = new Vector3((float)W.A.MinkowskiPoint.X, (float)W.A.MinkowskiPoint.Y, 0);
Vector3 W2 = new Vector3((float)W.B.MinkowskiPoint.X, (float)W.B.MinkowskiPoint.Y, 0);
Vector3 W3 = new Vector3((float)W.C.MinkowskiPoint.X, (float)W.C.MinkowskiPoint.Y, 0);
Vector3 n = Vector3.Cross((W2 - W1), (W3 - W1));
Vector3 result = Vector3.Multiply(((Vector3.Multiply(n, W1)) / (Vector3.Multiply(n, n))), n);
return(new Vector(result.X, result.Y));
}
}
return(new Vector(0, 0));
}
