summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/tests/joint.cpp
diff options
context:
space:
mode:
authorsanine <sanine.not@pm.me>2022-10-01 20:59:36 -0500
committersanine <sanine.not@pm.me>2022-10-01 20:59:36 -0500
commitc5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch)
tree277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/tests/joint.cpp
parent1cf9cc3408af7008451f9133fb95af66a9697d15 (diff)
add ode
Diffstat (limited to 'libs/ode-0.16.1/tests/joint.cpp')
-rw-r--r--libs/ode-0.16.1/tests/joint.cpp3054
1 files changed, 3054 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/tests/joint.cpp b/libs/ode-0.16.1/tests/joint.cpp
new file mode 100644
index 0000000..465fb1b
--- /dev/null
+++ b/libs/ode-0.16.1/tests/joint.cpp
@@ -0,0 +1,3054 @@
+/*************************************************************************
+ * *
+ * 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/joint.cpp
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+#include <UnitTest++.h>
+#include <ode/ode.h>
+#include "../ode/src/config.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(1, 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);
+ }
+
+
+// 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(1, joint->limotR.limit);
+ CHECK_EQUAL(6, 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) );
+ }
+
+
+
+ struct Fixture_Simple_Hinge
+ {
+ Fixture_Simple_Hinge ()
+ {
+ wId = dWorldCreate();
+
+ bId1 = dBodyCreate(wId);
+ dBodySetPosition(bId1, 0, -1, 0);
+
+ bId2 = dBodyCreate(wId);
+ dBodySetPosition(bId2, 0, 1, 0);
+
+
+ jId = dJointCreateHinge(wId, 0);
+
+ dJointAttach(jId, bId1, bId2);
+ }
+
+ ~Fixture_Simple_Hinge()
+ {
+ dWorldDestroy(wId);
+ }
+
+ dJointID jId;
+
+ dWorldID wId;
+
+ dBodyID bId1;
+ dBodyID bId2;
+ };
+
+ // Test that it is possible to have joint without a body
+ TEST_FIXTURE(Fixture_Simple_Hinge, test_dJointAttach)
+ {
+ bool only_body1_OK = true;
+ try {
+ dJointAttach(jId, bId1, 0);
+ dWorldStep (wId, 1);
+ }
+ catch (...) {
+ only_body1_OK = false;
+ }
+ CHECK_EQUAL(true, only_body1_OK);
+
+ bool only_body2_OK = true;
+ try {
+ dJointAttach(jId, 0, bId2);
+ dWorldStep (wId, 1);
+ }
+ catch (...) {
+ only_body2_OK = false;
+ }
+ CHECK_EQUAL(true, only_body2_OK);
+
+ bool no_body_OK = true;
+ try {
+ dJointAttach(jId, 0, 0);
+ dWorldStep (wId, 1);
+ }
+ catch (...) {
+ no_body_OK = false;
+ }
+ CHECK_EQUAL(true, no_body_OK);
+ }
+
+
+
+} // End of SUITE(JointPiston)
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 12:37:32 +0000