lovr-ODE/tests/joint.cpp

/*************************************************************************
  *                                                                       *
  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
  * All rights reserved.  Email: [email protected]   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.                     *
  *                                                                       *
  *************************************************************************/
//234567890123456789012345678901234567890123456789012345678901234567890123456789
//        1         2         3         4         5         6         7

////////////////////////////////////////////////////////////////////////////////
// This file create unit test for some of the functions found in:
// ode/src/joint.cpp
//
//
////////////////////////////////////////////////////////////////////////////////
#include <UnitTest++.h>
#include <ode/ode.h>
#include "../ode/src/joints/joints.h"


////////////////////////////////////////////////////////////////////////////////
// Testing the Hinge2 Joint
//
SUITE(JointHinge2)
{

  struct Hinge2GetInfo1_Fixture_1 {
    Hinge2GetInfo1_Fixture_1() {
      wId = dWorldCreate();

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

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


      jId = dJointCreateHinge2(wId, 0);
      joint = (dxJointHinge2*)jId;

      dJointAttach(jId, bId1, bId2);

      dJointSetHinge2Anchor (jId, REAL(0.0), REAL(0.0), REAL(0.0));
    }

    ~Hinge2GetInfo1_Fixture_1() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointHinge2* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };

TEST_FIXTURE(Hinge2GetInfo1_Fixture_1, test_hinge2GetInfo1)
{
  //       ^Y
  //     |---|                             HiStop
  //     |   |                     ^Y         /
  //     |B_2|                     |       /
  //     |---|                     |    /
  //       |               -----  | /
  // Z <-- *            Z<--|B_2|--*
  //     / | \              -----  | \
  //    /|---|\                  |---| \
  //   / |   | \                 |   |   \
  //  /  |B_1|  \                |B_1|     \
  // /   |---|   \               |---|       \
  //LoStop        HiStop                   LoStop
  //
  //
  //
  //
  dMatrix3 R;

  dJointSetHinge2Param(jId, dParamLoStop, -M_PI/4.0);
  dJointSetHinge2Param(jId, dParamHiStop,  M_PI/4.0);

  dxJoint::Info1 info;


  dxJointHinge2* joint = (dxJointHinge2*)jId;

  // Original position inside the limits
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);

  // Move the body outside the Lo limits
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(1, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // Keep the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, REAL(0.0), REAL(0.0), REAL(0.0));
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  // Move the body outside the Lo limits
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(1, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);



  // Return to original position
  // and remove the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, REAL(0.0), REAL(0.0), REAL(0.0));
  dBodySetRotation (bId2, R);
  dJointSetHinge2Param(jId, dParamLoStop, -2*M_PI);
  dJointSetHinge2Param(jId, dParamHiStop,  2*M_PI);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  // Set the limits
  // Move pass the Hi limits
  dJointSetHinge2Param(jId, dParamLoStop, -M_PI/4.0);
  dJointSetHinge2Param(jId, dParamHiStop,  M_PI/4.0);
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(2, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // Keep the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, REAL(0.0), REAL(0.0), REAL(0.0));
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  // Move the pass the Hi limit
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(2, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // and remove the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  dJointSetHinge2Param(jId, dParamLoStop, -2*M_PI);
  dJointSetHinge2Param(jId, dParamHiStop,  2*M_PI);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  /// Motorize the first joint angle
  dJointSetHinge2Param(jId, dParamFMax, 2);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  /// Motorize the second joint angle
  dJointSetHinge2Param(jId, dParamFMax2, 2);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(6, info.m);

  /// Unmotorize the first joint angle
  dJointSetHinge2Param(jId, dParamFMax, 0);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);
}
} // End of SUITE(JointHinge2)


////////////////////////////////////////////////////////////////////////////////
// Testing the Universal Joint
//
SUITE(JointUniversal)
{

  struct UniversalGetInfo1_Fixture_1 {
    UniversalGetInfo1_Fixture_1() {
      wId = dWorldCreate();

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

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


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

      dJointAttach(jId, bId1, bId2);

      dJointSetUniversalAnchor (jId, REAL(0.0), REAL(0.0), REAL(0.0));
    }

    ~UniversalGetInfo1_Fixture_1() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointUniversal* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };

TEST_FIXTURE(UniversalGetInfo1_Fixture_1, test_hinge2GetInfo1_RotAroundX)
{
  //       ^Y
  //     |---|                             HiStop
  //     |   |                     ^Y         /
  //     |B_2|                     |       /
  //     |---|                     |    /
  //       |               -----  | /
  // Z <-- *            Z<--|B_2|--*
  //     / | \              -----  | \
  //    /|---|\                  |---| \
  //   / |   | \                 |   |   \
  //  /  |B_1|  \                |B_1|     \
  // /   |---|   \               |---|       \
  //LoStop        HiStop                   LoStop
  //
  //
  //
  //
  dMatrix3 R;

  dJointSetUniversalParam(jId, dParamLoStop, -M_PI/4.0);
  dJointSetUniversalParam(jId, dParamHiStop,  M_PI/4.0);
  dJointSetUniversalParam(jId, dParamLoStop2, -M_PI/4.0);
  dJointSetUniversalParam(jId, dParamHiStop2,  M_PI/4.0);

  dxJoint::Info1 info;


  dxJointUniversal* joint = (dxJointUniversal*)jId;

  // Original position inside the limits
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);

  // Move the body outside the Lo limits
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(1, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // Keep the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, REAL(0.0), REAL(0.0), REAL(0.0));
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  // Move the body outside the Lo limits
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(1, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);



  // Return to original position
  // and remove the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, REAL(0.0), REAL(0.0), REAL(0.0));
  dBodySetRotation (bId2, R);
  dJointSetUniversalParam(jId, dParamLoStop, -2*M_PI);
  dJointSetUniversalParam(jId, dParamHiStop,  2*M_PI);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  // Set the limits
  // Move pass the Hi limits
  dJointSetUniversalParam(jId, dParamLoStop, -M_PI/4.0);
  dJointSetUniversalParam(jId, dParamHiStop,  M_PI/4.0);
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(2, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // Keep the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, REAL(0.0), REAL(0.0), REAL(0.0));
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  // Move the pass the Hi limit
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(2, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // and remove the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  dJointSetUniversalParam(jId, dParamLoStop, -2*M_PI);
  dJointSetUniversalParam(jId, dParamHiStop,  2*M_PI);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);


  /// Motorize the first joint angle
  dJointSetUniversalParam(jId, dParamFMax, 2);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);


  /// Motorize the second joint angle
  dJointSetUniversalParam(jId, dParamFMax2, 2);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(6, info.m);

  /// Unmotorize the first joint angle
  dJointSetUniversalParam(jId, dParamFMax, 0);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(5, info.m);
}

TEST_FIXTURE(UniversalGetInfo1_Fixture_1, test_hinge2GetInfo1_RotAroundY)
{
  //       ^Y
  //     |---|                             HiStop
  //     |   |                     ^Y         /
  //     |B_2|                     |       /
  //     |---|                     |    /
  //       |               -----  | /
  // Z <-- *            Z<--|B_2|--*
  //     / | \              -----  | \
  //    /|---|\                  |---| \
  //   / |   | \                 |   |   \
  //  /  |B_1|  \                |B_1|     \
  // /   |---|   \               |---|       \
  //LoStop        HiStop                   LoStop
  //
  //
  //
  //
  dMatrix3 R;

  dJointSetUniversalParam(jId, dParamLoStop, -M_PI/4.0);
  dJointSetUniversalParam(jId, dParamHiStop,  M_PI/4.0);
  dJointSetUniversalParam(jId, dParamLoStop2, -M_PI/4.0);
  dJointSetUniversalParam(jId, dParamHiStop2,  M_PI/4.0);

  dxJoint::Info1 info;


  dxJointUniversal* joint = (dxJointUniversal*)jId;

  // Original position inside the limits
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot1.limit);
  CHECK_EQUAL(4, info.m);

  // Move the body outside the Lo limits
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 0, 1, 0, M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(1, joint->limot2.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // Keep the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 0, 1, 0, 0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(4, info.m);


  // Move the body outside the Lo limits
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 0, 1, 0, M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(1, joint->limot2.limit);
  CHECK_EQUAL(5, info.m);



  // Return to original position
  // and remove the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 0, 1, 0, 0);
  dBodySetRotation (bId2, R);
  dJointSetUniversalParam(jId, dParamLoStop2, -2*M_PI);
  dJointSetUniversalParam(jId, dParamHiStop2,  2*M_PI);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(4, info.m);


  // Set the limits
  // Move pass the Hi limits
  dJointSetUniversalParam(jId, dParamLoStop2, -M_PI/4.0);
  dJointSetUniversalParam(jId, dParamHiStop2,  M_PI/4.0);
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 0, 1, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(2, joint->limot2.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // Keep the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 0, 1, 0, 0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(4, info.m);


  // Move the pass the Hi limit
  dBodySetPosition (bId2, 0, 0, 1);
  dRFromAxisAndAngle (R, 0, 1, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  joint->getInfo1(&info);
  CHECK_EQUAL(2, joint->limot2.limit);
  CHECK_EQUAL(5, info.m);


  // Return to original position
  // and remove the limits
  dBodySetPosition (bId2, 0, 1, 0);
  dRFromAxisAndAngle (R, 0, 1, 0, -M_PI/2.0);
  dBodySetRotation (bId2, R);
  dJointSetUniversalParam(jId, dParamLoStop2, -2*M_PI);
  dJointSetUniversalParam(jId, dParamHiStop2,  2*M_PI);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(4, info.m);


  /// Motorize the first joint angle
  dJointSetUniversalParam(jId, dParamFMax, 2);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(5, info.m);


  /// Motorize the second joint angle
  dJointSetUniversalParam(jId, dParamFMax2, 2);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(6, info.m);

  /// Unmotorize the first joint angle
  dJointSetUniversalParam(jId, dParamFMax, 0);
  joint->getInfo1(&info);
  CHECK_EQUAL(0, joint->limot2.limit);
  CHECK_EQUAL(5, info.m);
}
} // End of SUITE(JointUniversal)



// // //
// Testing the PR Joint
//
SUITE(JointPR)
{
  struct PRGetInfo1_Fixture_1 {
    PRGetInfo1_Fixture_1() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePR(wId, 0);
      joint = (dxJointPR*)jId;

      dJointAttach(jId, bId1, bId2);

      dJointSetPRAnchor (jId, REAL(0.0), REAL(0.0), REAL(0.0));
    }

    ~PRGetInfo1_Fixture_1() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPR* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };


////////////////////////////////////////////////////////////////////////////////
// Test when there is no limits.
// The 2 bodies stay aligned.
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_1, test1_PRGetInfo1_)
    {
      dJointSetPRParam(jId, dParamLoStop, -dInfinity);
      dJointSetPRParam(jId, dParamHiStop,  dInfinity);
      dJointSetPRParam(jId, dParamLoStop2, -M_PI);
      dJointSetPRParam(jId, dParamHiStop2, M_PI);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(4, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// The Body 2 is moved -100 unit then at 100
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_1, test2_PRGetInfo1)
    {
      dJointSetPRParam(jId, dParamLoStop, -10);
      dJointSetPRParam(jId, dParamHiStop,  10);
      dJointSetPRParam(jId, dParamLoStop2, -M_PI);
      dJointSetPRParam(jId, dParamHiStop2, M_PI);


      dBodySetPosition(bId2, 0, -100, 0);

      joint->getInfo1(&info);

      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);


      dBodySetPosition(bId2, 0, 100, 0);

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 0, 1, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(4, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for the rotoide at -45deg and 45deg.
// The Body 2 is only rotated by 90deg since the rotoide limits are not
// used this should not change the limit value.
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_1, test3_PRGetInfo1)
    {
      dJointSetPRParam(jId, dParamLoStop, -10);
      dJointSetPRParam(jId, dParamHiStop,  10);
      dJointSetPRParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPRParam(jId, dParamHiStop2, M_PI/4.0);


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

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(4, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 0, 1, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(4, info.m);
    }


// The joint is now powered. (i.e. info->fmax > 0
  struct PRGetInfo1_Fixture_2 {
    PRGetInfo1_Fixture_2() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePR(wId, 0);
      joint = (dxJointPR*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPRAnchor (jId, REAL(0.0), REAL(0.0), REAL(0.0));

      joint->limotP.fmax = 1;
    }

    ~PRGetInfo1_Fixture_2() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPR* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };



////////////////////////////////////////////////////////////////////////////////
// Test when there is no limits.
// The 2 bodies stay align.
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_2, test1_PRGetInfo1)
    {
      dJointSetPRParam(jId, dParamLoStop, -dInfinity);
      dJointSetPRParam(jId, dParamHiStop,  dInfinity);
      dJointSetPRParam(jId, dParamLoStop2, -M_PI);
      dJointSetPRParam(jId, dParamHiStop2, M_PI);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// The Body 2 is moved -100 unit then at 100
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_2, test2_PRGetInfo1)
    {

      dJointSetPRParam(jId, dParamLoStop, -10);
      dJointSetPRParam(jId, dParamHiStop,  10);
      dJointSetPRParam(jId, dParamLoStop2, -M_PI);
      dJointSetPRParam(jId, dParamHiStop2, M_PI);


      dBodySetPosition(bId2, 0, -100, 0);

      joint->getInfo1(&info);

      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);


      dBodySetPosition(bId2, 0, 100, 0);

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 0, 1, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for the rotoide at -45deg and 45deg
// The Body 2 is only rotated by 90deg since the rotoide limits are not
// used this should not change the limit value.
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_2, test3_PRGetInfo1)
    {

      dJointSetPRParam(jId, dParamLoStop, -10);
      dJointSetPRParam(jId, dParamHiStop,  10);
      dJointSetPRParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPRParam(jId, dParamHiStop2, M_PI/4.0);


      dMatrix3 R;
      dBodySetPosition (bId2, 0, 0, 100);
      dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
      dBodySetRotation (bId2, R);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 0, 1, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test the setting and getting of parameters
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_1, test_SetPRParam)
    {
      dJointSetPRParam(jId, dParamHiStop, REAL(5.0) );
      CHECK_EQUAL(REAL(5.0), joint->limotP.histop);

      dJointSetPRParam(jId, dParamVel, REAL(7.0) );
      CHECK_EQUAL(REAL(7.0), joint->limotP.vel);

#ifdef dParamFudgeFactor1
      dJointSetPRParam(jId, dParamFudgeFactor1, REAL(5.5) );
      CHECK_EQUAL(REAL(5.5), joint->limotP.dParamFudgeFactor);
#endif

      dJointSetPRParam(jId, dParamCFM2, REAL(9.0) );
      CHECK_EQUAL(REAL(9.0), joint->limotR.normal_cfm);

      dJointSetPRParam(jId, dParamStopERP2, REAL(11.0) );
      CHECK_EQUAL(REAL(11.0), joint->limotR.stop_erp);
    }

  TEST_FIXTURE(PRGetInfo1_Fixture_1, test_GetPRParam)
    {
      joint->limotP.histop = REAL(5.0);
      CHECK_EQUAL(joint->limotP.histop,
                  dJointGetPRParam(jId, dParamHiStop) );

      joint->limotP.vel = REAL(7.0);

      CHECK_EQUAL(joint->limotP.vel,
                  dJointGetPRParam(jId, dParamVel) );

#ifdef dParamFudgeFactor1
      joint->limotP.dParamFudgeFactor =  REAL(5.5);

      CHECK_EQUAL(joint->limotP.dParamFudgeFactor,
                  dJointGetPRParam(jId, dParamFudgeFactor1) );
#endif

      joint->limotR.normal_cfm = REAL(9.0);
      CHECK_EQUAL(joint->limotR.normal_cfm,
                  dJointGetPRParam(jId, dParamCFM2) );

      joint->limotR.stop_erp = REAL(11.0);
      CHECK_EQUAL(joint->limotR.stop_erp,
                  dJointGetPRParam(jId, dParamStopERP2) );
    }



////////////////////////////////////////////////////////////////////////////////
// Fixture for testing the PositionRate
//
// Default Position
//                       ^Z
//                       |
//                       |
//
//    Body2              R            Body1
//   +---------+         _      -    +-----------+
//   |         |--------(_)----|-----|           |  ----->Y
//   +---------+                -    +-----------+
//
// N.B. X is comming out of the page
////////////////////////////////////////////////////////////////////////////////
  struct PRGetInfo1_Fixture_3 {
    PRGetInfo1_Fixture_3() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePR(wId, 0);
      joint = (dxJointPR*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPRAnchor (jId, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel (bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel (bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

    }

    ~PRGetInfo1_Fixture_3() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPR* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [0,  1, 0]
// Position Body2 [0, -1, 0]
// Axis of the prismatic [0, 1, 0]
// Axis of the rotoide   [1, 0, ]0
//
// Move at the same speed
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_3, test_GetPRPositionRate_1)
    {
      // They move with the same linear speed
      // Angular speed == 0
      dBodySetLinearVel(bId1, REAL(0.0), REAL(3.33), REAL(0.0));
      dBodySetLinearVel(bId2, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(1.11), REAL(3.33), REAL(0.0));
      dBodySetLinearVel(bId2, REAL(1.11), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(1.11), REAL(3.33), REAL(2.22));
      dBodySetLinearVel(bId2, REAL(1.11), REAL(3.33), REAL(2.22));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );


      // Reset for the next set of test.
      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));


      // They move with the same angular speed
      // linear speed == 0

      dBodySetAngularVel(bId1, REAL(1.22), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(1.22), REAL(2.33), REAL(0.0));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(1.22), REAL(2.33), REAL(3.44));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(2.33), REAL(3.44));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );
    }


