/*************************************************************************
  *                                                                       *
  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
  *                                                                       *
  * This library is free software; you can redistribute it and/or         *
  * modify it under the terms of EITHER:                                  *
  *   (1) The GNU Lesser General Public License as published by the Free  *
  *       Software Foundation; either version 2.1 of the License, or (at  *
  *       your option) any later version. The text of the GNU Lesser      *
  *       General Public License is included with this library in the     *
  *       file LICENSE.TXT.                                               *
  *   (2) The BSD-style license that is included with this library in     *
  *       the file LICENSE-BSD.TXT.                                       *
  *                                                                       *
  * This library is distributed in the hope that it will be useful,       *
  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  *                                                                       *
  *************************************************************************/

////////////////////////////////////////////////////////////////////////////////
// This file creates unit tests for some of the functions found in:
// ode/src/joinst/universal.cpp
//
//
////////////////////////////////////////////////////////////////////////////////

#include <iostream>

#include <UnitTest++.h>
#include <ode/ode.h>

#include "../../ode/src/config.h"
#include "../../ode/src/joints/universal.h"

dReal d2r(dReal degree)
{
    return degree * (dReal)(M_PI / 180.0);
}
dReal r2d(dReal degree)
{
    return degree * (dReal)(180.0/M_PI);
}

