void IPositionUpdateable.PreUpdatePosition(float dt)
{
Vector3 increment;
if (MotionSettings.UseRk4AngularIntegration && isDynamic)
{
Toolbox.UpdateOrientationRK4(ref orientation, ref localInertiaTensorInverse, ref angularMomentum, dt, out orientation);
}
else
{
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 orientationMatrix);
//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);
}
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
MathChecker.Validate(linearMomentum);
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularMomentum);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
}