////////////////////////////////////////////////////////////////////////////////
// Position Body1 [0,  1, 0]
// Position Body2 [0, -1, 0]
// Axis of the prismatic [0, 1, 0]
// Axis of the rotoide   [1, 0, ]0
//
// Only the first body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_3, GetPRPositionRate_Bodies_in_line_B1_moves)
    {
      dBodySetLinearVel(bId1, REAL(3.33), REAL(0.0), REAL(0.0)); // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(3.33), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );


      // Only the first body as angular velocity
      dBodySetAngularVel(bId1, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );
    }

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [0,  1, 0]
// Position Body2 [0, -1, 0]
// Axis of the prismatic [0, 1, 0]
// Axis of the rotoide   [1, 0, ]0
//
// Only the second body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_3, GetPRPositionRate_Bodies_in_line_B2_moves)
    {
      dBodySetLinearVel(bId2, REAL(3.33), REAL(0.0), REAL(0.0)); // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      // The length was at zero and this will give an negative length
      dBodySetLinearVel(bId2, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(-3.33), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId2, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );
    }


////////////////////////////////////////////////////////////////////////////////
// Fixture for testing the PositionRate
//
// The second body is at 90deg w.r.t. the first body
//
//
// Default Position
//                       ^Z
//                       |
//                       |
//
//                     +---+
//                     |   |Body2
//                     |   |
//                     |   |
//                     +---+
//                       |
//                       |
//                       |
//                       |            Body1
//                     R _      -    +-----------+
//                      (_)----|-----|           |  ----->Y
//                              -    +-----------+
//
// N.B. X is comming out of the page
////////////////////////////////////////////////////////////////////////////////
  struct PRGetInfo1_Fixture_4 {
    PRGetInfo1_Fixture_4() {
      wId = dWorldCreate();

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

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

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


      jId = dJointCreatePR(wId, 0);
      joint = (dxJointPR*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPRAnchor (jId, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

    }

    ~PRGetInfo1_Fixture_4() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPR* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };


////////////////////////////////////////////////////////////////////////////////
// Position Body1 [0,  1, 0]
// Position Body2 [0,  0, 1]
// Axis of the prismatic [0, 1, 0]
// Axis of the rotoide   [1, 0, 0]
//
// Only the first body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_4, GetPRPositionRate_Bodies_at90deg_B1_moves)
    {
      dBodySetLinearVel(bId1, REAL(3.33), REAL(0.0), REAL(0.0)); // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      // The length was at zero and this will give an negative length
      dBodySetLinearVel(bId1, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(3.33), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId1, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );
    }

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [0, 1, 0]
// Position Body2 [0, 0, 1]
// Axis of the prismatic [0, 1, 0]
// Axis of the rotoide   [1, 0, 0]
//
// Only the second body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PRGetInfo1_Fixture_4,  GetPRPositionRate_Bodies_at90deg_B2_moves)
    {
      dBodySetLinearVel(bId2, REAL(3.33), REAL(0.0), REAL(0.0)); // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(-3.33), dJointGetPRPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId2, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(-1.0*1.22), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPRPositionRate (jId) );
    }
 
} // End of SUITE(JointPR)





// // //
// Testing the PU Joint
//
// // 
////////////////////////////////////////////////////////////////////////////////
// Default Position:
// Position Body1 (3, 0, 0)
// Position Body2 (1, 0, 0)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
//               Y                ^ Axis2
//              ^                 |
//             /                  |     ^ Axis1
// Z^         /                   |    /
//  |        / Body 2             |   /         Body 1
//  |       /  +---------+        |  /          +-----------+
//  |      /  /         /|        | /          /           /|
//  |     /  /         / +        _/     -    /           / +
//  |    /  /         /-/--------(_)----|--- /-----------/-------> AxisP
//  |   /  +---------+ /                 -  +-----------+ /
//  |  /   |         |/                     |           |/
//  | /    +---------+                      +-----------+
//  |/
//  .-----------------------------------------> X
//             |----------------->
//             Anchor2           <--------------|
//                               Anchor1
//
////////////////////////////////////////////////////////////////////////////////
SUITE(JointPU)
{
  struct PUGetInfo1_Fixture_1 {
    PUGetInfo1_Fixture_1() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePU(wId, 0);
      joint = (dxJointPU*)jId;

      dJointAttach(jId, bId1, bId2);

      dJointSetPUAnchor (jId, 2, 0, 0);
    }

    ~PUGetInfo1_Fixture_1() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPU* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };


////////////////////////////////////////////////////////////////////////////////
// Test when there is no limits.
// The 2 bodies stay aligned.
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_1, test1_SetPUParam)
    {
      dJointSetPUParam(jId, dParamLoStop1, -M_PI);
      dJointSetPUParam(jId, dParamHiStop1 , M_PI);
      dJointSetPUParam(jId, dParamLoStop2, -M_PI);
      dJointSetPUParam(jId, dParamHiStop2,  M_PI);
      dJointSetPUParam(jId, dParamLoStop3, -dInfinity);
      dJointSetPUParam(jId, dParamHiStop3,  dInfinity);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(3, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// The Body 2 is moved -100 unit then at 100
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_1, test1_GetPUParam)
    {
      dJointSetPUParam(jId, dParamLoStop3, -10);
      dJointSetPUParam(jId, dParamHiStop3,  10);

      dBodySetPosition(bId2, REAL(-100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);


      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);


      dBodySetPosition(bId2, REAL(100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(1, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);


      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(3, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for Axis1 and Axis2 at -45deg and 45deg.
// The Body 2 is rotated by 90deg around Axis1
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_1, test2_PUGetInfo1)
    {
      dJointSetPUParam(jId, dParamLoStop1, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop1, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop2, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop3, -10);
      dJointSetPUParam(jId, dParamHiStop3,  10);


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

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(3, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for Axis1 and Axis2 at -45deg and 45deg.
// The Body 2 is rotated by 90deg around Axis1 and
// Body1 is moved at X=100
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_1, test3_PUGetInfo1)
    {
      dJointSetPUParam(jId, dParamLoStop1, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop1, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop2, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop3, -10);
      dJointSetPUParam(jId, dParamHiStop3,  10);


      dBodySetPosition (bId1, REAL(100.0), REAL(0.0), REAL(0.0));

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

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId1, 3, 0, 0);

      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(3, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Default Position:
// Position Body1 (3, 0, 0)
// Position Body2 (1, 0, 0)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
// The motor on axis1 is now powered. (i.e. joint->limot1->fmax > 0
//
//               Y                ^ Axis2
//              ^                 |
//             /                  |     ^ Axis1
// Z^         /                   |    /
//  |        / Body 2             |   /         Body 1
//  |       /  +---------+        |  /          +-----------+
//  |      /  /         /|        | /          /           /|
//  |     /  /         / +        _/     -    /           / +
//  |    /  /         /-/--------(_)----|--- /-----------/-------> AxisP
//  |   /  +---------+ /                 -  +-----------+ /
//  |  /   |         |/                     |           |/
//  | /    +---------+                      +-----------+
//  |/
//  .-----------------------------------------> X
//             |----------------->
//             Anchor2           <--------------|
//                               Anchor1
//
////////////////////////////////////////////////////////////////////////////////
  struct PUGetInfo1_Fixture_2 {
    PUGetInfo1_Fixture_2() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePU(wId, 0);
      joint = (dxJointPU*)jId;

      dJointAttach(jId, bId1, bId2);

      dJointSetPUAnchor (jId, 2, 0, 0);

      joint->limot1.fmax = 1;
    }

    ~PUGetInfo1_Fixture_2() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPU* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };



////////////////////////////////////////////////////////////////////////////////
// Test when there is no limits.
// The 2 bodies stay aligned.
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_2,  test0_PUGetInfo1)
    {
      dJointSetPUParam(jId, dParamLoStop1, -M_PI);
      dJointSetPUParam(jId, dParamHiStop1 , M_PI);
      dJointSetPUParam(jId, dParamLoStop2, -M_PI);
      dJointSetPUParam(jId, dParamHiStop2,  M_PI);
      dJointSetPUParam(jId, dParamLoStop3, -dInfinity);
      dJointSetPUParam(jId, dParamHiStop3,  dInfinity);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// The Body 2 is moved -100 unit then at 100
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_2, test1_PUGetInfo1)
    {
      dJointSetPUParam(jId, dParamLoStop3, -10);
      dJointSetPUParam(jId, dParamHiStop3,  10);

      dBodySetPosition(bId2, REAL(-100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);


      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(5, info.m);


      dBodySetPosition(bId2, REAL(100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(1, joint->limotP.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);


      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for Axis1 and Axis2 at -45deg and 45deg.
// The Body 2 is rotated by 90deg around Axis1
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_2, test2_PUGetInfo1)
    {
      dJointSetPUParam(jId, dParamLoStop1, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop1, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop2, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop3, -10);
      dJointSetPUParam(jId, dParamHiStop3,  10);


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

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for Axis1 and Axis2 at -45deg and 45deg.
// The Body 2 is rotated by 90deg around Axis1 and
// Body1 is moved at X=100
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_2, test3_PUGetInfo1)
    {
      dJointSetPUParam(jId, dParamLoStop1, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop1, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPUParam(jId, dParamHiStop2, M_PI/4.0);
      dJointSetPUParam(jId, dParamLoStop3, -10);
      dJointSetPUParam(jId, dParamHiStop3,  10);


      dBodySetPosition (bId1, REAL(100.0), REAL(0.0), REAL(0.0));

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

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId1, 3, 0, 0);

      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limot1.limit);
      CHECK_EQUAL(0, joint->limot2.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);
    }



  TEST_FIXTURE(PUGetInfo1_Fixture_2, test_SetPUParam)
    {
      dJointSetPUParam(jId, dParamHiStop, REAL(5.0) );
      CHECK_EQUAL(REAL(5.0), joint->limot1.histop);

      dJointSetPUParam(jId, dParamVel, REAL(7.0) );
      CHECK_EQUAL(REAL(7.0), joint->limot1.vel);

#ifdef dParamFudgeFactor1
      dJointSetPUParam(jId, dParamFudgeFactor1, REAL(5.5) );
      CHECK_EQUAL(REAL(5.5), joint->limot1.dParamFudgeFactor);
#endif

      dJointSetPUParam(jId, dParamCFM2, REAL(9.0) );
      CHECK_EQUAL(REAL(9.0), joint->limot2.normal_cfm);

      dJointSetPUParam(jId, dParamStopERP2, REAL(11.0) );
      CHECK_EQUAL(REAL(11.0), joint->limot2.stop_erp);


      dJointSetPUParam(jId, dParamBounce3, REAL(13.0) );
      CHECK_EQUAL(REAL(13.0), joint->limotP.bounce);
    }



  TEST_FIXTURE(PUGetInfo1_Fixture_1, test_GetPUParam)
    {
      joint->limotP.histop = REAL(5.0);
      CHECK_EQUAL(joint->limot1.histop,
                  dJointGetPUParam(jId, dParamHiStop) );

      joint->limotP.vel = REAL(7.0);

      CHECK_EQUAL(joint->limot1.vel,
                  dJointGetPUParam(jId, dParamVel) );

#ifdef dParamFudgeFactor1
      joint->limotP.dParamFudgeFactor =  REAL(5.5);

      CHECK_EQUAL(joint->limot1.dParamFudgeFactor,
                  dJointGetPUParam(jId, dParamFudgeFactor1) );
#endif

      joint->limot2.normal_cfm = REAL(9.0);
      CHECK_EQUAL(joint->limot2.normal_cfm,
                  dJointGetPUParam(jId, dParamCFM2) );

      joint->limot2.stop_erp = REAL(11.0);
      CHECK_EQUAL(joint->limot2.stop_erp,
                  dJointGetPUParam(jId, dParamStopERP2) );

      joint->limotP.bounce = REAL(13.0);
      CHECK_EQUAL(joint->limotP.bounce,
                  dJointGetPUParam(jId, dParamBounce3) );
    }



////////////////////////////////////////////////////////////////////////////////
// Texture for testing the PositionRate
//
// Default Position:
//   Position Body1 (3, 0, 0)
//   Position Body2 (1, 0, 0)
//   Angchor        (2, 0, 0)
//   Axis1          (0, 1, 0)
//   Axis2          (0, 0, 1)
//   AxisP1         (1, 0, 0)
//
// Default velocity:
//   Body 1 lvel=( 0, 0, 0)    avel=( 0, 0, 0)
//   Body 2 lvel=( 0, 0, 0)    avel=( 0, 0, 0)
//
//
//               Y                ^ Axis2
//              ^                 |
//             /                  |     ^ Axis1
// Z^         /                   |    /
//  |        / Body 2             |   /         Body 1
//  |       /  +---------+        |  /          +-----------+
//  |      /  /         /|        | /          /           /|
//  |     /  /         / +        _/     -    /           / +
//  |    /  /         /-/--------(_)----|--- /-----------/-------> AxisP
//  |   /  +---------+ /                 -  +-----------+ /
//  |  /   |         |/                     |           |/
//  | /    +---------+                      +-----------+
//  |/
//  .-----------------------------------------> X
//             |----------------->
//             Anchor2           <--------------|
//                               Anchor1
//
////////////////////////////////////////////////////////////////////////////////
  struct PUGetInfo1_Fixture_3 {
    PUGetInfo1_Fixture_3() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePU(wId, 0);
      joint = (dxJointPU*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPUAnchor (jId, 2, 0, 0);

      dBodySetLinearVel (bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel (bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

    }

    ~PUGetInfo1_Fixture_3() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPU* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [3, 0, 0]
// Position Body2 [1, 0, 0]
// Axis of the prismatic [1, 0, 0]
// Axis1                 [0, 1, 0]
// Axis2                 [0, 0, 1]
//
// Move at the same speed
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_3, test1_GetPUPositionRate)
    {
      // They move with the same linear speed
      // Angular speed == 0
      dBodySetLinearVel(bId1, REAL(0.0), REAL(3.33), REAL(0.0));
      dBodySetLinearVel(bId2, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(1.11), REAL(3.33), REAL(0.0));
      dBodySetLinearVel(bId2, REAL(1.11), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(1.11), REAL(3.33), REAL(2.22));
      dBodySetLinearVel(bId2, REAL(1.11), REAL(3.33), REAL(2.22));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );


      // Reset for the next set of test.
      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));


      // They move with the same angular speed
      // linear speed == 0

      dBodySetAngularVel(bId1, REAL(1.22), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(1.22), REAL(2.33), REAL(0.0));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(1.22), REAL(2.33), REAL(3.44));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(2.33), REAL(3.44));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );
    }


////////////////////////////////////////////////////////////////////////////////
// Position Body1 [3, 0, 0]
// Position Body2 [1, 0, 0]
// Axis of the prismatic [1, 0, 0]
// Axis1                 [0, 1, 0]
// Axis2                 [0, 0, 1]
//
// Only the first body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_3, GetPUPositionRate_Bodies_in_line_B1_moves)
    {
      dBodySetLinearVel(bId1, REAL(3.33), REAL(0.0), REAL(0.0)); // This is impossible but ...
      CHECK_EQUAL(REAL(3.33), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );


      // Only the first body as angular velocity
      dBodySetAngularVel(bId1, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );
    }

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [3, 0, 0]
// Position Body2 [1, 0, 0]
// Axis of the prismatic [1, 0, 0]
// Axis1                 [0, 1, 0]
// Axis2                 [0, 0, 1]
//
// Only the second body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_3, GetPUPositionRate_Bodies_in_line_B2_moves)
    {
      // The length was at zero and this will give an negative length
      dBodySetLinearVel(bId2, REAL(3.33), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(-3.33), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(3.33), REAL(0.0));      // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId2, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );
    }


////////////////////////////////////////////////////////////////////////////////
// Fixture for testing the PositionRate
//
// Default Position:
// Position Body1 (3, 0, 0)
// Position Body2 (0, 0, 1)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (1, 0, 0)
// AxisP          (1, 0, 0)
//
// The second body is at 90deg w.r.t. the first body
//
//
// Default Position
//                       ^Z
//                       |
//                       |
//
//                     +---+
//                     |   |Body2
//                     |   |
//                     |   |
//                     +---+
//                       |      ^ Axis1
//                       |     /
//                       |    /
//                       |   /         Body1
//                     R _      -    +-----------+
//                      (_)----|-----|           |  ----->X  AxisP, Axis2
//                              -    +-----------+
//
// N.B. Y is going into the page
////////////////////////////////////////////////////////////////////////////////
  struct PUGetInfo1_Fixture_4 {
    PUGetInfo1_Fixture_4() {
      wId = dWorldCreate();

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

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

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


      jId = dJointCreatePU(wId, 0);
      joint = (dxJointPU*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPUAnchor (jId, 2, 0, 0);
      dJointSetPUAxis1 (jId, 0, 1, 0);
      dJointSetPUAxis2 (jId, 1, 0, 0);
      dJointSetPUAxisP (jId, 1, 0, 0);


      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

    }

    ~PUGetInfo1_Fixture_4() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPU* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };


////////////////////////////////////////////////////////////////////////////////
// Position Body1 (3, 0, 0)
// Position Body2 (1, 0, 0)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
// Only the first body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_4, GetPUPositionRate_Bodies_at90deg_B1_moves)
    {
      dBodySetLinearVel(bId1, REAL(3.33), REAL(0.0), REAL(0.0)); // This is impossible but ...
      CHECK_EQUAL(REAL(3.33), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(3.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId1, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );
    }

////////////////////////////////////////////////////////////////////////////////
// Position Body1 (3, 0, 0)
// Position Body2 (1, 0, 0)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
// Only the second body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PUGetInfo1_Fixture_4,  GetPUPositionRate_Bodies_at90deg_B2_moves)
    {
      // The length was at zero and this will give an negative length
      dBodySetLinearVel(bId2, REAL(3.33), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(-3.33), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(3.33), REAL(0.0));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId2, REAL(1.22), REAL(0.0), REAL(0.0));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(2.33), REAL(0.0));
      CHECK_EQUAL(REAL(-1.0*2.330), dJointGetPUPositionRate (jId) );

      dBodySetAngularVel(bId2, REAL(0.0), REAL(0.0), REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPUPositionRate (jId) );
    }
 
} // End of SUITE(JointPU)


// =============================================================================
// =============================================================================
//
// Testing the Piston Joint
//
// =============================================================================
// =============================================================================

////////////////////////////////////////////////////////////////////////////////
// Default Position:
// Position Body1 (1, 0, 0)
// Position Body2 (3, 0, 0)
// Angchor        (2, 0, 0)
// AxisR          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
/// <PRE>
///^Z                             |- Anchor point
/// |     Body_1                  |                       Body_2
/// |     +---------------+       V                       +------------------+
/// |    /               /|                             /                  /|
/// |   /               / +       |--      ______      /                  / +
/// .- /      x        /./........x.......(_____()..../         x        /.......> axis
///   +---------------+ /         |--                +------------------+ /        X
///   |               |/                             |                  |/
///   +---------------+                              +------------------+
///          |                                                 |
///          |                                                 |
///          |------------------> <----------------------------|
///              anchor1                  anchor2
///
///
/// Axis Y is going into the page
////////////////////////////////////////////////////////////////////////////////
SUITE(JointPiston)
{
  struct PistonGetInfo1_Fixture_1 {
    PistonGetInfo1_Fixture_1() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePiston(wId, 0);
      joint = (dxJointPiston*)jId;

      dJointAttach(jId, bId1, bId2);

      dJointSetPistonAnchor (jId, 2, 0, 0);
    }

    ~PistonGetInfo1_Fixture_1() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPiston* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };


////////////////////////////////////////////////////////////////////////////////
// Test when there is no limits.
// The 2 bodies stay aligned.
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_1, test1_SetPistonParam)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -dInfinity);
      dJointSetPistonParam(jId, dParamHiStop1,  dInfinity);
      dJointSetPistonParam(jId, dParamLoStop2, -M_PI);
      dJointSetPistonParam(jId, dParamHiStop2 , M_PI);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(4, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// The Body 2 is moved -100 unit then at 100
//
// Default value for axisR1 = 1,0,0
// Default value for axisP1 = 0,1,0
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_1, test1_GetPistonParam)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -10);
      dJointSetPistonParam(jId, dParamHiStop1,  10);

      dBodySetPosition(bId2, REAL(-100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);

      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);


      dBodySetPosition(bId2, REAL(100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(1, joint->limotP.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 1, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);


      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and the rotoide at -45deg and 45deg.
// The Body 2 is rotated by 90deg around the axis
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_1, test2_PistonGetInfo1)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -10);
      dJointSetPistonParam(jId, dParamHiStop1,  10);
      dJointSetPistonParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPistonParam(jId, dParamHiStop2, M_PI/4.0);

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

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limotR.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(4, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for rotoide at -45deg and 45deg.
// The Body 2 is rotated by 90deg around the axis
// Body1 is moved at X=100
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_1, test3_PistonGetInfo1)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -10);
      dJointSetPistonParam(jId, dParamHiStop1,  10);
      dJointSetPistonParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPistonParam(jId, dParamHiStop2, M_PI/4.0);


      dBodySetPosition (bId1, REAL(100.0), REAL(0.0), REAL(0.0));

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

      joint->getInfo1(&info);

      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(1, joint->limotR.limit);

      CHECK_EQUAL(6, info.m);

      // Reset Position and test
      dBodySetPosition(bId1, 1, 0, 0);

      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(4, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Default Position:
// Position Body1 (1, 0, 0)
// Position Body2 (3, 0, 0)
// Angchor        (2, 0, 0)
// AxisR          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
// The motor on axis1 is now powered. (i.e. joint->limot1->fmax > 0
//
/// <PRE>
///^Z                             |- Anchor point
/// |     Body_1                  |                       Body_2
/// |     +---------------+       V                       +------------------+
/// |    /               /|                             /                  /|
/// |   /               / +       |--      ______      /                  / +
/// .- /      x        /./........x.......(_____()..../         x        /.......> axis
///   +---------------+ /         |--                +------------------+ /        X
///   |               |/                             |                  |/
///   +---------------+                              +------------------+
///          |                                                 |
///          |                                                 |
///          |------------------> <----------------------------|
///              anchor1                  anchor2
///
///
/// Axis Y is going into the page
////////////////////////////////////////////////////////////////////////////////
  struct PistonGetInfo1_Fixture_2 {
    PistonGetInfo1_Fixture_2() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePiston(wId, 0);
      joint = (dxJointPiston*)jId;

      dJointAttach(jId, bId1, bId2);

      dJointSetPistonAnchor (jId, 2, 0, 0);

      joint->limotP.fmax = 1;
    }

    ~PistonGetInfo1_Fixture_2() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPiston* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };



