diff options
Diffstat (limited to 'libs/ode-0.16.1/tests/joints/fixed.cpp')
| -rw-r--r-- | libs/ode-0.16.1/tests/joints/fixed.cpp | 149 |
1 files changed, 149 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/tests/joints/fixed.cpp b/libs/ode-0.16.1/tests/joints/fixed.cpp new file mode 100644 index 0000000..c069073 --- /dev/null +++ b/libs/ode-0.16.1/tests/joints/fixed.cpp @@ -0,0 +1,149 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joinst/fixed.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// + +#include <UnitTest++.h> +#include <ode/ode.h> + +#include "../../ode/src/config.h" +#include "../../ode/src/joints/fixed.h" + +SUITE (TestdxJointFixed) +{ + struct dxJointFixed_Fixture_1 + { + dxJointFixed_Fixture_1() + { + wId = dWorldCreate(); + + bId1 = dBodyCreate (wId); + dBodySetPosition (bId1, 0, -1, 0); + + bId2 = dBodyCreate (wId); + dBodySetPosition (bId2, 0, 1, 0); + + jId = dJointCreateFixed (wId, 0); + joint = (dxJointFixed*) jId; + + + dJointAttach (jId, bId1, bId2); + } + + ~dxJointFixed_Fixture_1() + { + dWorldDestroy (wId); + } + + dWorldID wId; + + dBodyID bId1; + dBodyID bId2; + + + dJointID jId; + dxJointFixed* joint; + }; + + TEST_FIXTURE (dxJointFixed_Fixture_1, test_dJointSetFixed) + { + // the 2 bodies are align + dJointSetFixed (jId); + 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); + + dJointSetFixed (jId); + 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); + + dJointSetFixed (jId); + 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); + + dJointSetFixed (jId); + 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); + + dJointSetFixed (jId); + 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); + + dJointSetFixed (jId); + 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); + } + + +} // End of SUITE TestdxJointFixed |