BEPUphysics.CollisionTests.CollisionAlgorithms.GJK.PairSimplex.GetNewSimplexPoint C# (CSharp) Méthode

        public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
        {
            Vector3 negativeDirection;
            Vector3.Negate(ref closestPoint, out negativeDirection);
            Vector3 sa, sb;
            shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
            shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
            Vector3 S;
            Vector3.Subtract(ref sa, ref sb, out S);
            //If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
            float dotS;
            Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
            float distanceToClosest = closestPoint.LengthSquared();

            float progression = dotS + distanceToClosest;
            //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
            //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
            //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
            //After a bunch of iterations, the system lets it pick the 'better' one.
            if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
                return true;
            if (distanceToClosest < previousDistanceToClosest)
                previousDistanceToClosest = distanceToClosest;

            //If "A" is the new point always, then the switch statement can be removed
            //in favor of just pushing three points up.
            switch (State)
            {
                case SimplexState.Point:
                    if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Segment;
                    B = S;
                    SimplexA.B = sa;
                    SimplexB.B = sb;
                    return false;
                case SimplexState.Segment:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Triangle;
                    C = S;
                    SimplexA.C = sa;
                    SimplexB.C = sb;
                    return false;
                case SimplexState.Triangle:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Tetrahedron;
                    D = S;
                    SimplexA.D = sa;
                    SimplexB.D = sb;
                    return false;
            }
            return false;
        }

        private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
                                             ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB)
        {
            var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);

            Vector3 closestPoint;
            int     count = 0;

            while (true)
            {
                if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary.
                    closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
                {
                    //Intersecting.
                    localClosestPointA = Toolbox.ZeroVector;
                    localClosestPointB = Toolbox.ZeroVector;

                    simplex.UpdateCachedSimplex(ref cachedSimplex);
                    return(true);
                }

                if (++count > MaximumGJKIterations)
                {
                    break; //Must break BEFORE a new vertex is added if we're over the iteration limit.  This guarantees final simplex is not a tetrahedron.
                }
                if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
                {
                    //No progress towards origin, not intersecting.
                    break;
                }
            }
            //Compute closest points from the contributing simplexes and barycentric coordinates
            simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
            //simplex.VerifyContributions();
            //if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
            //    Debug.WriteLine("break.");
            simplex.UpdateCachedSimplex(ref cachedSimplex);
            return(false);
        }