////////////////////////////////////////////////////////////////////////////////
// Test when there is no limits.
// The 2 bodies stay aligned.
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_2,  test0_PistonGetInfo1)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -dInfinity);
      dJointSetPistonParam(jId, dParamHiStop1,  dInfinity);
      dJointSetPistonParam(jId, dParamLoStop2, -M_PI);
      dJointSetPistonParam(jId, dParamHiStop2,  M_PI);


      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }


////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// The Body 2 is moved -100 unit then at 100
//
// Default value for axis = 1,0,0
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_2, test1_PistonGetInfo1)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -10);
      dJointSetPistonParam(jId, dParamHiStop1,  10);

      dBodySetPosition(bId2, REAL(-100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);

      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);


      dBodySetPosition(bId2, REAL(100.0), REAL(0.0), REAL(0.0));

      joint->getInfo1(&info);

      CHECK_EQUAL(1, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);

      // Reset Position and test
      dBodySetPosition(bId2, 3, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for the rotoide at -45deg and 45deg.
// The Body 2 is rotated by 90deg around the axis
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_2, test2_PistonGetInfo1)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -10);
      dJointSetPistonParam(jId, dParamHiStop1,  10);
      dJointSetPistonParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPistonParam(jId, dParamHiStop2, M_PI/4.0);

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

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(1, joint->limotR.limit);
      CHECK_EQUAL(6, info.m);

      // Reset Position and test
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }

