BEPUphysics.Constraints.TwoEntity.Joints.PointOnPlaneJoint.SolveIteration C# (CSharp) Method

        public override float SolveIteration()
        {
            //TODO: This could technically be faster.
            //Form the jacobian explicitly.
            //Cross cross add add subtract dot
            //vs
            //dot dot dot dot and then scalar adds
            Vector3 dv;
            Vector3 aVel, bVel;
            Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel);
            Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel);
            Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel);
            Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel);
            Vector3.Subtract(ref aVel, ref bVel, out dv);
            float velocityDifference;
            Vector3.Dot(ref dv, ref worldPlaneNormal, out velocityDifference);
            //if(velocityDifference > 0)
            //    Debug.WriteLine("Velocity difference: " + velocityDifference);
            //Debug.WriteLine("softness velocity: " + softness * accumulatedImpulse);
            float lambda = negativeEffectiveMass * (velocityDifference + biasVelocity + softness * accumulatedImpulse);
            accumulatedImpulse += lambda;

            Vector3 impulse;
            Vector3 torque;
            Vector3.Multiply(ref worldPlaneNormal, lambda, out impulse);
            if (connectionA.isDynamic)
            {
                Vector3.Multiply(ref rAcrossN, lambda, out torque);
                connectionA.ApplyLinearImpulse(ref impulse);
                connectionA.ApplyAngularImpulse(ref torque);
            }
            if (connectionB.isDynamic)
            {
                Vector3.Negate(ref impulse, out impulse);
                Vector3.Multiply(ref rBcrossN, lambda, out torque);
                connectionB.ApplyLinearImpulse(ref impulse);
                connectionB.ApplyAngularImpulse(ref torque);
            }

            return lambda;
        }