BEPUphysics.CollisionTests.CollisionAlgorithms.GJK.PairSimplex.UpdateCachedSimplex C# (CSharp) Метод

        public void UpdateCachedSimplex(ref CachedSimplex simplex)
        {
            simplex.LocalSimplexA = SimplexA;
            switch (State)
            {
                case SimplexState.Point:
                    Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                    Quaternion conjugate;
                    Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
                    Vector3.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
                    break;
                case SimplexState.Segment:
                    Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                    Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);

                    Matrix3x3 transform;
                    Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
                    break;
                case SimplexState.Triangle:
                    Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                    Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
                    Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);

                    Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
                    break;
                case SimplexState.Tetrahedron:
                    Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                    Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
                    Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
                    Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);

                    Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C);
                    Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D);
                    break;
            }
            simplex.State = State;
        }

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