3055 lines
97 KiB
C++
3055 lines
97 KiB
C++
/*************************************************************************
|
|
* *
|
|
* 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/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)
|