////////////////////////////////////////////////////////////////////////////////
// Test when there is limits for the prismatic at -10 and 10
// and for the rotoide axuis at -45deg and 45deg.
// The Body 2 is rotated by 90deg around the axis and
// Body1 is moved at X=100
//
//
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_2, test3_PistonGetInfo1)
    {
      dJointSetPistonParam(jId, dParamLoStop1, -10);
      dJointSetPistonParam(jId, dParamHiStop1,  10);
      dJointSetPistonParam(jId, dParamLoStop2, -M_PI/4.0);
      dJointSetPistonParam(jId, dParamHiStop2, M_PI/4.0);



      dBodySetPosition (bId1, REAL(100.0), REAL(0.0), REAL(0.0));

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

      joint->getInfo1(&info);

      CHECK_EQUAL(2, joint->limotP.limit);
      CHECK_EQUAL(1, joint->limotR.limit);
      CHECK_EQUAL(6, info.m);

      // Reset Position and test
      dBodySetPosition(bId1, 1, 0, 0);

      dBodySetPosition(bId2, 3, 0, 0);
      dMatrix3 R_final = { 1,0,0,0,
                           0,1,0,0,
                           0,0,1,0 };
      dBodySetRotation (bId2, R_final);

      joint->getInfo1(&info);

      CHECK_EQUAL(0, joint->limotP.limit);
      CHECK_EQUAL(0, joint->limotR.limit);
      CHECK_EQUAL(5, info.m);
    }



  TEST_FIXTURE(PistonGetInfo1_Fixture_2, test_SetPistonParam)
    {
      dJointSetPistonParam(jId, dParamHiStop, REAL(5.0) );
      CHECK_EQUAL(REAL(5.0), joint->limotP.histop);

      dJointSetPistonParam(jId, dParamVel, REAL(7.0) );
      CHECK_EQUAL(REAL(7.0), joint->limotP.vel);

#ifdef dParamFudgeFactor1
      dJointSetPistonParam(jId, dParamFudgeFactor1, REAL(5.5) );
      CHECK_EQUAL(REAL(5.5), joint->limotP.dParamFudgeFactor);
#endif

      dJointSetPistonParam(jId, dParamCFM2, REAL(9.0) );
      CHECK_EQUAL(REAL(9.0), joint->limotR.normal_cfm);

      dJointSetPistonParam(jId, dParamStopERP2, REAL(11.0) );
      CHECK_EQUAL(REAL(11.0), joint->limotR.stop_erp);
    }



  TEST_FIXTURE(PistonGetInfo1_Fixture_1, test_GetPistonParam)
    {
      joint->limotP.histop = REAL(5.0);
      CHECK_EQUAL(joint->limotP.histop,
                  dJointGetPistonParam(jId, dParamHiStop) );

      joint->limotP.vel = REAL(7.0);

      CHECK_EQUAL(joint->limotP.vel,
                  dJointGetPistonParam(jId, dParamVel) );

#ifdef dParamFudgeFactor1
      joint->limotP.dParamFudgeFactor =  REAL(5.5);

      CHECK_EQUAL(joint->limotP.dParamFudgeFactor,
                  dJointGetPistonParam(jId, dParamFudgeFactor1) );
#endif

      joint->limotR.normal_cfm = REAL(9.0);
      CHECK_EQUAL(joint->limotR.normal_cfm,
                  dJointGetPistonParam(jId, dParamCFM2) );

      joint->limotR.stop_erp = REAL(11.0);
      CHECK_EQUAL(joint->limotR.stop_erp,
                  dJointGetPistonParam(jId, dParamStopERP2) );
    }



