public static double EnergyFromOrientation(DQuaternion o, DVector3 L, DVector3 I)
{
var ir = DQuaternion.Inverse(o);
DVector3 body_l = ir * L;
double e = body_l.x * body_l.x / I.x +
body_l.y * body_l.y / I.y +
body_l.z * body_l.z / I.z;
e *= .5f;
return(e);
}
private void Normalize()
{
// Renormalize the orientation, and make sure angular momentum
// and energy are being conserved.
Orientation.Normalize();
if (ApplyAdjustment)
{
Orientation = ELSolver.AdjustOrientation(Orientation);
var ir = DQuaternion.Inverse(Orientation);
var body_l = ir * L;
_BodyOmega.x = body_l.x / I.x;
_BodyOmega.y = body_l.y / I.y;
_BodyOmega.z = body_l.z / I.z;
}
}
void UpdateOmega(targ_type dt)
{
var body_omega = BodyOmega;
DVector3 body_omega_dt = new DVector3();
// Euler's equations for torque free motion.
// Even the the Unity coordinate system is left-handed, this still works.
// Uses explicit Euler integration...
body_omega_dt.x = (I.y - I.z) * body_omega.y * body_omega.z / I.x;
body_omega_dt.y = (I.z - I.x) * body_omega.z * body_omega.x / I.y;
body_omega_dt.z = (I.x - I.y) * body_omega.x * body_omega.y / I.z;
body_omega += body_omega_dt * dt;
BodyOmega = body_omega;
Debug.Assert(DVector3.Distance(body_omega, DQuaternion.Inverse(Orientation) * Omega) < 1.0e10);
}