/**
* Convert quaternions to direction cosines.
* @param aQ1 1st Quaternion.
* @param aQ2 2nd Quaternion.
* @param aQ3 3rd Quaternion.
* @param aQ4 4th Quaternion.
* @param rDirCos Vector of direction cosines.
*/
void SimbodyEngine::convertQuaternionsToDirectionCosines(double aQ1, double aQ2, double aQ3, double aQ4, double rDirCos[3][3]) const
{
Rotation R;
R.setRotationFromQuaternion(Quaternion(Vec4(aQ1, aQ2, aQ3, aQ4)));
Mat33::updAs(&rDirCos[0][0]) = R.asMat33();
}
/**
* Convert quaternions to direction cosines.
* @param aQ1 1st Quaternion.
* @param aQ2 2nd Quaternion.
* @param aQ3 3rd Quaternion.
* @param aQ4 4th Quaternion.
* @param rDirCos Vector of direction cosines.
*/
void SimbodyEngine::convertQuaternionsToDirectionCosines(double aQ1, double aQ2, double aQ3, double aQ4, double *rDirCos) const
{
if(rDirCos==NULL) return;
Rotation R;
R.setRotationFromQuaternion(Quaternion(Vec4(aQ1, aQ2, aQ3, aQ4)));
Mat33::updAs(rDirCos) = R.asMat33();
}
//-------------------------------------------------------------------
bool testQuaternion( Real e0, Real e1, Real e2, Real e3 )
{
// Construct quaternion and normalize it
Quaternion qe0e1e2e3( e0, e1, e2, e3 );
// Convert quaternion to a Rotation matrix
Rotation rotationSpecified( qe0e1e2e3 );
// Convert Rotation back to quaternion
Quaternion qTest = rotationSpecified.convertRotationToQuaternion();
// Convert quaternion to a Rotation matrix
Rotation testRotation; testRotation.setRotationFromQuaternion( qTest );
// Test to see if they are the same
bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
return test;
}