////////////////////////////////////////////////////////////////////////////////
// Texture for testing the PositionRate
//
// Default Position:
//   Position Body1 (3, 0, 0)
//   Position Body2 (1, 0, 0)
//   Angchor        (2, 0, 0)
//   Axis1          (0, 1, 0)
//   Axis2          (0, 0, 1)
//   AxisP1         (1, 0, 0)
//
// Default velocity:
//   Body 1 lvel=( 0, 0, 0)    avel=( 0, 0, 0)
//   Body 2 lvel=( 0, 0, 0)    avel=( 0, 0, 0)
//
//
//               Y                ^ Axis2
//              ^                 |
//             /                  |     ^ Axis1
// Z^         /                   |    /
//  |        / Body 2             |   /         Body 1
//  |       /  +---------+        |  /          +-----------+
//  |      /  /         /|        | /          /           /|
//  |     /  /         / +        _/     -    /           / +
//  |    /  /         /-/--------(_)----|--- /-----------/-------> AxisP
//  |   /  +---------+ /                 -  +-----------+ /
//  |  /   |         |/                     |           |/
//  | /    +---------+                      +-----------+
//  |/
//  .-----------------------------------------> X
//             |----------------->
//             Anchor2           <--------------|
//                               Anchor1
//
////////////////////////////////////////////////////////////////////////////////
  struct PistonGetInfo1_Fixture_3 {
    PistonGetInfo1_Fixture_3() {
      wId = dWorldCreate();

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

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


      jId = dJointCreatePiston(wId, 0);
      joint = (dxJointPiston*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPistonAnchor (jId, 2, 0, 0);

      dBodySetLinearVel (bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel (bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

    }

    ~PistonGetInfo1_Fixture_3() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPiston* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [3, 0, 0]
// Position Body2 [1, 0, 0]
// Axis of the prismatic [1, 0, 0]
// Axis1                 [0, 1, 0]
// Axis2                 [0, 0, 1]
//
// Move at the same speed
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_3, test1_GetPistonPositionRate)
    {
      // They move with the same linear speed
      // Angular speed == 0
      dBodySetLinearVel(bId1, 0, REAL(3.33), 0);
      dBodySetLinearVel(bId2, 0, REAL(3.33), 0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(1.11), REAL(3.33), 0);
      dBodySetLinearVel(bId2, REAL(1.11), REAL(3.33), 0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId1, REAL(1.11), REAL(3.33), REAL(2.22));
      dBodySetLinearVel(bId2, REAL(1.11), REAL(3.33), REAL(2.22));
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );


      // Reset for the next set of test.
      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));


      // They move with the same angular speed
      // linear speed == 0

      dBodySetAngularVel(bId1, REAL(1.22), 0.0, 0.0);
      dBodySetAngularVel(bId2, REAL(1.22), 0.0, 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(1.22), REAL(2.33), 0.0);
      dBodySetAngularVel(bId2, REAL(1.22), REAL(2.33), 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId1, REAL(1.22), REAL(2.33), REAL(3.44));
      dBodySetAngularVel(bId2, REAL(1.22), REAL(2.33), REAL(3.44));
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );
    }


