BEPUphysics.Constraints.TwoEntity.Motors.RevoluteMotor.Update C# (CSharp) Метод

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


            if (settings.mode == MotorMode.Servomechanism)
            {
                float y, x;
                Vector3 yAxis;
                Vector3.Cross(ref basis.primaryAxis, ref basis.xAxis, out yAxis);
                Vector3.Dot(ref worldTestAxis, ref yAxis, out y);
                Vector3.Dot(ref worldTestAxis, ref basis.xAxis, out x);
                var angle = (float)Math.Atan2(y, x);

                //****** VELOCITY BIAS ******//
                //Compute the correction velocity.
                error = GetDistanceFromGoal(angle);


                float absErrorOverDt = Math.Abs(error / dt);
                float errorReduction;
                settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness);
                biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction;

                biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity);
            }
            else
            {
                biasVelocity = settings.velocityMotor.goalVelocity;
                usedSoftness = settings.velocityMotor.softness / dt;
                error = 0;
            }


            //Compute the jacobians
            jacobianA = basis.primaryAxis;
            jacobianB.X = -jacobianA.X;
            jacobianB.Y = -jacobianA.Y;
            jacobianB.Z = -jacobianA.Z;


            //****** 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 / (usedSoftness + entryA + entryB);


            //Update the maximum force
            ComputeMaxForces(settings.maximumForce, dt);



        }