[ODE] Energy Status

[Bill Sellers]

Dear All,

I want to calculate the energy status of the bodies in my simulation  
and whilst I think the following is right I'd really appreciate it if  
someone who's 3D physics wasn't as rusty as mine could take a look at  
the following (very straightforward) code.

I'm pretty damn certain that linear kinetic is right, and I think  
that gravitational potential energy is OK too but the rotational  
kinetic energy could easily be wrong (I'm getting the rotational  
velocity in body local coordinates, squaring the individual values,  
multiplying it by the inertial tensor, summing the values and halving  
the whole thing). I've checked that it gives sensible results in  
simple test cases (i.e. effectively 2D) but I've no idea whether I  
get the right answer when I'm not using the principle moments of  
inertia and I'm a bit worried that there are interaction terms with  
linear and kinetic velocity that I might have to deal with (although  
I have always though they were independent when the centre of mass  
was used as the origin).

If the code happens to be right then these might make useful  
functions...

Cheers
Bill

dReal Body::GetRotationalKineticEnergy()
{

     // rotational KE = 0.5 I omega^2
     dMass mass;
     dBodyGetMass(m_BodyID, &mass);

     const dReal *ow = dBodyGetAngularVel(m_BodyID);
     dVector3 o;
     dBodyVectorFromWorld (m_BodyID, ow[0], ow[1], ow[2], o);

     dVector3 oo;
     oo[0] = o[0] * o[0]; oo[1] = o[1] * o[1]; oo[2] = o[2] * o[2];
     dVector3 rke;
     dMULTIPLYOP0_331(rke, =, mass.I, oo);
     dReal rotationalKE = 0.5 * (rke[0] + rke[1] + rke[2]);

     return rotationalKE;
}

dReal Body::GetLinearKineticEnergy()
{
     // linear KE = 0.5 m v^2
     dMass mass;
     dBodyGetMass(m_BodyID, &mass);

     const dReal *v = dBodyGetLinearVel(m_BodyID);
     dReal linearKE = 0.5 * mass.mass * (v[0]*v[0] + v[1]*v[1] + v[2] 
*v[2]);

     return linearKE;
}

dReal Body::GetGravitationalPotentialEnergy()
{
     dMass mass;
     dBodyGetMass(m_BodyID, &mass);
     dVector3 g;
     dWorldGetGravity (m_WorldID, g);
     const dReal *p = dBodyGetPosition(m_BodyID);

     // gravitational PE = mgh
     dReal gravitationalPotentialEnergy = - mass.mass * (g[0]*p[0] + g 
[1]*p[1] + g[2]*p[2]);

     return gravitationalPotentialEnergy;
}