public static void UpdateOrientationRK4(ref Quaternion q, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, float dt, out Quaternion newOrientation)
{
//TODO: This is a little goofy
//Quaternion diff = differentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum);
Quaternion d1;
DifferentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum, out d1);
Quaternion s2;
Quaternion.Multiply(ref d1, dt * .5f, out s2);
Quaternion.Add(ref q, ref s2, out s2);
Quaternion d2;
DifferentiateQuaternion(ref s2, ref localInertiaTensorInverse, ref angularMomentum, out d2);
Quaternion s3;
Quaternion.Multiply(ref d2, dt * .5f, out s3);
Quaternion.Add(ref q, ref s3, out s3);
Quaternion d3;
DifferentiateQuaternion(ref s3, ref localInertiaTensorInverse, ref angularMomentum, out d3);
Quaternion s4;
Quaternion.Multiply(ref d3, dt, out s4);
Quaternion.Add(ref q, ref s4, out s4);
Quaternion d4;
DifferentiateQuaternion(ref s4, ref localInertiaTensorInverse, ref angularMomentum, out d4);
Quaternion.Multiply(ref d1, dt / 6, out d1);
Quaternion.Multiply(ref d2, dt / 3, out d2);
Quaternion.Multiply(ref d3, dt / 3, out d3);
Quaternion.Multiply(ref d4, dt / 6, out d4);
Quaternion added;
Quaternion.Add(ref q, ref d1, out added);
Quaternion.Add(ref added, ref d2, out added);
Quaternion.Add(ref added, ref d3, out added);
Quaternion.Add(ref added, ref d4, out added);
Quaternion.Normalize(ref added, out newOrientation);
}
