/**
* Convert angles to direction cosines.
* @param aE1 1st Euler angle.
* @param aE2 2nd Euler angle.
* @param aE3 3rd Euler angle.
* @param rDirCos Vector of direction cosines.
*/
void SimbodyEngine::convertAnglesToDirectionCosines(double aE1, double aE2, double aE3, double rDirCos[3][3]) const
{
Vec3 angs(aE1, aE2, aE3);
Rotation aRot;
aRot.setRotationToBodyFixedXYZ(angs);
Mat33::updAs(&rDirCos[0][0]) = aRot.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[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 doRequiredTasks( ) {
// Use the next Rotation to test against (simple theta-lambda rotation)
Rotation testRotation;
// Test default constructor
Rotation defaultRotationConstructor;
testRotation.setRotationToIdentityMatrix();
bool test = defaultRotationConstructor.areAllRotationElementsSameToMachinePrecision( testRotation );
// Test copy constructor
testRotation.setRotationFromAngleAboutNonUnitVector( 1.0, Vec3(0.2, 0.4, 0.6) );
Rotation rotationCopyConstructor( testRotation );
test = test && rotationCopyConstructor.areAllRotationElementsSameToMachinePrecision( testRotation );
// Test operator =
Rotation rotationOperatorEqual = testRotation;
test = test && rotationOperatorEqual.areAllRotationElementsSameToMachinePrecision( testRotation );
// Test rotation by angle about arbitrary CoordinateAxis
testRotation.setRotationFromAngleAboutNonUnitVector( 0.1, Vec3(1.0, 0.0, 0.0) );
CoordinateAxis coordAxis = XAxis;
Rotation rotationCoordAxis( 0.1, coordAxis );
test = test && rotationCoordAxis.areAllRotationElementsSameToMachinePrecision( testRotation );
Real testTheta = rotationCoordAxis.convertOneAxisRotationToOneAngle( coordAxis );
test = test && fabs(0.1 - testTheta) < 10*SignificantReal;
// Test rotation by angle about XAxis, YAxis, ZAxis
test = test && testRotationOneAxis( 0.2, XAxis );
test = test && testRotationOneAxis( -0.2, XAxis );
test = test && testRotationOneAxis( 2.1, YAxis );
test = test && testRotationOneAxis( -2.1, YAxis );
test = test && testRotationOneAxis( 3.1, ZAxis );
test = test && testRotationOneAxis( -3.1, ZAxis );
// Test rotation with two angles and two axes XX, XY, XZ
test = test && testRotationTwoAxes( BodyRotationSequence, 0.2, XAxis, 0.3, XAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, XAxis );
test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, XAxis,-1.3, YAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, YAxis );
test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, XAxis, 1.2, ZAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, ZAxis );
// Test rotation with two angles and two axes YX, YY, YZ
test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, YAxis, 0.3, XAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, YAxis, 0.3, XAxis );
test = test && testRotationTwoAxes( BodyRotationSequence, 2.2, YAxis,-1.3, YAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, 2.2, YAxis,-1.3, YAxis );
test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, YAxis, 1.2, ZAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, ZAxis );
// Test rotation with two angles and two axes ZX, ZY, ZZ
test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, ZAxis, 0.3, XAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, ZAxis, 0.3, XAxis );
test = test && testRotationTwoAxes( BodyRotationSequence, 2.2, ZAxis,-1.3, YAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, 2.2, ZAxis,-1.3, YAxis );
test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, ZAxis );
test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, ZAxis );
// Test the construction of rotations from two given axes.
const UnitVec3 vi(0.01, 0.02, .9);
const Vec3 vj(-0.5, 0.5, 0.2);
test = test && testRotationFromTwoGivenAxes(vi, XAxis, vj, YAxis);
test = test && testRotationFromTwoGivenAxes(vi, YAxis, vj, XAxis);
test = test && testRotationFromTwoGivenAxes(vi, YAxis, vj, ZAxis);
test = test && testRotationFromTwoGivenAxes(vi, ZAxis, vj, YAxis);
test = test && testRotationFromTwoGivenAxes(vi, ZAxis, vj, XAxis);
test = test && testRotationFromTwoGivenAxes(vi, XAxis, vj, ZAxis);
// Test rotation with three angles and three axes XXX, XXY, XXZ, XYX, XYY, XYZ, XZX, XZY, XZZ
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, XAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, XAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, XAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, XAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, XAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, XAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, YAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, YAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, YAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, YAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, YAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, YAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, ZAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, ZAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, ZAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, ZAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, ZAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, ZAxis, 1.3, ZAxis );
// Test rotation with three angles and three axes YXX, YXY, YXZ, YYX, YYY, YYZ, YZX, YZY, YZZ
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, XAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, XAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, XAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, XAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, XAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, XAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, YAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, YAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, YAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, YAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, YAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, YAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, ZAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, ZAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, ZAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, ZAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, ZAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, ZAxis, 1.3, ZAxis );
// Test rotation with three angles and three axes ZXX, ZXY, ZXZ, ZYX, ZYY, ZYZ, ZZX, ZZY, ZZZ
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, XAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, XAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, XAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, XAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, XAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, XAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, YAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, YAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, YAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, YAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, YAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, YAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, ZAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, ZAxis, 0.4, XAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, ZAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, ZAxis,-1.4, YAxis );
test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, ZAxis, 1.3, ZAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, ZAxis, 1.3, ZAxis );
// This used to fail
test = test && testRotationThreeAxes( BodyRotationSequence, -3.2288591161895095, XAxis, -3.1415926535897931, YAxis, -3.1415926535897931, XAxis );
test = test && testRotationThreeAxes( SpaceRotationSequence, -3.2288591161895095, XAxis, -3.1415926535897931, YAxis, -3.1415926535897931, XAxis );
// Test Rotation quaternion methods.
test = test && testQuaternion( 0.5, 0.1, 0.2, 0.3 );
test = test && testQuaternion(-0.5, 0.1, 0.2, -0.3 );
Quaternion unnorm(Vec4(1,2,3,4), true); // don't do this at home
SimTK_TEST_NOTEQ(unnorm.norm(), Real(1)); // shouldn't have normalized
Quaternion fixedUp;
fixedUp = unnorm.normalize();
SimTK_TEST_EQ(fixedUp.norm(), Real(1));
unnorm.normalizeThis();
SimTK_TEST_EQ(unnorm.norm(), Real(1));
// Test construction of nearby orthogonal rotation matrix from a generic Mat33.
Rotation nearbyRotation( testRotation.asMat33() );
test = test && nearbyRotation.areAllRotationElementsSameToMachinePrecision( testRotation );
// Exhaustive test of 1-angle, 2-angle, and 3-angle rotations
test = test && exhaustiveTestof1AngleRotation();
test = test && exhaustiveTestof2AngleRotation();
test = test && exhaustiveTestof3AngleRotation();
// Exhaustive test of 3-angle rotations near singularity
test = test && exhaustiveTestof3AngleTwoAxesRotationNearSingularity();
test = test && exhaustiveTestof3AngleThreeAxesRotationNearSingularity();
// Exhaustive test of Quaterions
test = test && exhaustiveTestofQuaternions();
// Test special handling of body-fixed XYZ.
test = test && testSetRotationToBodyFixedXYZ();
// Test out special code for rotating symmetric matrices.
test = test && testReexpressSymMat33();
return test;
}