add ode

1 files changed, 928 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/tests/joints/hinge.cpp b/libs/ode-0.16.1/tests/joints/hinge.cpp
new file mode 100644
index 0000000..f715a6e
--- /dev/null
+++ b/libs/ode-0.16.1/tests/joints/hinge.cpp
@@ -0,0 +1,928 @@
+/*************************************************************************
+  *                                                                       *
+  * 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.                     *
+  *                                                                       *
+  *************************************************************************/
+//234567890123456789012345678901234567890123456789012345678901234567890123456789
+//        1         2         3         4         5         6         7
+
+////////////////////////////////////////////////////////////////////////////////
+// This file create unit test for some of the functions found in:
+// ode/src/joinst/hinge.cpp
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <UnitTest++.h>
+#include <ode/ode.h>
+
+#include "../../ode/src/config.h"
+#include "../../ode/src/joints/hinge.h"
+
+SUITE (TestdxJointHinge)
+{
+  // The 2 bodies are positionned at (0, 0, 0), with no rotation
+  // The joint is an Hinge Joint
+  // Axis is along the X axis
+  // Anchor at (0, 0, 0)
+  //         ^Y
+  //         |
+  //         |
+  //         |
+  //         |
+  //         |
+  // Z <---- . (X going out of the page)
+  struct dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X {
+    dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X()
+    {
+      wId = dWorldCreate();
+
+      bId1 = dBodyCreate (wId);
+      dBodySetPosition (bId1, 0, 0, 0);
+
+      bId2 = dBodyCreate (wId);
+      dBodySetPosition (bId2, 0, 0, 0);
+
+      jId   = dJointCreateHinge (wId, 0);
+      joint = (dxJointHinge*) jId;
+
+
+      dJointAttach (jId, bId1, bId2);
+      dJointSetHingeAnchor (jId, 0, 0, 0);
+
+      axis[0] = 1;
+      axis[1] = 0;
+      axis[2] = 0;
+    }
+
+    ~dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X()
+    {
+      dWorldDestroy (wId);
+    }
+
+    dWorldID wId;
+
+    dBodyID bId1;
+    dBodyID bId2;
+
+
+    dJointID jId;
+    dxJointHinge* joint;
+
+    dVector3 axis;
+  };
+
+  // Rotate 2nd body 90deg around X then back to original position
+  //
+  //   ^  ^       ^
+  //   |  |  =>   |  <---
+  //   |  |       |
+  //  B1  B2     B1   B2
+  //
+  // Start with a Delta of 90deg
+  //   ^           ^   ^
+  //   | <---  =>  |   |
+  //   |           |   |
+  //  B1  B2      B1   B2
+  TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_B2_90deg) {
+    dMatrix3 R;
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -M_PI/2.0);
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+  // Rotate 2nd body -90deg around X then back to original position
+  //
+  //   ^  ^       ^
+  //   |  |  =>   |  --->
+  //   |  |       |
+  //  B1  B2     B1   B2
+  //
+  // Start with a Delta of 90deg
+  //   ^           ^   ^
+  //   | --->  =>  |   |
+  //   |           |   |
+  //  B1  B2      B1   B2
+  TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_B2_Minus90deg) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  M_PI/2.0);
+    CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 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 (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_B1_0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, REAL(0.23) );
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  REAL(0.23));
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+  // Rotate 1st 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 (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_B1_Minus0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -REAL(0.23));
+    CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+  // The 2 bodies are positionned at (0, 0, 0), with no rotation
+  // The joint is an Hinge 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 dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X {
+    dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X()
+    {
+      wId = dWorldCreate();
+
+      bId1 = dBodyCreate (wId);
+      dBodySetPosition (bId1, 0, -1, 0);
+
+      bId2 = dBodyCreate (wId);
+      dBodySetPosition (bId2, 0, 1, 0);
+
+      jId   = dJointCreateHinge (wId, 0);
+      joint = (dxJointHinge*) jId;
+
+
+      dJointAttach (jId, bId1, bId2);
+      dJointSetHingeAnchor (jId, 0, 0, 0);
+
+      axis[0] = -1;
+      axis[1] = 0;
+      axis[2] = 0;
+    }
+
+    ~dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X()
+    {
+      dWorldDestroy (wId);
+    }
+
+    dWorldID wId;
+
+    dBodyID bId1;
+    dBodyID bId2;
+
+
+    dJointID jId;
+    dxJointHinge* joint;
+
+    dVector3 axis;
+  };
+
+  // Rotate 2nd body 90deg around X then back to original position
+  //
+  //   ^  ^       ^
+  //   |  |  =>   |  <---
+  //   |  |       |
+  //  B1  B2     B1   B2
+  //
+  // Start with a Delta of 90deg
+  //   ^           ^   ^
+  //   | <---  =>  |   |
+  //   |           |   |
+  //  B1  B2      B1   B2
+  TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X,
+                test_dJointSetHingeAxisOffset_B2_90Deg) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  M_PI/2.0);
+    CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+  // Rotate 2nd body -90deg around X then back to original position
+  //
+  //   ^  ^       ^
+  //   |  |  =>   |  --->
+  //   |  |       |
+  //  B1  B2     B1   B2
+  //
+  // Start with a Delta of 90deg
+  //   ^           ^   ^
+  //   | --->  =>  |   |
+  //   |           |   |
+  //  B1  B2      B1   B2
+  TEST_FIXTURE (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X,
+                test_dJointSetHingeAxisOffset_B2_Minus90Deg) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -M_PI/2.0);
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 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 (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X,
+                test_dJointSetHingeAxisOffset_B1_0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, REAL(0.23));
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -REAL(0.23));
+    CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 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 (dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Inverse_of_X,
+                test_dJointSetHingeAxisOffset_B1_Minus0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  REAL(0.23));
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0.0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+  // Only one body body1 at (0,0,0)
+  // The joint is an Hinge Joint.
+  // Axis is along the X axis
+  // Anchor at (0, 0, 0)
+  //
+  //       ^Y
+  //       |
+  //       |
+  //       |
+  //       |
+  //       |
+  // Z <-- X
+  struct dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X {
+    dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X()
+    {
+      wId = dWorldCreate();
+
+      bId1 = dBodyCreate (wId);
+      dBodySetPosition (bId1, 0, 0, 0);
+
+      jId   = dJointCreateHinge (wId, 0);
+      joint = (dxJointHinge*) jId;
+
+
+      dJointAttach (jId, bId1, NULL);
+      dJointSetHingeAnchor (jId, 0, 0, 0);
+
+      axis[0] = 1;
+      axis[1] = 0;
+      axis[2] = 0;
+    }
+
+    ~dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X()
+    {
+      dWorldDestroy (wId);
+    }
+
+    dWorldID wId;
+
+    dBodyID bId1;
+
+
+    dJointID jId;
+    dxJointHinge* 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 (dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_1Body_B1_90Deg) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  M_PI/2.0);
+    CHECK_CLOSE (M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0, dJointGetHingeAngle (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 (dxJointHinge_Fixture_B1_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_1Body_B1_Minus0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -REAL(0.23));
+    CHECK_CLOSE (-REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+
+  // Only one body body1 at (0,0,0)
+  // The joint is an Hinge Joint.
+  // Axis the inverse of the X axis
+  // Anchor at (0, 0, 0)
+  //
+  //       ^Y
+  //       |
+  //       |
+  //       |
+  //       |
+  //       |
+  // Z <-- X
+  struct dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X {
+    dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X()
+    {
+      wId = dWorldCreate();
+
+      bId1 = dBodyCreate (wId);
+      dBodySetPosition (bId1, 0, 0, 0);
+
+      jId   = dJointCreateHinge (wId, 0);
+      joint = (dxJointHinge*) jId;
+
+
+      dJointAttach (jId, bId1, NULL);
+      dJointSetHingeAnchor (jId, 0, 0, 0);
+
+      axis[0] = -1;
+      axis[1] = 0;
+      axis[2] = 0;
+    }
+
+    ~dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X()
+    {
+      dWorldDestroy (wId);
+    }
+
+    dWorldID wId;
+
+    dBodyID bId1;
+
+
+    dJointID jId;
+    dxJointHinge* 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 (dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X,
+                test_dJointSetHingeAxisOffset_1Body_B1_90Deg) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -M_PI/2.0);
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0, dJointGetHingeAngle (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 (dxJointHinge_Fixture_B1_At_Zero_Axis_Inverse_of_X,
+                test_dJointSetHingeAxisOffset_1Body_B1_Minus0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  REAL(0.23));
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId1, R);
+
+    CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+
+
+
+  // Only one body body2 at (0,0,0)
+  // The joint is an Hinge Joint.
+  // Axis is along the X axis
+  // Anchor at (0, 0, 0)
+  //
+  //       ^Y
+  //       |
+  //       |
+  //       |
+  //       |
+  //       |
+  // Z <-- X
+  struct dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X {
+    dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X()
+    {
+      wId = dWorldCreate();
+
+      bId2 = dBodyCreate (wId);
+      dBodySetPosition (bId2, 0, 0, 0);
+
+      jId   = dJointCreateHinge (wId, 0);
+      joint = (dxJointHinge*) jId;
+
+
+      dJointAttach (jId, NULL, bId2);
+      dJointSetHingeAnchor (jId, 0, 0, 0);
+
+      axis[0] = 1;
+      axis[1] = 0;
+      axis[2] = 0;
+    }
+
+    ~dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X()
+    {
+      dWorldDestroy (wId);
+    }
+
+    dWorldID wId;
+
+    dBodyID bId2;
+
+
+    dJointID jId;
+    dxJointHinge* joint;
+
+    dVector3 axis;
+  };
+
+  // Rotate B2 by 90deg around X then back to original position
+  //
+  //   ^
+  //   |  => <---
+  //   |
+  //  B2      B2
+  //
+  // Start with a Delta of 90deg
+  //            ^
+  //  <---  =>  |
+  //            |
+  //   B2      B2
+  TEST_FIXTURE (dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_1Body_B2_90Deg) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  -M_PI/2.0);
+    CHECK_CLOSE (-M_PI/2.0, dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+  // Rotate B2 by -0.23rad around X then back to original position
+  //
+  //   ^         ^
+  //   |  =>    /
+  //   |       /
+  //  B2      B2
+  //
+  // Start with a Delta of -0.23rad
+  //     ^     ^
+  //    /  =>  |
+  //   /       |
+  //   B2     B2
+  TEST_FIXTURE (dxJointHinge_Fixture_B2_At_Zero_Axis_Along_X,
+                test_dJointSetHingeAxisOffset_1Body_B2_Minus0_23rad) {
+    dMatrix3 R;
+
+    dJointSetHingeAxis (jId, axis[0], axis[1], axis[2]);
+
+    CHECK_CLOSE (dJointGetHingeAngle (jId), 0.0, 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, -REAL(0.23));
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dJointSetHingeAxisOffset (jId, axis[0], axis[1], axis[2],  REAL(0.23));
+    CHECK_CLOSE (REAL(0.23), dJointGetHingeAngle (jId), 1e-4);
+
+    dRFromAxisAndAngle (R, 1, 0, 0, 0);
+    dBodySetRotation (bId2, R);
+
+    CHECK_CLOSE (0, dJointGetHingeAngle (jId), 1e-4);
+  }
+
+
+
+  // Create 2 bodies attached by a Hinge joint
+  // Axis is along the X axis (Default value
+  // Anchor at (0, 0, 0)      (Default value)
+  //
+  //       ^Y
+  //       |
+  //       * Body2
+  //       |
+  //       |
+  // Body1 |
+  // *     Z-------->
+  struct dxJointHinge_Test_Initialization {
+    dxJointHinge_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]   = dJointCreateHinge (wId, 0);
+        dJointAttach (jId[j], bId[j][0], bId[j][1]);
+        //        dJointSetHingeParam(jId[j], dParamLoStop, 1);
+        //        dJointSetHingeParam(jId[j], dParamHiStop, 2);
+        //        dJointSetHingeParam(jId[j], dParamFMax, 200);
+      }
+    }
+
+    ~dxJointHinge_Test_Initialization()
+    {
+      dWorldDestroy (wId);
+    }
+
+    dWorldID wId;
+
+    dBodyID bId[2][2];
+
+
+    dJointID jId[2];
+
+  };
+
+
+  // Test if setting a Hinge with its default values
+  // will behave the same as a default Hinge joint
+  TEST_FIXTURE (dxJointHinge_Test_Initialization,
+                test_Hinge_Initialization) {
+    using namespace std;
+
+    dVector3 axis;
+    dJointGetHingeAxis(jId[1], axis);
+    dJointSetHingeAxis(jId[1], axis[0], axis[1], axis[2]);
+
+
+    dVector3 anchor;
+    dJointGetHingeAnchor(jId[1], anchor);
+    dJointSetHingeAnchor(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-6);
+      CHECK_CLOSE (qA[1], qB[1], 1e-6);
+      CHECK_CLOSE (qA[2], qB[2], 1e-6);
+      CHECK_CLOSE (qA[3], qB[3], 1e-6);
+    }
+
+    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-6);
+      CHECK_CLOSE (qA[1], qB[1], 1e-6);
+      CHECK_CLOSE (qA[2], qB[2], 1e-6);
+      CHECK_CLOSE (qA[3], qB[3], 1e-6);
+
+
+      const dReal *posA = dBodyGetPosition(bId[0][b]);
+      const dReal *posB = dBodyGetPosition(bId[1][b]);
+      CHECK_CLOSE (posA[0], posB[0], 1e-6);
+      CHECK_CLOSE (posA[1], posB[1], 1e-6);
+      CHECK_CLOSE (posA[2], posB[2], 1e-6);
+      CHECK_CLOSE (posA[3], posB[3], 1e-6);
+    }
+  }
+
+               
+  TEST_FIXTURE(dxJointHinge_Fixture_B1_and_B2_At_Zero_Axis_Along_X,
+               test_Hinge_dParamVel)
+      {
+          const dReal targetvel = 100;
+          const dReal tolerance = targetvel *
+#ifdef dSINGLE
+              1e-2
+#else
+              1e-6
+#endif
+              ;
+
+          dJointSetHingeParam(jId, dParamFMax, dInfinity);
+          dJointSetHingeParam(jId, dParamVel, targetvel);
+          
+          dWorldStep(wId, 0.001);
+          
+          const dReal *v1 = dBodyGetAngularVel(bId1);
+          const dReal *v2 = dBodyGetAngularVel(bId2);
+          dVector3 rvel = { v1[0]-v2[0], v1[1]-v2[1], v1[2]-v2[2] };
+          CHECK_CLOSE(rvel[0], targetvel, tolerance);
+          CHECK_CLOSE(rvel[1], 0, tolerance);
+          CHECK_CLOSE(rvel[2], 0, tolerance);
+      }
+  
+  
+
+} // End of SUITE TestdxJointHinge
+
+