Example #1
0
//-------------------------------------------------------------------
bool  testRotationOneAxis( const Real angle, const CoordinateAxis& axis ) {

    // Form rotation about specified axis
    Rotation rotationSpecified;
    if( axis == XAxis )  rotationSpecified.setRotationFromAngleAboutX( angle );
    if( axis == YAxis )  rotationSpecified.setRotationFromAngleAboutY( angle );
    if( axis == ZAxis )  rotationSpecified.setRotationFromAngleAboutZ( angle );

    // Form equivalent rotation by another means
    Real unitX = axis == XAxis ? 1 : 0;
    Real unitY = axis == YAxis ? 1 : 0;
    Real unitZ = axis == ZAxis ? 1 : 0;
    UnitVec3 unitVector( unitX, unitY, unitZ );
    Rotation testRotation( angle, unitVector );

    // Test to see if they are the same
    bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );

    // Do the inverse problem to back out the angle
    const Real theta = rotationSpecified.convertOneAxisRotationToOneAngle( axis );

    // Create a Rotation matrix with the backed-out angle and compare to the original Rotation matrix
    testRotation.setRotationFromAngleAboutAxis( theta, axis );
    test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );

    // Conversion should produce  angle = theta   if  angle is in proper range (-pi < angle <= pi)
    test = test && testInverseRotation1Angle( angle, theta );

    return test;
}
Example #2
0
//-------------------------------------------------------------------
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;
}