/// <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)); }