summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/tests/joints/amotor.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/tests/joints/amotor.cpp')
-rw-r--r--libs/ode-0.16.1/tests/joints/amotor.cpp324
1 files changed, 324 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/tests/joints/amotor.cpp b/libs/ode-0.16.1/tests/joints/amotor.cpp
new file mode 100644
index 0000000..0dc1c2d
--- /dev/null
+++ b/libs/ode-0.16.1/tests/joints/amotor.cpp
@@ -0,0 +1,324 @@
+/*************************************************************************
+ * *
+ * 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 "config.h"
+#include "../../ode/src/joints/amotor.h"
+
+const dReal tol = 1e-5;
+
+SUITE (TestdxJointAMotor)
+{
+ struct FixtureBase {
+ dWorldID world;
+ dBodyID body;
+ dJointID joint;
+
+ FixtureBase()
+ {
+ world = dWorldCreate();
+ body = dBodyCreate(world);
+ joint = dJointCreateAMotor(world, 0);
+ }
+
+ ~FixtureBase()
+ {
+ dJointDestroy(joint);
+ dBodyDestroy(body);
+ dWorldDestroy(world);
+ }
+ };
+
+
+ struct FixtureXUser: FixtureBase {
+ FixtureXUser()
+ {
+ // body only allowed to rotate around X axis
+ dBodySetFiniteRotationMode(body, 1);
+ dBodySetFiniteRotationAxis(body, 1, 0, 0);
+ dJointAttach(joint, body, 0);
+ dJointSetAMotorNumAxes(joint, 2);
+ dJointSetAMotorAxis(joint, 0, 2, 0, 1, 0);
+ dJointSetAMotorAxis(joint, 1, 2, 0, 0, 1);
+ dJointSetAMotorParam(joint, dParamVel, 0);
+ dJointSetAMotorParam(joint, dParamFMax, dInfinity);
+ dJointSetAMotorParam(joint, dParamVel2, 0);
+ dJointSetAMotorParam(joint, dParamFMax2, dInfinity);
+ }
+ };
+
+ TEST_FIXTURE(FixtureXUser, rotate_x)
+ {
+ const dReal h = 1;
+ const dReal v = 1;
+ dMatrix3 identity = {1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0};
+ dBodySetRotation(body, identity);
+ dBodySetAngularVel(body, v, 0, 0);
+ dWorldQuickStep(world, h);
+ const dReal* rot = dBodyGetRotation(body);
+ CHECK_CLOSE(1, rot[0], tol);
+ CHECK_CLOSE(0, rot[4], tol);
+ CHECK_CLOSE(0, rot[8], tol);
+
+ CHECK_CLOSE(0, rot[1], tol);
+ CHECK_CLOSE(dCos(v*h), rot[5], tol);
+ CHECK_CLOSE(dSin(v*h), rot[9], tol);
+
+ CHECK_CLOSE(0, rot[2], tol);
+ CHECK_CLOSE(-dSin(v*h), rot[6], tol);
+ CHECK_CLOSE( dCos(v*h), rot[10], tol);
+ }
+
+ TEST_FIXTURE(FixtureXUser, rotate_yz)
+ {
+ const dReal h = 1;
+ const dReal v = 1;
+ dMatrix3 identity = {1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0};
+ dBodySetRotation(body, identity);
+
+ dVector3 axis_y;
+ dJointGetAMotorAxis(joint, 0, axis_y);
+ CHECK_CLOSE(0, axis_y[0], tol);
+ CHECK_CLOSE(1, axis_y[1], tol);
+ CHECK_CLOSE(0, axis_y[2], tol);
+
+ dVector3 axis_z;
+ dJointGetAMotorAxis(joint, 1, axis_z);
+ CHECK_CLOSE(0, axis_z[0], tol);
+ CHECK_CLOSE(0, axis_z[1], tol);
+ CHECK_CLOSE(1, axis_z[2], tol);
+
+ dBodySetAngularVel(body, 0, v, v);
+ dWorldStep(world, h);
+ const dReal* rot = dBodyGetRotation(body);
+ CHECK_CLOSE(1, rot[0], tol);
+ CHECK_CLOSE(0, rot[4], tol);
+ CHECK_CLOSE(0, rot[8], tol);
+
+ CHECK_CLOSE(0, rot[1], tol);
+ CHECK_CLOSE(1, rot[5], tol);
+ CHECK_CLOSE(0, rot[9], tol);
+
+ CHECK_CLOSE(0, rot[2], tol);
+ CHECK_CLOSE(0, rot[6], tol);
+ CHECK_CLOSE(1, rot[10], tol);
+ }
+
+
+ TEST_FIXTURE(FixtureBase, sanity_check)
+ {
+ dMatrix3 R;
+ dRFromAxisAndAngle(R, 1, 1, 1, 10*M_PI/180);
+ dBodySetRotation(body, R);
+
+ dVector3 res;
+
+ dJointAttach(joint, body, 0);
+ dJointSetAMotorNumAxes(joint, 3);
+ CHECK_EQUAL(3, dJointGetAMotorNumAxes(joint));
+
+ // axes relative to world
+ dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
+ dJointGetAMotorAxis(joint, 0, res);
+ CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0));
+ CHECK_CLOSE(1, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0);
+ dJointGetAMotorAxis(joint, 1, res);
+ CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(1, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
+ dJointGetAMotorAxis(joint, 2, res);
+ CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(1, res[2], tol);
+
+ // axes relative to body1
+ dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0);
+ dJointGetAMotorAxis(joint, 0, res);
+ CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0));
+ CHECK_CLOSE(1, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0);
+ dJointGetAMotorAxis(joint, 1, res);
+ CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(1, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1);
+ dJointGetAMotorAxis(joint, 2, res);
+ CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(1, res[2], tol);
+
+ // axes relative to body2
+ dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0);
+ dJointGetAMotorAxis(joint, 0, res);
+ CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0));
+ CHECK_CLOSE(1, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0);
+ dJointGetAMotorAxis(joint, 1, res);
+ CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(1, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1);
+ dJointGetAMotorAxis(joint, 2, res);
+ CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(1, res[2], tol);
+
+ // reverse attachment to force internal reversal
+ dJointAttach(joint, 0, body);
+ // axes relative to world
+ dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
+ dJointGetAMotorAxis(joint, 0, res);
+ CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0));
+ CHECK_CLOSE(1, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0);
+ dJointGetAMotorAxis(joint, 1, res);
+ CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(1, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
+ dJointGetAMotorAxis(joint, 2, res);
+ CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(1, res[2], tol);
+
+ // axes relative to body1
+ dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0);
+ dJointGetAMotorAxis(joint, 0, res);
+ CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0));
+ CHECK_CLOSE(1, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0);
+ dJointGetAMotorAxis(joint, 1, res);
+ CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(1, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1);
+ dJointGetAMotorAxis(joint, 2, res);
+ CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(1, res[2], tol);
+
+ // axes relative to body2
+ dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0);
+ dJointGetAMotorAxis(joint, 0, res);
+ CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0));
+ CHECK_CLOSE(1, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0);
+ dJointGetAMotorAxis(joint, 1, res);
+ CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(1, res[1], tol);
+ CHECK_CLOSE(0, res[2], tol);
+
+ dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1);
+ dJointGetAMotorAxis(joint, 2, res);
+ CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2));
+ CHECK_CLOSE(0, res[0], tol);
+ CHECK_CLOSE(0, res[1], tol);
+ CHECK_CLOSE(1, res[2], tol);
+ }
+
+
+ struct FixtureXEuler : FixtureBase {
+ FixtureXEuler()
+ {
+ // body only allowed to rotate around X axis
+ dJointAttach(joint, 0, body);
+ dJointSetAMotorMode(joint, dAMotorEuler);
+ dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
+ dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
+ }
+ };
+
+
+ TEST_FIXTURE(FixtureXEuler, check_axes)
+ {
+ // test patch #181 bug fix
+ dVector3 axis_x;
+ dJointGetAMotorAxis(joint, 0, axis_x);
+ CHECK_CLOSE(1, axis_x[0], tol);
+ CHECK_CLOSE(0, axis_x[1], tol);
+ CHECK_CLOSE(0, axis_x[2], tol);
+
+ dVector3 axis_y;
+ dJointGetAMotorAxis(joint, 1, axis_y);
+ CHECK_CLOSE(0, axis_y[0], tol);
+ CHECK_CLOSE(1, axis_y[1], tol);
+ CHECK_CLOSE(0, axis_y[2], tol);
+
+ dVector3 axis_z;
+ dJointGetAMotorAxis(joint, 2, axis_z);
+ CHECK_CLOSE(0, axis_z[0], tol);
+ CHECK_CLOSE(0, axis_z[1], tol);
+ CHECK_CLOSE(1, axis_z[2], tol);
+ }
+
+} // End of SUITE TestdxJointAMotor
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 02:26:46 +0000