/************************************************************************* * * * 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 creates unit tests for some of the functions found in: // ode/src/joinst/slider.cpp // // //////////////////////////////////////////////////////////////////////////////// #include #include #include "../../ode/src/config.h" #include "../../ode/src/joints/slider.h" SUITE (TestdxJointSlider) { struct dxJointSlider_Fixture_1 { dxJointSlider_Fixture_1() { wId = dWorldCreate(); bId1 = dBodyCreate (wId); dBodySetPosition (bId1, 0, -1, 0); bId2 = dBodyCreate (wId); dBodySetPosition (bId2, 0, 1, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, bId1, bId2); } ~dxJointSlider_Fixture_1() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId1; dBodyID bId2; dJointID jId; dxJointSlider* joint; }; TEST_FIXTURE (dxJointSlider_Fixture_1, test_dJointSetSlider) { // the 2 bodies are align dJointSetSliderAxis (jId, 1, 0, 0); CHECK_CLOSE (joint->qrel[0], 1.0, 1e-4); CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); dMatrix3 R; // Rotate 2nd body 90deg around X dBodySetPosition (bId2, 0, 0, 1); dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0); dBodySetRotation (bId2, R); dJointSetSliderAxis (jId, 1, 0 ,0); CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); CHECK_CLOSE (joint->qrel[1], 0.70710678118654757, 1e-4); CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); // Rotate 2nd body -90deg around X dBodySetPosition (bId2, 0, 0, -1); dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0); dBodySetRotation (bId2, R); dJointSetSliderAxis (jId, 1, 0 ,0); CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); CHECK_CLOSE (joint->qrel[1], -0.70710678118654757, 1e-4); CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); // Rotate 2nd body 90deg around Z dBodySetPosition (bId2, 0, 1, 0); dRFromAxisAndAngle (R, 0, 0, 1, M_PI/2.0); dBodySetRotation (bId2, R); dJointSetSliderAxis (jId, 1, 0 ,0); CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4); CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[3], 0.70710678118654757, 1e-4); // Rotate 2nd body 45deg around Y dBodySetPosition (bId2, 0, 1, 0); dRFromAxisAndAngle (R, 0, 1, 0, M_PI/4.0); dBodySetRotation (bId2, R); dJointSetSliderAxis (jId, 1, 0 ,0); CHECK_CLOSE (joint->qrel[0], 0.92387953251128674, 1e-4); CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4); CHECK_CLOSE (joint->qrel[2], 0.38268343236508984, 1e-4); CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4); // Rotate in a strange manner // Both bodies at origin dRFromEulerAngles (R, REAL(0.23), REAL(3.1), REAL(-0.73)); dBodySetPosition (bId1, 0, 0, 0); dBodySetRotation (bId1, R); dRFromEulerAngles (R, REAL(-0.57), REAL(1.49), REAL(0.81)); dBodySetPosition (bId2, 0, 0, 0); dBodySetRotation (bId2, R); dJointSetSliderAxis (jId, 1, 0 ,0); CHECK_CLOSE (joint->qrel[0], -0.25526036263124319, 1e-4); CHECK_CLOSE (joint->qrel[1], 0.28434861188441968, 1e-4); CHECK_CLOSE (joint->qrel[2], -0.65308047160141625, 1e-4); CHECK_CLOSE (joint->qrel[3], 0.65381489108282143, 1e-4); } // The 2 bodies are positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is along the X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X { Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X() { wId = dWorldCreate(); bId1 = dBodyCreate (wId); dBodySetPosition (bId1, 0, 0, 0); bId2 = dBodyCreate (wId); dBodySetPosition (bId2, 0, 0, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, bId1, bId2); dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); } ~Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId1; dBodyID bId2; dJointID jId; dxJointSlider* joint; static const dVector3 axis; static const dReal offset; }; const dVector3 Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X::axis = {1, 0, 0}; const dReal Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X::offset = REAL(3.1); // Move 1st body offset unit in the X direction // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 // // Start with a Offset of offset unit // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B1_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Move 1st body offset unit in the opposite X direction // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 // // Start with a Offset of -offset unit // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B1_Minus_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, -offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Move 2nd body offset unit in the X direction // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 // // Start with a Offset of offset unit // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B2_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Move 2nd body offset unit in the opposite X direction // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 // // Start with a Offset of -offset unit // // X-------> X---------> Axis --> // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B2_Minus_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, -offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // The 2 bodies are positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is the opposite of the X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X { Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X() { wId = dWorldCreate(); bId1 = dBodyCreate (wId); dBodySetPosition (bId1, 0, 0, 0); bId2 = dBodyCreate (wId); dBodySetPosition (bId2, 0, 0, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, bId1, bId2); dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); } ~Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId1; dBodyID bId2; dJointID jId; dxJointSlider* joint; static const dVector3 axis; static const dReal offset; }; const dVector3 Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X::axis = {-1, 0, 0}; const dReal Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X::offset = REAL(3.1); // Move 1st body offset unit in the X direction // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 // // Start with a Offset of offset unit // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B1_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Move 1st body offset unit in the opposite X direction // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 // // Start with a Offset of offset unit // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B1_Minus_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, -offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Move 2nd body offset unit in the X direction // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 // // Start with a Offset of offset unit // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B2_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Move 2nd body offset unit in the opposite X direction // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 // // Start with a Offset of -offset unit // // X-------> X---------> <-- Axis // B1 => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B2_Minus_3Unit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, -offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Only body 1 // The body is positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is along the X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X { Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X() { wId = dWorldCreate(); bId1 = dBodyCreate (wId); dBodySetPosition (bId1, 0, 0, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, bId1, NULL); dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); } ~Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId1; dJointID jId; dxJointSlider* joint; static const dVector3 axis; static const dReal offset; }; const dVector3 Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X::axis = {1, 0, 0}; const dReal Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X::offset = REAL(3.1); // Move 1st body offset unit in the X direction // // X-------> X---------> Axis --> // B1 => B1 // TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B1_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Move 1st body offset unit in the opposite X direction // // X-------> X---------> Axis --> // B1 => B1 // TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B1_Minus_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, -offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Only body 1 // The body is positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is in the oppsite X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X { Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X() { wId = dWorldCreate(); bId1 = dBodyCreate (wId); dBodySetPosition (bId1, 0, 0, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, bId1, NULL); dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); } ~Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId1; dJointID jId; dxJointSlider* joint; static const dVector3 axis; static const dReal offset; }; const dVector3 Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X::axis = {-1, 0, 0}; const dReal Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X::offset = REAL(3.1); // Move 1st body offset unit in the X direction // // X-------> X---------> <--- Axis // B1 => B1 // TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B1_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Move 1st body offset unit in the opposite X direction // // X-------> X---------> <--- Axis // B1 => B1 // TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B1_Minus_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId1, -offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Only body 2 // The body is positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is along the X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X { Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X() { wId = dWorldCreate(); bId2 = dBodyCreate (wId); dBodySetPosition (bId2, 0, 0, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, NULL, bId2); dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); } ~Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId2; dJointID jId; dxJointSlider* joint; static const dVector3 axis; static const dReal offset; }; const dVector3 Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X::axis = {1, 0, 0}; const dReal Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X::offset = REAL(3.1); // Move 2nd body offset unit in the X direction // // X-------> X---------> Axis --> // B2 => B2 // TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B2_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // Move 2nd body offset unit in the opposite X direction // // X-------> X---------> Axis --> // B2 => B2 // TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, test_dJointSetSliderAxisOffset_B2_Minus_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, -offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Only body 2 // The body is positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is in the oppsite X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X { Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X() { wId = dWorldCreate(); bId2 = dBodyCreate (wId); dBodySetPosition (bId2, 0, 0, 0); jId = dJointCreateSlider (wId, 0); joint = (dxJointSlider*) jId; dJointAttach (jId, NULL, bId2); dJointSetSliderAxis(jId, axis[0], axis[1], axis[2]); } ~Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId2; dJointID jId; dxJointSlider* joint; static const dVector3 axis; static const dReal offset; }; const dVector3 Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X::axis = {-1, 0, 0}; const dReal Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X::offset = REAL(3.1); // Move 2nd body offset unit in the X direction // // X-------> X---------> <--- Axis // B2 => B2 // TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B2_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, offset, 0, 0); CHECK_CLOSE (offset, dJointGetSliderPosition(jId), 1e-4); } // Move 2nd body offset unit in the opposite X direction // // X-------> X---------> <--- Axis // B2 => B2 // TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderAxisOffset_B2_Minus_OffsetUnit) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); dBodySetPosition(bId2, -offset, 0, 0); CHECK_CLOSE (-offset, dJointGetSliderPosition(jId), 1e-4); } // ========================================================================== // Test Position Rate // ========================================================================== // Apply force on 1st body in the X direction that also is the axis direction // // X-------> X---------> Axis --> // B1 F-> => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Along_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the inverse X direction // // X-------> X---------> Axis --> // B1 <-F => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the X direction that also is the axis direction // // X-------> X---------> <-- Axis // B1 F-> => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the inverse X direction // // X-------> X---------> <-- Axis // B1 <-F => B1 // B2 B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the X direction that also is the axis direction // // X-------> X---------> Axis --> // B1 => B1 // B2 F-> B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Along_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the inverse X direction // // X-------> X---------> Axis --> // B1 => B1 // B2 <-F B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the X direction that also is the axis direction // // X-------> X---------> <-- Axis // B1 => B1 // B2 F-> B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the inverse X direction // // X-------> X---------> <-- Axis // B1 => B1 // B2 <-F B2 TEST_FIXTURE (Fixture_dxJointSlider_B1_and_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the X direction that also is the axis direction // // X-------> X---------> Axis --> // B1 F-> => B1 TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Along_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the inverse X direction // // X-------> X---------> Axis --> // B1 <-F => B1 TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the X direction that also is the axis direction // // X-------> X---------> <-- Axis // B1 F-> => B1 TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on 1st body in the inverse X direction // // X-------> X---------> <-- Axis // B1 <-F => B1 TEST_FIXTURE (Fixture_dxJointSlider_B1_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B1) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId1, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on body 2 in the X direction that also is the axis direction // // X-------> X---------> Axis --> // B2 F-> B2 TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Along_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on body 2 in the inverse X direction // // X-------> X---------> Axis --> // B2 <-F B2 TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Along_X, test_dJointSetSliderPositionRate_Force_Inverse_of_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on body 2 in the X direction that also is the axis direction // // X-------> X---------> <-- Axis // B2 F-> B2 TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Inverse_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, 1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (1, dJointGetSliderPositionRate(jId), 1e-4); } // Apply force on body 2 in the inverse X direction // // X-------> X---------> <-- Axis // B2 <-F B2 TEST_FIXTURE (Fixture_dxJointSlider_B2_At_Zero_Axis_Inverse_of_X, test_dJointSetSliderPositionRate_Force_Along_of_Axis_on_B2) { CHECK_CLOSE (0.0, dJointGetSliderPosition(jId), 1e-4); CHECK_CLOSE (0.0, dJointGetSliderPositionRate(jId), 1e-4); dBodyAddForce(bId2, -1.0, 0, 0); dWorldQuickStep (wId, 1.0); CHECK_CLOSE (-1, dJointGetSliderPositionRate(jId), 1e-4); } // Create 2 bodies attached by a Slider joint // Axis is along the X axis (Default value) // Anchor at (0, 0, 0) (Default value) // // ^Y // | // | // | // | // Body1 | Body2 // * Z-----*->x struct dxJointSlider_Test_Initialization { dxJointSlider_Test_Initialization() { wId = dWorldCreate(); // Remove gravity to have the only force be the force of the joint dWorldSetGravity(wId, 0,0,0); for (int j=0; j<2; ++j) { bId[j][0] = dBodyCreate (wId); dBodySetPosition (bId[j][0], -1, -2, -3); bId[j][1] = dBodyCreate (wId); dBodySetPosition (bId[j][1], 11, 22, 33); dMatrix3 R; dVector3 axis; // Random axis axis[0] = REAL(0.53); axis[1] = -REAL(0.71); axis[2] = REAL(0.43); dNormalize3(axis); dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], REAL(0.47123)); // 27deg dBodySetRotation (bId[j][0], R); axis[0] = REAL(1.2); axis[1] = REAL(0.87); axis[2] = -REAL(0.33); dNormalize3(axis); dRFromAxisAndAngle (R, axis[0], axis[1], axis[2], REAL(0.47123)); // 27deg dBodySetRotation (bId[j][1], R); jId[j] = dJointCreateSlider (wId, 0); dJointAttach (jId[j], bId[j][0], bId[j][1]); } } ~dxJointSlider_Test_Initialization() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId[2][2]; dJointID jId[2]; }; // Test if setting a Slider joint with its default values // will behave the same as a default Slider joint TEST_FIXTURE (dxJointSlider_Test_Initialization, test_Slider_Initialization) { using namespace std; dVector3 axis; dJointGetSliderAxis(jId[1], axis); dJointSetSliderAxis(jId[1], axis[0], axis[1], axis[2]); CHECK_CLOSE (dJointGetSliderPosition(jId[0]), dJointGetSliderPosition(jId[1]), 1e-6); for (int b=0; b<2; ++b) { // Compare body b of the first joint with its equivalent on the // second joint const dReal *qA = dBodyGetQuaternion(bId[0][b]); const dReal *qB = dBodyGetQuaternion(bId[1][b]); CHECK_CLOSE (qA[0], qB[0], 1e-6); CHECK_CLOSE (qA[1], qB[1], 1e-6); CHECK_CLOSE (qA[2], qB[2], 1e-6); CHECK_CLOSE (qA[3], qB[3], 1e-6); } dWorldStep (wId,0.5); dWorldStep (wId,0.5); dWorldStep (wId,0.5); dWorldStep (wId,0.5); for (int b=0; b<2; ++b) { // Compare body b of the first joint with its equivalent on the // second joint const dReal *qA = dBodyGetQuaternion(bId[0][b]); const dReal *qB = dBodyGetQuaternion(bId[1][b]); CHECK_CLOSE (qA[0], qB[0], 1e-6); CHECK_CLOSE (qA[1], qB[1], 1e-6); CHECK_CLOSE (qA[2], qB[2], 1e-6); CHECK_CLOSE (qA[3], qB[3], 1e-6); const dReal *posA = dBodyGetPosition(bId[0][b]); const dReal *posB = dBodyGetPosition(bId[1][b]); CHECK_CLOSE (posA[0], posB[0], 1e-6); CHECK_CLOSE (posA[1], posB[1], 1e-6); CHECK_CLOSE (posA[2], posB[2], 1e-6); CHECK_CLOSE (posA[3], posB[3], 1e-6); } } // Compare Only body 1 to 2 bodies with one fixed. // // The body are positioned at (0, 0, 0) with no rotation // The joint is a Slider Joint // Axis is along the X axis // Anchor at (0, 0, 0) struct Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X { Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X() { wId = dWorldCreate(); bId1_12 = dBodyCreate (wId); dBodySetPosition (bId1_12, 0, 0, 0); bId2_12 = dBodyCreate (wId); dBodySetPosition (bId2_12, 0, 0, 0); // The force will be added in the function since it is not // always on the same body jId_12 = dJointCreateSlider (wId, 0); dJointAttach(jId_12, bId1_12, bId2_12); fixed = dJointCreateFixed (wId, 0); bId = dBodyCreate (wId); dBodySetPosition (bId, 0, 0, 0); dBodyAddForce (bId, 4, 0, 0); jId = dJointCreateSlider (wId, 0); } ~Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X() { dWorldDestroy (wId); } dWorldID wId; dBodyID bId1_12; dBodyID bId2_12; dJointID jId_12; // Joint with 2 bodies dJointID fixed; dBodyID bId; dJointID jId; // Joint with one body }; // This test compares the result of a slider with 2 bodies where body 2 is // fixed to the world to a slider with only one body at position 1. // // Test the limits [-1, 0.25] when only one body is attached to the joint // using dJointAttach(jId, bId, 0); // TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, test_Limit_minus1_025_One_Body_on_left) { dBodyAddForce (bId1_12, 4, 0, 0); dJointAttach(jId_12, bId1_12, bId2_12); dJointSetSliderParam(jId_12, dParamLoStop, -1); dJointSetSliderParam(jId_12, dParamHiStop, 0.25); dJointAttach(fixed, 0, bId2_12); dJointSetFixed(fixed); dJointAttach(jId, bId, 0); dJointSetSliderParam(jId, dParamLoStop, -1); dJointSetSliderParam(jId, dParamHiStop, 0.25); for (int i=0; i<50; ++i) dWorldStep(wId, 1.0); const dReal *pos1_12 = dBodyGetPosition(bId1_12); const dReal *pos = dBodyGetPosition(bId); CHECK_CLOSE (pos[0], pos1_12[0], 1e-2); CHECK_CLOSE (pos[1], pos1_12[1], 1e-2); CHECK_CLOSE (pos[2], pos1_12[2], 1e-2); } // This test compares the result of a slider with 2 bodies where body 1 is // fixed to the world to a slider with only one body at position 2. // // Test the limits [-1, 0.25] when only one body is attached to the joint // using dJointAttach(jId, 0, bId); // TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, test_Limit_minus1_025_One_Body_on_right) { dBodyAddForce (bId2_12, 4, 0, 0); dJointAttach(jId_12, bId1_12, bId2_12); dJointSetSliderParam(jId_12, dParamLoStop, -1); dJointSetSliderParam(jId_12, dParamHiStop, 0.25); dJointAttach(fixed, bId1_12, 0); dJointSetFixed(fixed); dJointAttach(jId, 0, bId); dJointSetSliderParam(jId, dParamLoStop, -1); dJointSetSliderParam(jId, dParamHiStop, 0.25); for (int i=0; i<50; ++i) { dWorldStep(wId, 1.0); } const dReal *pos2_12 = dBodyGetPosition(bId2_12); const dReal *pos = dBodyGetPosition(bId); CHECK_CLOSE (pos[0], pos2_12[0], 1e-2); CHECK_CLOSE (pos[1], pos2_12[1], 1e-2); CHECK_CLOSE (pos[2], pos2_12[2], 1e-2); } // This test compares the result of a slider with 2 bodies where body 2 is // fixed to the world to a slider with only one body at position 1. // // Test the limits [0, 0] when only one body is attached to the joint // using dJointAttach(jId, bId, 0); // // The body should not move since their is no room between the two limits // TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, test_Limit_0_0_One_Body_on_left) { dBodyAddForce (bId1_12, 4, 0, 0); dJointAttach(jId_12, bId1_12, bId2_12); dJointSetSliderParam(jId_12, dParamLoStop, 0); dJointSetSliderParam(jId_12, dParamHiStop, 0); dJointAttach(fixed, 0, bId2_12); dJointSetFixed(fixed); dJointAttach(jId, bId, 0); dJointSetSliderParam(jId, dParamLoStop, 0); dJointSetSliderParam(jId, dParamHiStop, 0); for (int i=0; i<500; ++i) dWorldStep(wId, 1.0); const dReal *pos1_12 = dBodyGetPosition(bId1_12); const dReal *pos = dBodyGetPosition(bId); CHECK_CLOSE (pos[0], pos1_12[0], 1e-4); CHECK_CLOSE (pos[1], pos1_12[1], 1e-4); CHECK_CLOSE (pos[2], pos1_12[2], 1e-4); CHECK_CLOSE (pos[0], 0, 1e-4); CHECK_CLOSE (pos[1], 0, 1e-4); CHECK_CLOSE (pos[2], 0, 1e-4); } // This test compares the result of a slider with 2 bodies where body 1 is // fixed to the world to a slider with only one body at position 2. // // Test the limits [0, 0] when only one body is attached to the joint // using dJointAttach(jId, 0, bId); // // The body should not move since their is no room between the two limits // TEST_FIXTURE(Fixture_dxJointSlider_Compare_Body_At_Zero_Axis_Along_X, test_Limit_0_0_One_Body_on_right) { dBodyAddForce (bId2_12, 4, 0, 0); dJointAttach(jId_12, bId1_12, bId2_12); dJointSetSliderParam(jId_12, dParamLoStop, 0); dJointSetSliderParam(jId_12, dParamHiStop, 0); dJointAttach(fixed, bId1_12, 0); dJointSetFixed(fixed); dJointAttach(jId, 0, bId); dJointSetSliderParam(jId, dParamLoStop, 0); dJointSetSliderParam(jId, dParamHiStop, 0); for (int i=0; i<500; ++i) dWorldStep(wId, 1.0); const dReal *pos2_12 = dBodyGetPosition(bId2_12); const dReal *pos = dBodyGetPosition(bId); CHECK_CLOSE (pos[0], pos2_12[0], 1e-4); CHECK_CLOSE (pos[1], pos2_12[1], 1e-4); CHECK_CLOSE (pos[2], pos2_12[2], 1e-4); CHECK_CLOSE (pos[0], 0, 1e-4); CHECK_CLOSE (pos[1], 0, 1e-4); CHECK_CLOSE (pos[2], 0, 1e-4); } } // End of SUITE TestdxJointSlider