void IForceUpdateable.UpdateForForces(float dt)
{
//Linear velocity
if (IsAffectedByGravity)
{
Vector3.Add(ref forceUpdater.gravityDt, ref linearVelocity, out linearVelocity);
}
//Boost damping at very low velocities. This is a strong stabilizer; removes a ton of energy from the system.
if (activityInformation.DeactivationManager.useStabilization && activityInformation.allowStabilization &&
(activityInformation.isSlowing || activityInformation.velocityTimeBelowLimit > activityInformation.DeactivationManager.lowVelocityTimeMinimum))
{
float energy = linearVelocity.LengthSquared() + angularVelocity.LengthSquared();
if (energy < activityInformation.DeactivationManager.velocityLowerLimitSquared)
{
float boost = 1 - (float)(Math.Sqrt(energy) / (2f * activityInformation.DeactivationManager.velocityLowerLimit));
ModifyAngularDamping(boost);
ModifyLinearDamping(boost);
}
}
//Damping
float linear = LinearDamping + linearDampingBoost;
if (linear > 0)
{
Vector3.Multiply(ref linearVelocity, (float)Math.Pow(MathHelper.Clamp(1 - linear, 0, 1), dt), out linearVelocity);
}
//When applying angular damping, the momentum or velocity is damped depending on the conservation setting.
float angular = AngularDamping + angularDampingBoost;
if (angular > 0 && MotionSettings.ConserveAngularMomentum)
{
Vector3.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum);
}
else if (angular > 0)
{
Vector3.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity);
}
linearDampingBoost = 0;
angularDampingBoost = 0;
//Linear momentum
Vector3.Multiply(ref linearVelocity, mass, out linearMomentum);
//Update world inertia tensors.
Matrix3x3 multiplied;
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse);
Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor);
//Update angular velocity or angular momentum.
if (MotionSettings.ConserveAngularMomentum)
{
Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity);
}
else
{
Matrix3x3.Transform(ref angularVelocity, ref inertiaTensor, out angularMomentum);
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(linearMomentum);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(angularMomentum);
}