////////////////////////////////////////////////////////////////////////////////
// Position Body1 [3, 0, 0]
// Position Body2 [1, 0, 0]
// Axis of the prismatic [1, 0, 0]
// Axis1                 [0, 1, 0]
// Axis2                 [0, 0, 1]
//
// Only the first body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_3, GetPistonPositionRate_Bodies_in_line_B1_moves)
    {
      dBodySetLinearVel(bId1, REAL(3.33), 0.0, 0.0); // This is impossible but ...
      CHECK_EQUAL(REAL(3.33), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId1, 0, REAL(3.33), 0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId1, 0, 0, REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );


      // Only the first body as angular velocity
      dBodySetAngularVel(bId1, REAL(1.22), 0.0, 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId1, 0.0, REAL(2.33), 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId1, 0.0, 0.0, REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );
    }

////////////////////////////////////////////////////////////////////////////////
// Position Body1 [3, 0, 0]
// Position Body2 [1, 0, 0]
// Axis of the prismatic [1, 0, 0]
// Axis1                 [0, 1, 0]
// Axis2                 [0, 0, 1]
//
// Only the second body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_3, GetPistonPositionRate_Bodies_in_line_B2_moves)
    {
      // The length was at zero and this will give an negative length
      dBodySetLinearVel(bId2, REAL(3.33), 0.0, 0.0);
      CHECK_EQUAL(REAL(-3.33), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId2, 0, REAL(3.33), 0);      // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId2, 0, 0, REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId2, REAL(1.22), 0.0, 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId2, 0.0, REAL(2.33), 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId2, 0.0, 0.0, REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );
    }


