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

        public override void Update(float dt)
        {
            //Compute the 'pre'-jacobians
            Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
            Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
            Vector3.Add(ref worldOffsetA, ref connectionA.position, out worldAnchorA);
            Vector3.Add(ref worldOffsetB, ref connectionB.position, out worldAnchorB);
            Vector3.Subtract(ref worldAnchorB, ref connectionA.position, out rA);
            Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis);

            if (settings.mode == MotorMode.Servomechanism)
            {
                //Compute error
#if !WINDOWS
            Vector3 separation = new Vector3();
#else
                Vector3 separation;
#endif
                separation.X = worldAnchorB.X - worldAnchorA.X;
                separation.Y = worldAnchorB.Y - worldAnchorA.Y;
                separation.Z = worldAnchorB.Z - worldAnchorA.Z;

                Vector3.Dot(ref separation, ref worldAxis, out error);

                //Compute error
                error = error - settings.servo.goal;


                //Compute bias
                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 jacobians
            jLinearA = worldAxis;
            jLinearB.X = -jLinearA.X;
            jLinearB.Y = -jLinearA.Y;
            jLinearB.Z = -jLinearA.Z;
            Vector3.Cross(ref rA, ref jLinearA, out jAngularA);
            Vector3.Cross(ref worldOffsetB, ref jLinearB, out jAngularB);

            //compute mass matrix
            float entryA, entryB;
            Vector3 intermediate;
            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jAngularA, ref connectionA.inertiaTensorInverse, out intermediate);
                Vector3.Dot(ref intermediate, ref jAngularA, out entryA);
                entryA += connectionA.inverseMass;
            }
            else
                entryA = 0;
            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jAngularB, ref connectionB.inertiaTensorInverse, out intermediate);
                Vector3.Dot(ref intermediate, ref jAngularB, out entryB);
                entryB += connectionB.inverseMass;
            }
            else
                entryB = 0;
            massMatrix = 1 / (entryA + entryB + usedSoftness);

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


            
        }