diff options
Diffstat (limited to 'libs/ode-0.16.1/tests/joint.cpp')
| -rw-r--r-- | libs/ode-0.16.1/tests/joint.cpp | 3054 |
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) |