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

        public override float SolveIteration()
        {

            //Compute relative velocity
            float lambda = (RelativeVelocity - bias) * velocityToImpulse; //convert to impulse

            //Clamp accumulated impulse
            float previousAccumulatedImpulse = accumulatedImpulse;
            accumulatedImpulse = MathHelper.Max(0, accumulatedImpulse + lambda);
            lambda = accumulatedImpulse - previousAccumulatedImpulse;
 

            //Apply the impulse
#if !WINDOWS
            Vector3 linear = new Vector3();
            Vector3 angular = new Vector3();
#else
            Vector3 linear, angular;
#endif
            linear.X = lambda * linearAX;
            linear.Y = lambda * linearAY;
            linear.Z = lambda * linearAZ;
            if (entityADynamic)
            {
                angular.X = lambda * angularAX;
                angular.Y = lambda * angularAY;
                angular.Z = lambda * 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 = lambda * angularBX;
                angular.Y = lambda * angularBY;
                angular.Z = lambda * angularBZ;
                entityB.ApplyLinearImpulse(ref linear);
                entityB.ApplyAngularImpulse(ref angular);
            }

            return Math.Abs(lambda);
        }

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