/// <summary> /// Initializes the constraint for the current frame. /// </summary> /// <param name="dt">Time between frames.</param> public override void Update(float dt) { basis.rotationMatrix = connectionA.orientationMatrix; basis.ComputeWorldSpaceAxes(); if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo. { //The error is computed using this equation: //GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation //GoalRelativeOrientation is the original rotation from A to B in A's local space. //Multiplying by A's orientation gives us where B *should* be. //Of course, B won't be exactly where it should be after initialization. //The Error component holds the difference between what is and what should be. //Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation //ConnectionA.Orientation is replaced in the above by the world space basis orientation. Quaternion worldBasis = Matrix3x3.CreateQuaternion(basis.WorldTransform); Quaternion bTarget; Quaternion.Concatenate(ref settings.servo.goal, ref worldBasis, out bTarget); Quaternion bTargetConjugate; Quaternion.Conjugate(ref bTarget, out bTargetConjugate); Quaternion error; Quaternion.Concatenate(ref bTargetConjugate, ref connectionB.orientation, out error); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out usedSoftness); //Turn this into an axis-angle representation. Toolbox.GetAxisAngleFromQuaternion(ref error, out axis, out angle); //Scale the axis by the desired velocity if the angle is sufficiently large (epsilon). if (angle > Toolbox.BigEpsilon) { float velocity = -(MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle / dt) + angle * errorReduction); biasVelocity.X = axis.X * velocity; biasVelocity.Y = axis.Y * velocity; biasVelocity.Z = axis.Z * velocity; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { biasVelocity.X = 0; biasVelocity.Y = 0; biasVelocity.Z = 0; } } else { usedSoftness = settings.velocityMotor.softness / dt; angle = 0; //Zero out the error; Matrix3x3 transform = basis.WorldTransform; Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity); } //Compute effective mass Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix); effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); }
/// <summary> /// Initializes the constraint for the current frame. /// </summary> /// <param name="dt">Time between frames.</param> public override void Update(float dt) { basis.rotationMatrix = entity.orientationMatrix; basis.ComputeWorldSpaceAxes(); float updateRate = 1 / dt; if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo. { Quaternion currentRelativeOrientation; var worldTransform = basis.WorldTransform; Matrix3x3.CreateQuaternion(ref worldTransform, out currentRelativeOrientation); //Compute the relative orientation R' between R and the target relative orientation. Quaternion errorOrientation; Quaternion.Conjugate(ref currentRelativeOrientation, out errorOrientation); Quaternion.Multiply(ref settings.servo.goal, ref errorOrientation, out errorOrientation); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness); //Turn this into an axis-angle representation. Toolbox.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle); //Scale the axis by the desired velocity if the angle is sufficiently large (epsilon). if (angle > Toolbox.BigEpsilon) { float velocity = MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle * updateRate) + angle * errorReduction; biasVelocity.X = axis.X * velocity; biasVelocity.Y = axis.Y * velocity; biasVelocity.Z = axis.Z * velocity; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { //Wouldn't want an old frame's bias velocity to sneak in. biasVelocity = new Vector3(); } } else { usedSoftness = settings.velocityMotor.softness * updateRate; angle = 0; //Zero out the error; Matrix3x3 transform = basis.WorldTransform; Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity); } //Compute effective mass effectiveMassMatrix = entity.inertiaTensorInverse; effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); }