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)
{
#if CONSERVE
Vector3.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum);
#else
Vector3.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity);
#endif
}
linearDampingBoost = 0;
angularDampingBoost = 0;
//Update world inertia tensors.
Matrix3x3 multiplied;
Matrix3x3.MultiplyTransposed(ref orientationMatrix4, ref localInertiaTensorInverse, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix4, out inertiaTensorInverse);
Matrix3x3.MultiplyTransposed(ref orientationMatrix4, ref localInertiaTensor, out multiplied);
Matrix3x3.Multiply(ref multiplied, ref orientationMatrix4, out inertiaTensor);
#if CONSERVE
//Update angular velocity.
//Note that this doesn't play nice with singular inertia tensors.
//Locked tensors result in zero angular velocity.
Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity);
MathChecker.Validate(angularMomentum);
#endif
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3 increment;
Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
Quaternion.Add(ref orientation, ref multiplier, out orientation);
orientation.Normalize();
Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix4);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3.Multiply(ref linearVelocity, dt, out increment);
Vector3.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
//The position update is complete if this is a discretely updated object.
if (PositionUpdated != null)
{
PositionUpdated(this);
}
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
#if CONSERVE
MathChecker.Validate(angularMomentum);
#endif
}
//These methods are very direct and quick. They don't activate the object or anything.
/// <summary>
/// Applies a linear velocity change to the entity using the given impulse.
/// This method does not wake up the object or perform any other nonessential operation;
/// it is meant to be used for performance-sensitive constraint solving.
/// Consider equivalently adding to the LinearMomentum property for convenience instead.
/// </summary>
/// <param name="impulse">Impulse to apply.</param>
public void ApplyLinearImpulse(ref Vector3 impulse)
{
linearVelocity.X += impulse.X * inverseMass;
linearVelocity.Y += impulse.Y * inverseMass;
linearVelocity.Z += impulse.Z * inverseMass;
MathChecker.Validate(linearVelocity);
}
/// <summary>
/// Applies an angular velocity change to the entity using the given impulse.
/// This method does not wake up the object or perform any other nonessential operation;
/// it is meant to be used for performance-sensitive constraint solving.
/// Consider equivalently adding to the AngularMomentum property for convenience instead.
/// </summary>
/// <param name="impulse">Impulse to apply.</param>
public void ApplyAngularImpulse(ref Vector3 impulse)
{
//There's some room here for SIMD-friendliness. However, since the phone doesn't accelerate non-XNA types, the Matrix3x3 operations don't gain much.
#if CONSERVE
angularMomentum.X += impulse.X;
angularMomentum.Y += impulse.Y;
angularMomentum.Z += impulse.Z;
angularVelocity.X = angularMomentum.X * inertiaTensorInverse.M11 + angularMomentum.Y * inertiaTensorInverse.M21 + angularMomentum.Z * inertiaTensorInverse.M31;
angularVelocity.Y = angularMomentum.X * inertiaTensorInverse.M12 + angularMomentum.Y * inertiaTensorInverse.M22 + angularMomentum.Z * inertiaTensorInverse.M32;
angularVelocity.Z = angularMomentum.X * inertiaTensorInverse.M13 + angularMomentum.Y * inertiaTensorInverse.M23 + angularMomentum.Z * inertiaTensorInverse.M33;
MathChecker.Validate(angularMomentum);
#else
angularVelocity.X += impulse.X * inertiaTensorInverse.M11 + impulse.Y * inertiaTensorInverse.M21 + impulse.Z * inertiaTensorInverse.M31;
angularVelocity.Y += impulse.X * inertiaTensorInverse.M12 + impulse.Y * inertiaTensorInverse.M22 + impulse.Z * inertiaTensorInverse.M32;
angularVelocity.Z += impulse.X * inertiaTensorInverse.M13 + impulse.Y * inertiaTensorInverse.M23 + impulse.Z * inertiaTensorInverse.M33;
#endif
MathChecker.Validate(angularVelocity);
}
void ICCDPositionUpdateable.UpdatePositionContinuously(float dt)
{
float minimumToi = 1;
for (int i = 0; i < collisionInformation.pairs.Count; i++)
{
if (collisionInformation.pairs.Elements[i].timeOfImpact < minimumToi)
{
minimumToi = collisionInformation.pairs.Elements[i].timeOfImpact;
}
}
//The orientation was already updated by the PreUpdatePosition.
//However, to be here, this object is not a discretely updated object.
//That means we still need to update the linear motion.
Vector3 increment;
Vector3.Multiply(ref linearVelocity, dt * minimumToi, out increment);
Vector3.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
if (PositionUpdated != null)
{
PositionUpdated(this);
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
#if CONSERVE
MathChecker.Validate(angularMomentum);
#endif
}