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