public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)
{
predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;
//exponential map
Vector3 axis;
float angle = angularVelocity.Length();
//limit the angular motion
if (angle * timeStep > AngularMotionTreshold)
{
angle = AngularMotionTreshold / timeStep;
}
if (angle < 0.001f)
{
// use Taylor's expansions of sync function
axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);
}
Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));
Quaternion ornA = MatrixOperations.GetRotation(currentTransform);
Quaternion predictedOrn = dorn * ornA;
predictedOrn.Normalize();
MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);
Matrix test = Matrix.CreateFromQuaternion(predictedOrn);
}
// synchronizes world transform from physics to user
// Bullet only calls the update of worldtransform for active objects
public override void SetWorldTransform(Matrix centerOfMassWorldTrans)
{
_graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset);
_graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation;
}