////////////////////////////////////////////////////////////////////////////////
// Fixture for testing the PositionRate
//
// Default Position:
// Position Body1 (3, 0, 0)
// Position Body2 (0, 0, 1)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (1, 0, 0)
// AxisP          (1, 0, 0)
//
// The second body is at 90deg w.r.t. the first body
// From
//
//               Y                ^ Axis2
//              ^                 |
//             /                  |     ^ Axis1
// Z^         /                   |    /
//  |        / Body 2             |   /         Body 1
//  |       /  +---------+        |  /          +-----------+
//  |      /  /         /|        | /          /           /|
//  |     /  /         / +        _/     -    /           / +
//  |    /  /         /-/--------(_)----|--- /-----------/-------> AxisP
//  |   /  +---------+ /                 -  +-----------+ /
//  |  /   |         |/                     |           |/
//  | /    +---------+                      +-----------+
//  |/
//  .-----------------------------------------> X
//             |----------------->
//             Anchor2           <--------------|
//                               Anchor1
// To
//
//               Y                ^ Axis2
//              ^                 |
//             /  Body 2          |     ^ Axis1
// Z^          +----------+       |    /
//  |        //          /|       |   /         Body 1
//  |       /+----------+ |       |  /          +-----------+
//  |      / |          | |       | /          /           /|
//  |     /  |          | |       _/     -    /           / +
//  |    /   |          |-|------(_)----|--- /-----------/-------> AxisP
//  |   /    |          | |              -  +-----------+ /
//  |  /     |          | |                 |           |/
//  | /      |          | +                 +-----------+
//  |/       |          |/
//  .--------+----------+--------------------> X
//             |---------------->
//             Anchor2           <--------------|
//                               Anchor1
// Default Position
//
// N.B. Y is going into the page
////////////////////////////////////////////////////////////////////////////////
  struct PistonGetInfo1_Fixture_4 {
    PistonGetInfo1_Fixture_4() {
      wId = dWorldCreate();

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

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

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


      jId = dJointCreatePiston(wId, 0);
      joint = (dxJointPiston*)jId;

      dJointAttach(jId, bId1, bId2);
      dJointSetPistonAnchor (jId, 2, 0, 0);


      dBodySetLinearVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

      dBodySetLinearVel(bId2, REAL(0.0), REAL(0.0), REAL(0.0));
      dBodySetAngularVel(bId1, REAL(0.0), REAL(0.0), REAL(0.0));

    }

    ~PistonGetInfo1_Fixture_4() {
      dWorldDestroy(wId);
    }

    dJointID jId;
    dxJointPiston* joint;

    dWorldID wId;

    dBodyID bId1;
    dBodyID bId2;

    dxJoint::Info1 info;
  };


