dgVector dgQuaternion::CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const
{
dgQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dgQuaternion dq (q0.Inverse() * q1);
dgVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3, dgFloat32 (0.0f));
dgFloat32 dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dgFloat32(dgFloat32 (1.0e-5f) * dgFloat32 (1.0e-5f))) {
return dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
}
dgFloat32 dirMagInv = dgRsqrt (dirMag2);
dgFloat32 dirMag = dirMag2 * dirMagInv;
dgFloat32 omegaMag = dgFloat32(2.0f) * dgAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale3 (dirMagInv * omegaMag);
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const
{
dQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dQuaternion dq (q0.Inverse() * q1);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dFloat dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dFloat dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dFloat dirMag = dirMag2 * dirMagInv;
dFloat omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &QB, dFloat dt) const
{
dFloat dirMag;
dFloat dirMag2;
dFloat omegaMag;
dFloat dirMagInv;
dQuaternion dq (Inverse() * QB);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dirMag = dirMag2 * dirMagInv;
omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) / dt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
