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; }