SUITE (TestdxJointUniversal)
{
    // The 2 bodies are positioned at (0, 0, 0)
    // The bodies have no rotation.
    // The joint is a Universal Joint
    // Axis1 is along the X axis
    // Axis2 is along the Y axis
    // Anchor at (0, 0, 0)
    struct Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y
    {
        Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y()
        {

            wId = dWorldCreate();

            bId1 = dBodyCreate (wId);
            dBodySetPosition (bId1, 0, 0, 0);

            bId2 = dBodyCreate (wId);
            dBodySetPosition (bId2, 0, 0, 0);


            jId   = dJointCreateUniversal (wId, 0);
            joint = (dxJointUniversal*) jId;


            dJointAttach (jId, bId1, bId2);
        }

        ~Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId1;
        dBodyID bId2;


        dJointID jId;
        dxJointUniversal* joint;
    };


    // The 2 bodies are positioned at (-1, -2, -3) and (11, 22, 33)
    // The bodies have rotation of 27deg around some axis.
    // The joint is a Universal Joint
    // Axis is along the X axis
    // Anchor at (0, 0, 0)
    struct Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X
    {
        Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X()
        {
            wId = dWorldCreate();

            bId1 = dBodyCreate (wId);
            dBodySetPosition (bId1, -1, -2, -3);

            bId2 = dBodyCreate (wId);
            dBodySetPosition (bId2, 11, 22, 33);

            dMatrix3 R;

            dVector3 axis;

            axis[0] =  REAL(0.53);
            axis[1] = -REAL(0.71);
            axis[2] =  REAL(0.43);
            dNormalize3(axis);
            dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
                                REAL(0.47123)); // 27deg
            dBodySetRotation (bId1, R);


            axis[0] =  REAL(1.2);
            axis[1] =  REAL(0.87);
            axis[2] = -REAL(0.33);
            dNormalize3(axis);
            dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
                                REAL(0.47123)); // 27deg
            dBodySetRotation (bId2, R);

            jId   = dJointCreateUniversal (wId, 0);
            joint = (dxJointUniversal*) jId;


            dJointAttach (jId, bId1, bId2);
        }

        ~Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId1;
        dBodyID bId2;


        dJointID jId;
        dxJointUniversal* joint;
    };


    // Only one body body1 at (0,0,0)
    // The joint is an Universal Joint.
    // Axis1 is along the X axis
    // Axis2 is along the Y axis
    // Anchor at (0, 0, 0)
    //
    //       ^Y
    //       |
    //       |
    //       |
    //       |
    //       |
    // Z <-- X
    struct Fixture_dxJointUniversal_B1_At_Zero_Default_Axes
    {
        Fixture_dxJointUniversal_B1_At_Zero_Default_Axes()
        {
            wId = dWorldCreate();

            bId1 = dBodyCreate (wId);
            dBodySetPosition (bId1, 0, 0, 0);

            jId   = dJointCreateUniversal (wId, 0);


            dJointAttach (jId, bId1, NULL);
            dJointSetUniversalAnchor (jId, 0, 0, 0);
        }

        ~Fixture_dxJointUniversal_B1_At_Zero_Default_Axes()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId1;


        dJointID jId;
    };



    // Only one body body2 at (0,0,0)
    // The joint is an Universal Joint.
    // Axis1 is along the X axis.
    // Axis2 is along the Y axis.
    // Anchor at (0, 0, 0)
    //
    //       ^Y
    //       |
    //       |
    //       |
    //       |
    //       |
    // Z <-- X
    struct Fixture_dxJointUniversal_B2_At_Zero_Default_Axes
    {
        Fixture_dxJointUniversal_B2_At_Zero_Default_Axes()
        {
            wId = dWorldCreate();

            bId2 = dBodyCreate (wId);
            dBodySetPosition (bId2, 0, 0, 0);

            jId   = dJointCreateUniversal (wId, 0);


            dJointAttach (jId, NULL, bId2);
            dJointSetUniversalAnchor (jId, 0, 0, 0);
        }

        ~Fixture_dxJointUniversal_B2_At_Zero_Default_Axes()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId2;

        dJointID jId;
    };


    // Test is dJointGetUniversalAngles versus
    // dJointGetUniversalAngle1 and dJointGetUniversalAngle2 dJointGetUniversalAxis
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetGetUniversalAngles_Versus_Angle1_and_Angle2)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dMatrix3 R;
        dReal ang1, ang2;


        dVector3 axis1;
        dJointGetUniversalAxis1 (jId, axis1);

        dVector3 axis2;
        dJointGetUniversalAxis2 (jId, axis2);

        ang1 = d2r(REAL(23.0));
        dRFromAxisAndAngle (R, axis1[0], axis1[1], axis1[2], ang1);
        dBodySetRotation (bId1, R);

        ang2 = d2r(REAL(17.0));
        dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);






        // ax1 and ax2 are pseudo-random axis. N.B. They are NOT the axis of the joints.
        dVector3 ax1;
        ax1[0] =  REAL(0.2);
        ax1[1] = -REAL(0.67);
        ax1[2] = -REAL(0.81);
        dNormalize3(ax1);

        dVector3 ax2;
        ax2[0] = REAL(0.62);
        ax2[1] = REAL(0.31);
        ax2[2] = REAL(0.43);
        dNormalize3(ax2);


        ang1 = d2r(REAL(23.0));
        dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang1);
        dBodySetRotation (bId1, R);

        ang2 = d2r(REAL(0.0));

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4);



        ang1 = d2r(REAL(0.0));

        ang2 = d2r(REAL(23.0));
        dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2);
        dBodySetRotation (bId1, R);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4);


        ang1 = d2r(REAL(38.0));
        dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang2);
        dBodySetRotation (bId1, R);

        ang2 = d2r(REAL(-43.0));
        dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2);
        dBodySetRotation (bId1, R);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4);


        // Try with random axis for the axis of the joints
        dRSetIdentity(R);
        dBodySetRotation (bId1, R);
        dBodySetRotation (bId1, R);

        axis1[0] =  REAL(0.32);
        axis1[1] = -REAL(0.57);
        axis1[2] =  REAL(0.71);
        dNormalize3(axis1);

        axis2[0] = -REAL(0.26);
        axis2[1] = -REAL(0.31);
        axis2[2] =  REAL(0.69);
        dNormalize3(axis2);

        dVector3 cross;
        dCalcVectorCross3(cross, axis1, axis2);
        dJointSetUniversalAxis1(jId, axis1[0], axis1[1], axis1[2]);
        dJointSetUniversalAxis2(jId, cross[0], cross[1], cross[2]);


        ang1 = d2r(REAL(23.0));
        dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang1);
        dBodySetRotation (bId1, R);

        ang2 = d2r(REAL(0.0));

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4);



        ang1 = d2r(REAL(0.0));

        ang2 = d2r(REAL(23.0));
        dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2);
        dBodySetRotation (bId1, R);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4);


        ang1 = d2r(REAL(38.0));
        dRFromAxisAndAngle (R, ax1[0], ax1[1], ax1[2], ang2);
        dBodySetRotation (bId1, R);

        ang2 = d2r(REAL(-43.0));
        dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2);
        dBodySetRotation (bId1, R);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (angle2, dJointGetUniversalAngle2 (jId), 1e-4);
    }


    // =========================================================================
    // Test ONE BODY behavior
    // =========================================================================


    // Test when there is only one body at position one on the joint
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes,
                  test_dJointGetUniversalAngle1_1Body_B1)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis1;
        dJointGetUniversalAxis1 (jId, axis1);
        dVector3 axis2;
        dJointGetUniversalAxis2 (jId, axis2);

        dMatrix3 R;

        dReal ang1 = REAL(0.23);
        dRFromAxisAndAngle (R, axis1[0], axis1[1], axis1[2], ang1);
        dBodySetRotation (bId1, R);

        dReal ang2 = REAL(0.0);


        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);



        dMatrix3 I;
        dRSetIdentity(I); // Set the rotation of the body to be the Identity (i.e. zero)
        dBodySetRotation (bId1, I);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);


        // Test the same rotation, when axis1 is inverted
        dJointSetUniversalAxis1 (jId, -axis1[0], -axis1[1], -axis1[2]);

        dBodySetRotation (bId1, R);

        CHECK_CLOSE (-ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (-ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        // Test the same rotation, when axis1 is default and axis2 is inverted
        dBodySetRotation (bId1, I);

        dJointSetUniversalAxis1 (jId, axis1[0], axis1[1], axis1[2]);
        dJointSetUniversalAxis2 (jId, -axis2[0], -axis2[1], -axis2[2]);


        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);
    }




    // Test when there is only one body at position two on the joint
    TEST_FIXTURE (Fixture_dxJointUniversal_B2_At_Zero_Default_Axes,
                  test_dJointGetUniversalAngle1_1Body_B2)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis1;
        dJointGetUniversalAxis1 (jId, axis1);

        dVector3 axis2;
        dJointGetUniversalAxis2 (jId, axis2);

        dMatrix3 R;

        dReal ang1 = REAL(0.0);

        dReal ang2 = REAL(0.23);
        dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2);
        dBodySetRotation (bId2, R);



        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);



        dMatrix3 I;
        dRSetIdentity(I); // Set the rotation of the body to be the Identity (i.e. zero)
        dBodySetRotation (bId2, I);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis2 (jId, -axis2[0], -axis2[1], -axis2[2]);

        dBodySetRotation (bId2, R);

        CHECK_CLOSE (-ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (-ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);

        // Test the same rotation, when axis1 is inverted and axis2 is default
        dBodySetRotation (bId2, I);

        dJointSetUniversalAxis1 (jId, -axis1[0], -axis1[1], -axis1[2]);
        dJointSetUniversalAxis2 (jId, axis2[0], axis2[1], axis2[2]);


        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);
    }






    // =========================================================================
    //
    // =========================================================================


    // Test is dJointSetUniversalAxis and dJointGetUniversalAxis return same value
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Random_Axis_Along_X,
                  test_dJointSetGetUniversalAxis)
    {
        dVector3 axisOrig, axis;


        dJointGetUniversalAxis1 (jId, axisOrig);
        dJointGetUniversalAxis1 (jId, axis);
        dJointSetUniversalAxis1 (jId, axis[0], axis[1], axis[2]);
        dJointGetUniversalAxis1 (jId, axis);
        CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4);
        CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4);
        CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4);


        dJointGetUniversalAxis2 (jId, axisOrig);
        dJointGetUniversalAxis2(jId, axis);
        dJointSetUniversalAxis2 (jId, axis[0], axis[1], axis[2]);
        dJointGetUniversalAxis2 (jId, axis);
        CHECK_CLOSE (axis[0], axisOrig[0] , 1e-4);
        CHECK_CLOSE (axis[1], axisOrig[1] , 1e-4);
        CHECK_CLOSE (axis[2], axisOrig[2] , 1e-4);


        dVector3 anchor1, anchor2, anchorOrig1, anchorOrig2;
        dJointGetUniversalAnchor (jId, anchorOrig1);
        dJointGetUniversalAnchor (jId, anchor1);
        dJointGetUniversalAnchor2 (jId, anchorOrig2);
        dJointGetUniversalAnchor2 (jId, anchor2);

        dJointSetUniversalAnchor (jId, anchor1[0], anchor1[1], anchor1[2]);
        dJointGetUniversalAnchor (jId, anchor1);
        dJointGetUniversalAnchor2 (jId, anchor2);
        CHECK_CLOSE (anchor1[0], anchorOrig1[0] , 1e-4);
        CHECK_CLOSE (anchor1[1], anchorOrig1[1] , 1e-4);
        CHECK_CLOSE (anchor1[2], anchorOrig1[2] , 1e-4);

        CHECK_CLOSE (anchor2[0], anchorOrig2[0] , 1e-4);
        CHECK_CLOSE (anchor2[1], anchorOrig2[1] , 1e-4);
        CHECK_CLOSE (anchor2[2], anchorOrig2[2] , 1e-4);
    }



    // Create 2 bodies attached by a Universal joint
    // Axis is along the X axis (Default value)
    // Anchor at (0, 0, 0)      (Default value)
    //
    //       ^Y
    //       |
    //       * Body2
    //       |
    //       |
    // Body1 |
    // *     Z-------->
    struct dxJointUniversal_Test_Initialization
    {
        dxJointUniversal_Test_Initialization()
        {
            wId = dWorldCreate();

            // Remove gravity to have the only force be the force of the joint
            dWorldSetGravity(wId, 0,0,0);

            for (int j=0; j<2; ++j)
            {
                bId[j][0] = dBodyCreate (wId);
                dBodySetPosition (bId[j][0], -1, -2, -3);

                bId[j][1] = dBodyCreate (wId);
                dBodySetPosition (bId[j][1], 11, 22, 33);


                dMatrix3 R;
                dVector3 axis; // Random axis

                axis[0] =  REAL(0.53);
                axis[1] = -REAL(0.71);
                axis[2] =  REAL(0.43);
                dNormalize3(axis);
                dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
                                    REAL(0.47123)); // 27deg
                dBodySetRotation (bId[j][0], R);


                axis[0] =  REAL(1.2);
                axis[1] =  REAL(0.87);
                axis[2] = -REAL(0.33);
                dNormalize3(axis);
                dRFromAxisAndAngle (R, axis[0], axis[1], axis[2],
                                    REAL(0.47123)); // 27deg
                dBodySetRotation (bId[j][1], R);

                jId[j]   = dJointCreateUniversal (wId, 0);
                dJointAttach (jId[j], bId[j][0], bId[j][1]);
                dJointSetUniversalParam(jId[j], dParamLoStop, 1);
                dJointSetUniversalParam(jId[j], dParamHiStop, 2);
                dJointSetUniversalParam(jId[j], dParamFMax, 200);
            }
        }

        ~dxJointUniversal_Test_Initialization()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId[2][2];


        dJointID jId[2];

    };


    // Test if setting a Universal with its default values
    // will behave the same as a default Universal joint
    TEST_FIXTURE (dxJointUniversal_Test_Initialization,
                  test_Universal_Initialization)
    {
        using namespace std;

        dVector3 axis;
        dJointGetUniversalAxis1(jId[1], axis);
        dJointSetUniversalAxis1(jId[1], axis[0], axis[1], axis[2]);

        dJointGetUniversalAxis2(jId[1], axis);
        dJointSetUniversalAxis2(jId[1], axis[0], axis[1], axis[2]);

        dVector3 anchor;
        dJointGetUniversalAnchor(jId[1], anchor);
        dJointSetUniversalAnchor(jId[1], anchor[0], anchor[1], anchor[2]);


        for (int b=0; b<2; ++b)
        {
            // Compare body b of the first joint with its equivalent on the
            // second joint
            const dReal *qA = dBodyGetQuaternion(bId[0][b]);
            const dReal *qB = dBodyGetQuaternion(bId[1][b]);
            CHECK_CLOSE (qA[0], qB[0], 1e-4);
            CHECK_CLOSE (qA[1], qB[1], 1e-4);
            CHECK_CLOSE (qA[2], qB[2], 1e-4);
            CHECK_CLOSE (qA[3], qB[3], 1e-4);
        }

        dWorldStep (wId,0.5);
        dWorldStep (wId,0.5);
        dWorldStep (wId,0.5);
        dWorldStep (wId,0.5);

        for (int b=0; b<2; ++b)
        {
            // Compare body b of the first joint with its equivalent on the
            // second joint
            const dReal *qA = dBodyGetQuaternion(bId[0][b]);
            const dReal *qB = dBodyGetQuaternion(bId[1][b]);
            CHECK_CLOSE (qA[0], qB[0], 1e-4);
            CHECK_CLOSE (qA[1], qB[1], 1e-4);
            CHECK_CLOSE (qA[2], qB[2], 1e-4);
            CHECK_CLOSE (qA[3], qB[3], 1e-4);


            const dReal *posA = dBodyGetPosition(bId[0][b]);
            const dReal *posB = dBodyGetPosition(bId[1][b]);
            CHECK_CLOSE (posA[0], posB[0], 1e-4);
            CHECK_CLOSE (posA[1], posB[1], 1e-4);
            CHECK_CLOSE (posA[2], posB[2], 1e-4);
            CHECK_CLOSE (posA[3], posB[3], 1e-4);
        }
    }













    // ==========================================================================
    // Testing the offset
    // TODO:
    // - Test Axis1Offset(...., 0, ang2);
    // ==========================================================================


    //  Rotate first body 90deg around X (Axis1) then back to original position
    //
    //    ^  ^           ^           Z ^
    //    |  |  => <---  |             |
    //    |  |           |             |
    //   B1  B2     B1   B2            .----->Y
    //                                /
    //                               /
    //                              v X    (N.B. X is going out of the screen)
    //
    //  Set Axis1 with an Offset of 90deg
    //       ^        ^   ^
    //  <--- |  =>    |   |
    //       |        |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxis1Offset_B1_90deg)
    {
        dMatrix3 R;

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);


        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);

        dReal ang1 = d2r(REAL(90.0));
        dReal ang2 = d2r(REAL(0.0));
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);



        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }

    //  Rotate 2nd body 90deg around (Axis2) then back to original position
    //  Offset when setting axis1
    //
    //    ^  ^           ^           Z ^
    //    |  |  => <---  |             |
    //    |  |           |             |
    //   B1  B2     B1   B2            .----->Y
    //                                /
    //                               /
    //                              v X    (N.B. X is going out of the screen)
    //
    //  Set Axis1 with an Offset of 90deg
    //       ^        ^   ^
    //  <--- |  =>    |   |
    //       |        |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxis1Offset_B2_90deg)
    {
        dMatrix3 R;

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);


        dVector3 ax1, ax2;
        dJointGetUniversalAxis1 (jId, ax1);
        dJointGetUniversalAxis2 (jId, ax2);

        dReal ang1 = d2r(REAL(0.0));
        dReal ang2 = d2r(REAL(90.0));
        dRFromAxisAndAngle (R, ax2[0], ax2[1], ax2[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);



        dJointSetUniversalAxis1Offset (jId, ax1[0], ax1[1], ax1[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }








    //  Rotate second body 90deg around Y (Axis2) then back to original position
    //
    //    ^  ^       ^           Z ^
    //    |  |  =>   |   .         |
    //    |  |       |             |
    //   B1  B2     B1   B2        .----->Y
    //                            /
    //                           /
    //                          v X    (N.B. X is going out of the screen)
    //
    //  Set Axis2 with an Offset of 90deg
    //   ^           ^   ^
    //   |   .  =>   |   |
    //   |           |   |
    //   B1  B2     B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxisOffset_B2_90deg)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = d2r(REAL(0.0));
        dReal ang2 = d2r(REAL(90.0));

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }



    //  Rotate 2nd body -90deg around Y (Axis2) then back to original position
    //
    //    ^  ^       ^               Z ^
    //    |  |  =>   |   x             |
    //    |  |       |                 |
    //   B1  B2     B1   B2          X .----> Y
    //                               N.B. X is going out of the screen
    //  Start with a Delta of 90deg
    //    ^           ^   ^
    //    |  x  =>    |   |
    //    |           |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxisOffset_B2_Minus90deg)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);
        CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = d2r(REAL(0.0));
        dReal ang2 = d2r(REAL(90.0));

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], -ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);



        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);


        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], 0);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }


    //  Rotate 1st body 0.23rad around X (Axis1) then back to original position
    //
    //    ^  ^     ^      ^           Z ^
    //    |  |  =>  \     |             |
    //    |  |       \    |             |
    //   B1  B2     B1   B2             .-------> Y
    //                                 /
    //                                /
    //                               v X  (N.B. X is going out of the screen)
    //
    //  Start with a Delta of 0.23rad
    //  ^    ^        ^   ^
    //   \   | =>     |   |
    //    \  |        |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxis1Offset_B1_0_23rad)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);

        dReal ang1 = REAL(0.23);
        dReal ang2 = REAL(0.0);

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }

    //  Rotate 2nd body 0.23rad around Y (Axis2) then back to original position
    //
    //    ^  ^      ^     ^           Z ^   ^ Y (N.B. Y is going in the screen)
    //    |  |  =>  |    /              |  /
    //    |  |      |   /               | /
    //   B1  B2     B1  B2              .-------> X
    //
    //  Start with a Delta of 0.23rad
    //   ^     ^    ^   ^
    //   |    /  => |   |
    //   |   /      |   |
    //   B1  B2     B1  B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxisOffset_B2_0_23rad)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = REAL(0.0);
        dReal ang2 = REAL(0.23);

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }


    // Rotate 1st body 0.23rad around X axis and 2nd body 0.37rad around Y (Axis2)
    // then back to their original position.
    // The Axis offsets are set one at a time
    //
    //    ^  ^    ^         ^          Z ^   ^ Y (N.B. Y is going in the screen)
    //    |  |  => \      /             |  /
    //    |  |      \   /               | /
    //   B1  B2     B1  B2              .-------> X
    //
    //  Start with a Delta of 0.23rad
    // ^         ^  ^   ^
    //  \      / => |   |
    //   \   /      |   |
    //   B1  B2     B1  B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Along_X_Axis2_Along_Y,
                  test_dJointSetUniversalAxisOffset_B1_0_23rad_B2_0_37rad_One_by_One)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis1;
        dJointGetUniversalAxis1 (jId, axis1);
        dVector3 axis2;
        dJointGetUniversalAxis2 (jId, axis2);

        dMatrix3 R;

        dReal ang1 = REAL(0.23);
        dRFromAxisAndAngle (R, axis1[0], axis1[1], axis1[2], ang1);
        dBodySetRotation (bId1, R);

        dReal ang2 = REAL(0.37);
        dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2);
        dBodySetRotation (bId2, R);


        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dJointSetUniversalAxis1Offset (jId, axis1[0], axis1[1], axis1[2],
                                       ang1, -ang2 );
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);

        dJointGetUniversalAxis1 (jId, axis1);
        dJointGetUniversalAxis2 (jId, axis2);

        dRFromAxisAndAngle (R, axis2[0], axis2[1], axis2[2], ang2);
        dBodySetRotation (bId2, R);

        dJointSetUniversalAxis2Offset (jId, axis2[0], axis2[1], axis2[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }



//  The 2 bodies are positioned at (0, 0, 0) with no rotation
//  The joint is an Universal Joint.
//  Axis in the inverse direction of the X axis
//  Anchor at (0, 0, 0)
//          ^Y
//          |
//          |
//          |
//          |
//          |
//  Z <---- x (X going out of the page)
    struct Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X
    {
        Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X()
        {
            wId = dWorldCreate();

            bId1 = dBodyCreate (wId);
            dBodySetPosition (bId1, 0, 0, 0);

            bId2 = dBodyCreate (wId);
            dBodySetPosition (bId2, 0, 0, 0);

            jId   = dJointCreateUniversal (wId, 0);
            joint = (dxJointUniversal*) jId;


            dJointAttach (jId, bId1, bId2);
            dJointSetUniversalAnchor (jId, 0, 0, 0);

            axis[0] = -1;
            axis[1] = 0;
            axis[2] = 0;
            dJointSetUniversalAxis1(jId, axis[0], axis[1], axis[2]);
        }

        ~Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId1;
        dBodyID bId2;


        dJointID jId;
        dxJointUniversal* joint;

        dVector3 axis;
    };


    // No offset when setting the Axis1 offset
    // x is a Symbol for lines pointing into the screen
    // . is a Symbol for lines pointing out of the screen
    //
    //    In 2D                   In 3D
    //    ^  ^      ^    ^        Z ^   ^ Y
    //    |  |  =>  |    |          |  /
    //    |  |      |    |          | /
    //   B1  B2     B1   B2         .-------> X     <-- Axis1
    //
    //  Start with a Delta of 90deg
    //    ^  ^         ^   ^
    //    |  |    =>  |   |
    //    |  |        |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxis1Offset_No_Offset_Axis1_Inverse_of_X)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);
        CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);

        dReal ang1 = REAL(0.0);
        dReal ang2 = REAL(0.0);

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);

        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }



    //  Rotate 1st body 90deg around axis1 then back to original position
    //  x is a Symbol for lines pointing into the screen
    //  . is a Symbol for lines pointing out of the screen
    //
    //    In 2D                   In 3D
    //    ^  ^           ^        Z ^   ^ Y
    //    |  |  =>   x   |          |  /
    //    |  |           |          | /
    //   B1  B2     B1   B2         .-------> X     <-- Axis1
    //
    //  Start with a Delta of 90deg
    //       ^         ^   ^
    //    x  |    =>  |   |
    //       |        |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxis1Offset_B1_90Deg_Axis1_Inverse_of_X)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);
        CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);

        dReal ang1 = d2r(90);
        dReal ang2 = REAL(0.0);

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (ang2, angle2, 1e-4);

        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }



    // No offset when setting the Axis 2 offset
    // x is a Symbol for lines pointing into the screen
    // . is a Symbol for lines pointing out of the screen
    //
    //    In 2D                   In 3D
    //    ^  ^       ^   ^        Z ^   ^ Y             ^ Axis2
    //    |  |  =>   |   |          |  /               /
    //    |  |       |   |          | /               /
    //   B1  B2     B1   B2         . ------->    <-- Axis1
    //
    //  Start with a Delta of 90deg
    //    ^  ^        ^   ^
    //    |  |    =>  |   |
    //    |  |        |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxis2Offset_No_Offset_Axis2_Inverse_of_X)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);
        CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = d2r(REAL(0.0));
        dReal ang2 = d2r(REAL(0.0));

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);

        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }

    //  Rotate 2nd body 90deg around axis2 then back to original position
    //
    //    In 2D                   In 3D
    //    ^  ^       ^            Z ^   ^ Y             ^ Axis2
    //    |  |  =>   |   -->        |  /               /
    //    |  |       |              | /               /
    //   B1  B2     B1   B2         . ------->    <-- Axis1
    //
    //  Start with a Delta of 90deg
    //    ^           ^   ^
    //    | <---  =>  |   |
    //    |           |   |
    //   B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxisOffset_B2_90Deg)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);
        CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = d2r(REAL(0.0));
        dReal ang2 = d2r(REAL(90.0));

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);

        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }


    //  Rotate 2nd body -90deg around axis2 then back to original position
    //
    //   ^  ^       ^
    //   |  |  =>   |  --->
    //   |  |       |
    //  B1  B2     B1   B2
    //
    // Start with a Delta of 90deg
    //   ^           ^   ^
    //   | --->  =>  |   |
    //   |           |   |
    //  B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxis1Offset_B2_Minus90Deg)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);
        CHECK_CLOSE (dJointGetUniversalAngle2 (jId), 0, 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = d2r(0.0);
        dReal ang2 = d2r(REAL(-90.0));


        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dJointGetUniversalAxis1 (jId, axis);
        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);


        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }


    // Rotate 1st body 0.23rad around X then back to original position
    //
    //   ^  ^     ^      ^
    //   |  |  =>  \     |
    //   |  |       \    |
    //  B1  B2     B1   B2
    //
    // Start with a Delta of 0.23rad
    // ^    ^        ^   ^
    //  \   | =>     |   |
    //   \  |        |   |
    //  B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxis1Offset_B1_0_23rad)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);

        dReal ang1 = REAL(0.23);
        dReal ang2 = REAL(0.0);

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],  ang1, ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }


    // Rotate 2nd body -0.23rad around Z then back to original position
    //
    //   ^  ^         ^  ^
    //   |  |  =>    /   |
    //   |  |       /    |
    //  B1  B2     B1   B2
    //
    // Start with a Delta of 0.23rad
    //     ^ ^        ^   ^
    //    /  | =>     |   |
    //   /   |        |   |
    //  B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_At_Zero_Axis1_Inverse_of_X,
                  test_dJointSetUniversalAxisOffset_B1_Minus0_23rad)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);

        dMatrix3 R;
        dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);
        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],  REAL(0.23), 0);
        CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4);

        dRFromAxisAndAngle (R, 1, 0, 0, 0);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
    }




    // Rotate the body by 90deg around X then back to original position.
    // The body is attached at the second position of the joint:
    // dJointAttach(jId, 0, bId);
    //
    //   ^
    //   |  => <---
    //   |
    //  B1      B1
    //
    // Start with a Delta of 90deg
    //            ^
    //  <---  =>  |
    //            |
    //   B1      B1
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes,
                  test_dJointSetUniversalAxisOffset_1Body_B1_90Deg)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);

        dMatrix3 R;
        dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (M_PI/2.0, dJointGetUniversalAngle1 (jId), 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);
        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],  M_PI/2.0, 0);
        CHECK_CLOSE (M_PI/2.0, dJointGetUniversalAngle1 (jId), 1e-4);

        dRFromAxisAndAngle (R, 1, 0, 0, 0);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
    }

    // Rotate the body by -0.23rad around X then back to original position.
    // The body is attached at the second position of the joint:
    // dJointAttach(jId, 0, bId);
    //
    //   ^         ^
    //   |  =>    /
    //   |       /
    //  B1      B1
    //
    // Start with a Delta of -0.23rad
    //     ^     ^
    //    /  =>  |
    //   /       |
    //   B1     B1
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes,
                  test_dJointSetUniversalAxisOffset_1Body_B1_Minus0_23rad)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);

        dMatrix3 R;
        dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (-REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);
        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],  -REAL(0.23), 0);
        CHECK_CLOSE (-REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4);

        dRFromAxisAndAngle (R, 1, 0, 0, 0);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
    }



    // Only one body body1 at (0,0,0)
    // The joint is an Universal Joint.
    // Axis the inverse of the X axis
    // Anchor at (0, 0, 0)
    //
    //       ^Y
    //       |
    //       |
    //       |
    //       |
    //       |
    // Z <-- X
    struct Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X
    {
        Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X()
        {
            wId = dWorldCreate();

            bId1 = dBodyCreate (wId);
            dBodySetPosition (bId1, 0, 0, 0);

            jId   = dJointCreateUniversal (wId, 0);
            joint = (dxJointUniversal*) jId;


            dJointAttach (jId, bId1, NULL);
            dJointSetUniversalAnchor (jId, 0, 0, 0);

            axis[0] = -1;
            axis[1] = 0;
            axis[2] = 0;
            dJointSetUniversalAxis1(jId, axis[0], axis[1], axis[2]);
        }

        ~Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X()
        {
            dWorldDestroy (wId);
        }

        dWorldID wId;

        dBodyID bId1;


        dJointID jId;
        dxJointUniversal* joint;

        dVector3 axis;
    };

    // Rotate B1 by 90deg around X then back to original position
    //
    //   ^
    //   |  => <---
    //   |
    //  B1      B1
    //
    // Start with a Delta of 90deg
    //            ^
    //  <---  =>  |
    //            |
    //   B1      B1
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X,
                  test_dJointSetUniversalAxisOffset_1Body_B1_90Deg)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1(jId, axis);

        dReal ang1 = d2r(REAL(90.0));

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang1);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);

        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], ang1, 0);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);

        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
    }

    // Rotate B1 by -0.23rad around X then back to original position
    //
    //   ^         ^
    //   |  =>    /
    //   |       /
    //  B1      B1
    //
    // Start with a Delta of -0.23rad
    //     ^     ^
    //    /  =>  |
    //   /       |
    //   B1     B1
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Axis_Inverse_of_X,
                  test_dJointSetUniversalAxisOffset_1Body_B1_Minus0_23rad)
    {
        CHECK_CLOSE (dJointGetUniversalAngle1 (jId), 0, 1e-4);

        dMatrix3 R;
        dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);
        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2],  REAL(0.23), 0);
        CHECK_CLOSE (REAL(0.23), dJointGetUniversalAngle1 (jId), 1e-4);

        dRFromAxisAndAngle (R, 1, 0, 0, 0);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
    }







    // Rotate B2 by 90deg around X then back to original position
    //
    //   ^
    //   |  => <---
    //   |
    //  B2      B2
    //
    // Start with a Delta of 90deg
    //            ^
    //  <---  =>  |
    //            |
    //   B2      B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B2_At_Zero_Default_Axes,
                  test_dJointSetUniversalAxisOffset_1Body_B2_90Deg)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);


        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang2 = d2r(REAL(90.0));

        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2], 0, -ang2);
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);


        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);
    }

    // Rotate B2 by -0.23rad around Y then back to original position
    //
    //   ^         ^
    //   |  =>    /
    //   |       /
    //  B2      B2
    //
    // Start with an offset of -0.23rad
    //     ^     ^
    //    /  =>  |
    //   /       |
    //   B2     B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B2_At_Zero_Default_Axes,
                  test_dJointSetUniversalAxis2Offset_1Body_B2_Minus0_23rad)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);

        dVector3 axis;
        dJointGetUniversalAxis2 (jId, axis);

        dReal ang1 = 0;
        dReal ang2 = REAL(-0.23);


        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], ang2);
        dBodySetRotation (bId2, R);


        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (ang1, angle1, 1e-4);
        CHECK_CLOSE (-ang2, angle2, 1e-4);


        dJointSetUniversalAxis2Offset (jId, axis[0], axis[1], axis[2],
                                       ang1, -ang2);
        CHECK_CLOSE (ang1, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (-ang2, dJointGetUniversalAngle2 (jId), 1e-4);

        dRSetIdentity(R); // Set the rotation of the body to be zero
        dBodySetRotation (bId2, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }












    // The 2 bodies are positioned at (0,0,0) and (0,0,0)
    // The bodies have no rotation.
    // The joint is a Universal Joint
    // The axis of the joint are at random (Still at 90deg w.r.t each other)
    // Anchor at (0, 0, 0)
    struct Fixture_dxJointUniversal_B1_and_B2_Axis_Random
    {
        Fixture_dxJointUniversal_B1_and_B2_Axis_Random()
        {
            wId = dWorldCreate();

            bId1 = dBodyCreate (wId);
            dBodySetPosition (bId1, -1, -2, -3);

            bId2 = dBodyCreate (wId);
            dBodySetPosition (bId2, 11, 22, 33);


            jId   = dJointCreateUniversal (wId, 0);


            dJointAttach (jId, bId1, bId2);

            dVector3 axis1;
            axis1[0] =  REAL(0.53);
            axis1[1] = -REAL(0.71);
            axis1[2] =  REAL(0.43);
            dNormalize3(axis1);

            dVector3 axis;
            axis[0] =  REAL(1.2);
            axis[1] =  REAL(0.87);
            axis[2] = -REAL(0.33);

            dVector3 axis2;
            dCalcVectorCross3(axis2, axis1, axis);

            dJointSetUniversalAxis1(jId, axis1[0], axis1[1], axis1[2]);
            dJointSetUniversalAxis2(jId, axis2[0], axis2[1], axis2[2]);
        }

        ~Fixture_dxJointUniversal_B1_and_B2_Axis_Random()
        {
            dWorldDestroy (wId);
        }


        dWorldID wId;

        dBodyID bId1;
        dBodyID bId2;


        dJointID jId;
    };


    // Rotate first body 90deg around Axis1 then back to original position
    //
    //   ^  ^           ^       Z ^
    //   |  |  => <---  |         |
    //   |  |           |         |
    //  B1  B2     B1   B2      X .----->Y
    //                          N.B. X is going out of the screen
    // Set Axis1 with an Offset of 90deg
    //      ^        ^   ^
    // <--- |  =>    |   |
    //      |        |   |
    //  B1  B2      B1   B2
    TEST_FIXTURE (Fixture_dxJointUniversal_B1_and_B2_Axis_Random,
                  test_dJointSetUniversalAxisOffset_B1_90deg_Axis_Random)
    {
        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);
        dReal angle1, angle2;
        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);


        dVector3 axis;
        dJointGetUniversalAxis1 (jId, axis);

        dReal angle = d2r(90);
        dMatrix3 R;
        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], angle);
        dBodySetRotation (bId1, R);


        CHECK_CLOSE (angle, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);



        dJointSetUniversalAxis1Offset (jId, axis[0], axis[1], axis[2], angle, 0);
        CHECK_CLOSE (angle, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (angle, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);


        dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], 0);
        dBodySetRotation (bId1, R);

        CHECK_CLOSE (0, dJointGetUniversalAngle1 (jId), 1e-4);
        CHECK_CLOSE (0, dJointGetUniversalAngle2 (jId), 1e-4);

        dJointGetUniversalAngles(jId, &angle1, &angle2);
        CHECK_CLOSE (0, angle1, 1e-4);
        CHECK_CLOSE (0, angle2, 1e-4);
    }

} // End of SUITE TestdxJointUniversal