BEPUphysics.Constraints.TwoEntity.JointLimits.EllipseSwingLimit.Update C# (CSharp) Method

        public override void Update(float dt)
        {
            //Transform the axes into world space.
            basis.rotationMatrix = connectionA.orientationMatrix;
            basis.ComputeWorldSpaceAxes();
            Matrix3x3.Transform(ref localTwistAxisB, ref connectionB.orientationMatrix, out worldTwistAxisB);

            //Compute the individual swing angles.
            Quaternion relativeRotation;
            Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisB, ref basis.primaryAxis, out relativeRotation);
            Vector3 axis;
            float angle;
            Toolbox.GetAxisAngleFromQuaternion(ref relativeRotation, out axis, out angle);

#if !WINDOWS
            Vector3 axisAngle = new Vector3();
#else
            Vector3 axisAngle;
#endif
            axisAngle.X = axis.X * angle;
            axisAngle.Y = axis.Y * angle;
            axisAngle.Z = axis.Z * angle;

            float angleX;
            Vector3.Dot(ref axisAngle, ref basis.xAxis, out angleX);
            float angleY;
            Vector3.Dot(ref axisAngle, ref basis.yAxis, out angleY);


            float maxAngleXSquared = maximumAngleX * maximumAngleX;
            float maxAngleYSquared = maximumAngleY * maximumAngleY;
            error = angleX * angleX * maxAngleYSquared + angleY * angleY * maxAngleXSquared - maxAngleXSquared * maxAngleYSquared;


            //float ellipseAngle = (float)Math.Atan2(angleY, angleX) + MathHelper.PiOver2;
            ////This angle may need to have pi/2 added to it, since it is perpendicular

            ////Compute the maximum angle, based on the ellipse
            //float cosAngle = (float)Math.Cos(ellipseAngle);
            //float sinAngle = (float)Math.Sin(ellipseAngle);
            //float denominator = myMaximumAngleX * cosAngle;
            //denominator *= denominator;
            //float denominator2 = myMaximumAngleY * sinAngle;
            //denominator += denominator2 * denominator2;
            //denominator = (float)Math.Sqrt(denominator);

            //float maximumAngle = myMaximumAngleX * myMaximumAngleY / denominator;
            //myError = angle - maximumAngle;

            //myError = angleX * angleX / (myMaximumAngleX * myMaximumAngleX) + angleY * angleY / (myMaximumAngleY * myMaximumAngleY) - 1;
            if (error <= 0)
            {
                isActiveInSolver = false;
                error = 0;
                accumulatedImpulse = 0;
                isLimitActive = false;
                return;
            }
            isLimitActive = true;

#if !WINDOWS
            Vector2 tangent = new Vector2();
#else
            Vector2 tangent;
#endif
            //tangent.X = angleX * myMaximumAngleY / myMaximumAngleX;
            //tangent.Y = angleY * myMaximumAngleX / myMaximumAngleY;
            tangent.X = angleX / maxAngleXSquared;
            tangent.Y = angleY / maxAngleYSquared;
            tangent.Normalize();

            //Create a rotation which swings our basis 'out' to b's world orientation.
            Quaternion.Conjugate(ref relativeRotation, out relativeRotation);
            Vector3 sphereTangentX, sphereTangentY;
            Vector3.Transform(ref basis.xAxis, ref relativeRotation, out sphereTangentX);
            Vector3.Transform(ref basis.yAxis, ref relativeRotation, out sphereTangentY);

            Vector3.Multiply(ref sphereTangentX, tangent.X, out jacobianA); //not actually jA, just storing it there.
            Vector3.Multiply(ref sphereTangentY, tangent.Y, out jacobianB); //not actually jB, just storing it there.
            Vector3.Add(ref jacobianA, ref jacobianB, out jacobianA);
            //jacobianA = tangent.X * sphereTangentX + tangent.Y * sphereTangentY;

            jacobianB.X = -jacobianA.X;
            jacobianB.Y = -jacobianA.Y;
            jacobianB.Z = -jacobianA.Z;


            float errorReduction;
            springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);

            //Compute the error correcting velocity
            error = error - margin;
            biasVelocity = MathHelper.Min(Math.Max(error, 0) * errorReduction, maxCorrectiveVelocity);
            if (bounciness > 0)
            {
                float relativeVelocity;
                float dot;
                //Find the velocity contribution from each connection
                Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity);
                Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot);
                relativeVelocity += dot;
                relativeVelocity /= 5; //HEEAAAAACK
                if (relativeVelocity > bounceVelocityThreshold)
                    biasVelocity = MathHelper.Max(biasVelocity, bounciness * relativeVelocity);
            }


            //****** EFFECTIVE MASS MATRIX ******//
            //Connection A's contribution to the mass matrix
            float entryA;
            Vector3 transformedAxis;
            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
            }
            else
                entryA = 0;

            //Connection B's contribution to the mass matrix
            float entryB;
            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
            }
            else
                entryB = 0;

            //Compute the inverse mass matrix
            velocityToImpulse = 1 / (softness + entryA + entryB);

            
        }