public void evalEulerEqnDeriv( ref btVector3 w1, ref btVector3 w0, double dt,
ref btMatrix3x3 I, out btMatrix3x3 result )
{
btMatrix3x3 w1x, Iw1x;
btMatrix3x3 tmpm;
btMatrix3x3 tmp2;
btVector3 Iwi; I.Apply( ref w1, out Iwi );
w1.getSkewSymmetricMatrix( out w1x.m_el0, out w1x.m_el1, out w1x.m_el2 );
#if !DISABLE_ROW4
w1x.m_el3 = btVector3.wAxis;
#endif
Iwi.getSkewSymmetricMatrix( out Iw1x.m_el0, out Iw1x.m_el1, out Iw1x.m_el2 );
#if !DISABLE_ROW4
Iw1x.m_el3 = btVector3.wAxis;
#endif
I.Mult( ref w1x, out tmpm );
tmpm.Sub( ref Iw1x, out tmp2 );
tmp2.Mult( dt, out result );
//btMatrix3x3 dfw1 = I + ( w1x * I - Iw1x ) * dt;
//return dfw1;
}
public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt,
ref btMatrix3x3 I, out btVector3 result )
{
btVector3 Iw0;
btVector3 dtT;
btVector3 Iw1;
btVector3 cross;
T.Mult( dt, out dtT );
I.Apply( ref w0, out Iw0 );
dtT.Add( ref Iw0, out Iw0 );
// Iw0 = ( T * dt + I * w0 )
I.Apply( ref w1, out Iw1 );
w1.cross( ref Iw1, out cross );
cross.Mult( dt, out cross );
// cross = w1.cross( I * w1 ) * dt
Iw1.Add( ref cross, out Iw1 );
// Iw1 = Iw1 + cross
Iw1.Sub( ref Iw0, out result );
//btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 );
//return w2;
}