////////////////////////////////////////////////////////////////////////////////
// Position Body1 (3, 0, 0)
// Position Body2 (1, 0, 0)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
// Only the first body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_4, GetPistonPositionRate_Bodies_at90deg_B1_moves)
    {
      dBodySetLinearVel(bId1, REAL(3.33), 0.0, 0.0); // This is impossible but ...
      CHECK_EQUAL(REAL(3.33), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId1, 0, REAL(3.33), 0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId1, 0, 0, REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId1, REAL(1.22), 0.0, 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId1, 0.0, REAL(2.33), 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId1, 0.0, 0.0, REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );
    }

////////////////////////////////////////////////////////////////////////////////
// Position Body1 (3, 0, 0)
// Position Body2 (1, 0, 0)
// Angchor        (2, 0, 0)
// Axis1          (0, 1, 0)
// Axis2          (0, 0, 1)
// AxisP1         (1, 0, 0)
//
// Only the second body moves
////////////////////////////////////////////////////////////////////////////////
  TEST_FIXTURE(PistonGetInfo1_Fixture_4,  GetPistonPositionRate_Bodies_at90deg_B2_moves)
    {
      // The length was at zero and this will give an negative length
      dBodySetLinearVel(bId2, REAL(3.33), 0.0, 0.0);
      CHECK_EQUAL(REAL(-3.33), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId2, 0, REAL(3.33), 0);     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetLinearVel(bId2, 0, 0, REAL(3.33));     // This is impossible but ...
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );


      // Only angular velocity
      dBodySetAngularVel(bId2, REAL(1.22), 0.0, 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId2, 0.0, REAL(2.33), 0.0);
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );

      dBodySetAngularVel(bId2, 0.0, 0.0, REAL(5.55));
      CHECK_EQUAL(REAL(0.0), dJointGetPistonPositionRate (jId) );
    }
 
} // End of SUITE(JointPiston)