BEPUphysics.Constraints.Collision.ContactPenetrationConstraint.ExclusiveUpdate C# (CSharp) Method

        public override void ExclusiveUpdate()
        {
            //Warm starting
#if !WINDOWS
            Vector3 linear = new Vector3();
            Vector3 angular = new Vector3();
#else
            Vector3 linear, angular;
#endif
            linear.X = accumulatedImpulse * linearAX;
            linear.Y = accumulatedImpulse * linearAY;
            linear.Z = accumulatedImpulse * linearAZ;
            if (entityADynamic)
            {
                angular.X = accumulatedImpulse * angularAX;
                angular.Y = accumulatedImpulse * angularAY;
                angular.Z = accumulatedImpulse * angularAZ;
                entityA.ApplyLinearImpulse(ref linear);
                entityA.ApplyAngularImpulse(ref angular);
            }
            if (entityBDynamic)
            {
                linear.X = -linear.X;
                linear.Y = -linear.Y;
                linear.Z = -linear.Z;
                angular.X = accumulatedImpulse * angularBX;
                angular.Y = accumulatedImpulse * angularBY;
                angular.Z = accumulatedImpulse * angularBZ;
                entityB.ApplyLinearImpulse(ref linear);
                entityB.ApplyAngularImpulse(ref angular);
            }
        }

        public static unsafe void Test()
        {
            ContactPenetrationConstraint constraint = new ContactPenetrationConstraint();
            Contact contact = new Contact { Normal = new Vector3(0, 1, 0), PenetrationDepth = 0, Position = new Vector3() };
            var pairHandler = new BoxPairHandler();
            var a = new Entity(new BoxShape(1, 1, 1), 1)
            {
                Position = new Vector3(0, 0, 0),
                Orientation = Quaternion.Identity,
                LinearVelocity = new Vector3(0, 0, 0)
            };
            var b = new Entity(new BoxShape(1, 1, 1), 1)
            {
                Position = new Vector3(0, 1, 0),
                Orientation = Quaternion.Identity,
                LinearVelocity = new Vector3(0, 0, 0)
            };
            pairHandler.Initialize(a.CollisionInformation, b.CollisionInformation);
            ContactManifoldConstraint manifoldConstraint = new ConvexContactManifoldConstraint(pairHandler);
            manifoldConstraint.Initialize(a, b);

            constraint.Setup(manifoldConstraint, contact);

            float dt = 1 / 60f;
            float inverseDt = 1 / dt;
            constraint.Update(dt);
            constraint.ExclusiveUpdate();
            constraint.SolveIteration();

            const int testCount = VectorizedConstraintTest.TestCount * 4;
            const int iterationCount = VectorizedConstraintTest.IterationCount;

            var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
            for (int i = 0; i < testCount; ++i)
            {
                constraint.Update(dt);
                constraint.ExclusiveUpdate();
                for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
                {
                    constraint.SolveIteration();
                }
            }

            var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;

            Console.WriteLine($"Scalar Old: {endtime - startTime}");
        }