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

        public override float SolveIteration()
        {

            Vector2 lambda = RelativeVelocity;

            //Convert to impulse
            //Matrix2x2.Transform(ref lambda, ref velocityToImpulse, out lambda);
            float x = lambda.X;
            lambda.X = x * velocityToImpulse.M11 + lambda.Y * velocityToImpulse.M21;
            lambda.Y = x * velocityToImpulse.M12 + lambda.Y * velocityToImpulse.M22;

            //Accumulate and clamp
            Vector2 previousAccumulatedImpulse = accumulatedImpulse;
            accumulatedImpulse.X += lambda.X;
            accumulatedImpulse.Y += lambda.Y;
            float length = accumulatedImpulse.LengthSquared();
            float maximumFrictionForce = 0;
            for (int i = 0; i < contactCount; i++)
            {
                maximumFrictionForce += contactManifoldConstraint.penetrationConstraints.Elements[i].accumulatedImpulse;
            }
            maximumFrictionForce *= friction;
            if (length > maximumFrictionForce * maximumFrictionForce)
            {
                length = maximumFrictionForce / (float)Math.Sqrt(length);
                accumulatedImpulse.X *= length;
                accumulatedImpulse.Y *= length;
            }
            lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
            lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
            //Single Axis clamp
            //float maximumFrictionForce = 0;
            //for (int i = 0; i < contactCount; i++)
            //{
            //    maximumFrictionForce += pair.contacts[i].penetrationConstraint.accumulatedImpulse;
            //}
            //maximumFrictionForce *= friction;
            //float previousAccumulatedImpulse = accumulatedImpulse.X;
            //accumulatedImpulse.X = MathHelper.Clamp(accumulatedImpulse.X + lambda.X, -maximumFrictionForce, maximumFrictionForce);
            //lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse;
            //previousAccumulatedImpulse = accumulatedImpulse.Y;
            //accumulatedImpulse.Y = MathHelper.Clamp(accumulatedImpulse.Y + lambda.Y, -maximumFrictionForce, maximumFrictionForce);
            //lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse;

            //Apply impulse
#if !WINDOWS
            Vector3 linear = new Vector3();
            Vector3 angular = new Vector3();
#else
            Vector3 linear, angular;
#endif
            //Matrix2x3.Transform(ref lambda, ref linearA, out linear);
            linear.X = lambda.X * linearA.M11 + lambda.Y * linearA.M21;
            linear.Y = lambda.X * linearA.M12 + lambda.Y * linearA.M22;
            linear.Z = lambda.X * linearA.M13 + lambda.Y * linearA.M23;
            if (entityADynamic)
            {
                //Matrix2x3.Transform(ref lambda, ref angularA, out angular);
                angular.X = lambda.X * angularA.M11 + lambda.Y * angularA.M21;
                angular.Y = lambda.X * angularA.M12 + lambda.Y * angularA.M22;
                angular.Z = lambda.X * angularA.M13 + lambda.Y * angularA.M23;
                entityA.ApplyLinearImpulse(ref linear);
                entityA.ApplyAngularImpulse(ref angular);
            }
            if (entityBDynamic)
            {
                linear.X = -linear.X;
                linear.Y = -linear.Y;
                linear.Z = -linear.Z;
                //Matrix2x3.Transform(ref lambda, ref angularB, out angular);
                angular.X = lambda.X * angularB.M11 + lambda.Y * angularB.M21;
                angular.Y = lambda.X * angularB.M12 + lambda.Y * angularB.M22;
                angular.Z = lambda.X * angularB.M13 + lambda.Y * angularB.M23;
                entityB.ApplyLinearImpulse(ref linear);
                entityB.ApplyAngularImpulse(ref angular);
            }


            return Math.Abs(lambda.X) + Math.Abs(lambda.Y);
        }