public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector p1, out JVector p2, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = JVector.Zero;
JVector r = position1 - position2;
JVector w, v;
JVector supVertexA;
JVector rn, vn;
rn = JVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
JVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = JVector.Zero;
int iter = 0;
float distSq = v.LengthSquared();
float epsilon = 0.00001f;
while ((distSq > epsilon) && (iter++ < MaxIterations))
{
IterationsTaken = iter;
vn = JVector.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
normal = v;
}
else distSq = 0.0f;
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
normal.Normalize();
simplexSolverPool.GiveBack(simplexSolver);
return true;
}
/// <summary>
/// Returns true if the point is inside the circle.
/// </summary>
/// <param name="point">The point.</param>
/// <returns>True if the point is inside the circle.</returns>
public override bool PointInsideLocal(JVector point)
{
return (point.LengthSquared() <= (radius * radius));
}