// Compute the derived values if required ...
void FGQuaternion::ComputeDerivedUnconditional(void) const
{
mCacheValid = true;
double q0 = data[0]; // use some aliases/shorthand for the quat elements.
double q1 = data[1];
double q2 = data[2];
double q3 = data[3];
// Now compute the transformation matrix.
double q0q0 = q0*q0;
double q1q1 = q1*q1;
double q2q2 = q2*q2;
double q3q3 = q3*q3;
double q0q1 = q0*q1;
double q0q2 = q0*q2;
double q0q3 = q0*q3;
double q1q2 = q1*q2;
double q1q3 = q1*q3;
double q2q3 = q2*q3;
mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
mT(1,2) = 2.0*(q1q2 + q0q3); // Stevens and Lewis
mT(1,3) = 2.0*(q1q3 - q0q2);
mT(2,1) = 2.0*(q1q2 - q0q3);
mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
mT(2,3) = 2.0*(q2q3 + q0q1);
mT(3,1) = 2.0*(q1q3 + q0q2);
mT(3,2) = 2.0*(q2q3 - q0q1);
mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
// Since this is an orthogonal matrix, the inverse is simply the transpose.
mTInv = mT;
mTInv.T();
// Compute the Euler-angles
mEulerAngles = mT.GetEuler();
// FIXME: may be one can compute those values easier ???
mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
// mEulerSines(eTht) = sin(mEulerAngles(eTht));
mEulerSines(eTht) = -mT(1,3);
mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
mEulerCosines(eTht) = cos(mEulerAngles(eTht));
mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
}
// Compute the derived values if required ...
void FGQuaternion::ComputeDerivedUnconditional(void) const
{
mCacheValid = true;
double q0 = data[0]; // use some aliases/shorthand for the quat elements.
double q1 = data[1];
double q2 = data[2];
double q3 = data[3];
// Now compute the transformation matrix.
double q0q0 = q0*q0;
double q1q1 = q1*q1;
double q2q2 = q2*q2;
double q3q3 = q3*q3;
double q0q1 = q0*q1;
double q0q2 = q0*q2;
double q0q3 = q0*q3;
double q1q2 = q1*q2;
double q1q3 = q1*q3;
double q2q3 = q2*q3;
mT(1,1) = q0q0 + q1q1 - q2q2 - q3q3; // This is found from Eqn. 1.3-32 in
mT(1,2) = 2.0*(q1q2 + q0q3); // Stevens and Lewis
mT(1,3) = 2.0*(q1q3 - q0q2);
mT(2,1) = 2.0*(q1q2 - q0q3);
mT(2,2) = q0q0 - q1q1 + q2q2 - q3q3;
mT(2,3) = 2.0*(q2q3 + q0q1);
mT(3,1) = 2.0*(q1q3 + q0q2);
mT(3,2) = 2.0*(q2q3 - q0q1);
mT(3,3) = q0q0 - q1q1 - q2q2 + q3q3;
// Since this is an orthogonal matrix, the inverse is simply the transpose.
mTInv = mT;
mTInv.T();
// Compute the Euler-angles
// Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
if (mT(3,3) == 0.0)
mEulerAngles(ePhi) = 0.5*M_PI;
else
mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
if (mT(1,3) < -1.0)
mEulerAngles(eTht) = 0.5*M_PI;
else if (1.0 < mT(1,3))
mEulerAngles(eTht) = -0.5*M_PI;
else
mEulerAngles(eTht) = asin(-mT(1,3));
if (mT(1,1) == 0.0)
mEulerAngles(ePsi) = 0.5*M_PI;
else {
double psi = atan2(mT(1,2), mT(1,1));
if (psi < 0.0)
psi += 2*M_PI;
mEulerAngles(ePsi) = psi;
}
// FIXME: may be one can compute those values easier ???
mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
// mEulerSines(eTht) = sin(mEulerAngles(eTht));
mEulerSines(eTht) = -mT(1,3);
mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
mEulerCosines(eTht) = cos(mEulerAngles(eTht));
mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
}
// Compute the derived values if required ...
void FGQuaternion::ComputeDerivedUnconditional(void) const
{
mCacheValid = true;
// First normalize the 4-vector
double norm = Magnitude();
if (norm == 0.0)
return;
double rnorm = 1.0/norm;
double q1 = rnorm*Entry(1);
double q2 = rnorm*Entry(2);
double q3 = rnorm*Entry(3);
double q4 = rnorm*Entry(4);
// Now compute the transformation matrix.
double q1q1 = q1*q1;
double q2q2 = q2*q2;
double q3q3 = q3*q3;
double q4q4 = q4*q4;
double q1q2 = q1*q2;
double q1q3 = q1*q3;
double q1q4 = q1*q4;
double q2q3 = q2*q3;
double q2q4 = q2*q4;
double q3q4 = q3*q4;
mT(1,1) = q1q1 + q2q2 - q3q3 - q4q4;
mT(1,2) = 2.0*(q2q3 + q1q4);
mT(1,3) = 2.0*(q2q4 - q1q3);
mT(2,1) = 2.0*(q2q3 - q1q4);
mT(2,2) = q1q1 - q2q2 + q3q3 - q4q4;
mT(2,3) = 2.0*(q3q4 + q1q2);
mT(3,1) = 2.0*(q2q4 + q1q3);
mT(3,2) = 2.0*(q3q4 - q1q2);
mT(3,3) = q1q1 - q2q2 - q3q3 + q4q4;
// Since this is an orthogonal matrix, the inverse is simply
// the transpose.
mTInv = mT;
mTInv.T();
// Compute the Euler-angles
if (mT(3,3) == 0.0)
mEulerAngles(ePhi) = 0.5*M_PI;
else
mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3));
if (mT(1,3) < -1.0)
mEulerAngles(eTht) = 0.5*M_PI;
else if (1.0 < mT(1,3))
mEulerAngles(eTht) = -0.5*M_PI;
else
mEulerAngles(eTht) = asin(-mT(1,3));
if (mT(1,1) == 0.0)
mEulerAngles(ePsi) = 0.5*M_PI;
else {
double psi = atan2(mT(1,2), mT(1,1));
if (psi < 0.0)
psi += 2*M_PI;
mEulerAngles(ePsi) = psi;
}
// FIXME: may be one can compute those values easier ???
mEulerSines(ePhi) = sin(mEulerAngles(ePhi));
// mEulerSines(eTht) = sin(mEulerAngles(eTht));
mEulerSines(eTht) = -mT(1,3);
mEulerSines(ePsi) = sin(mEulerAngles(ePsi));
mEulerCosines(ePhi) = cos(mEulerAngles(ePhi));
mEulerCosines(eTht) = cos(mEulerAngles(eTht));
mEulerCosines(ePsi) = cos(mEulerAngles(ePsi));
}
