diff options
Diffstat (limited to 'libs/ode-0.16.1/tests/friction.cpp')
| -rw-r--r-- | libs/ode-0.16.1/tests/friction.cpp | 176 |
1 files changed, 176 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/tests/friction.cpp b/libs/ode-0.16.1/tests/friction.cpp new file mode 100644 index 0000000..c082e85 --- /dev/null +++ b/libs/ode-0.16.1/tests/friction.cpp @@ -0,0 +1,176 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ +//234567890123456789012345678901234567890123456789012345678901234567890123456789 +// 1 2 3 4 5 6 7 + +//////////////////////////////////////////////////////////////////////////////// +// This file create unit test for some of the functions found in: +// ode/src/joint.cpp +// +// +//////////////////////////////////////////////////////////////////////////////// +#include <algorithm> +#include <UnitTest++.h> +#include <ode/ode.h> +#include "../ode/src/config.h" +#include "../ode/src/joints/joints.h" + + +/* + * Tests for contact friction + */ + +SUITE(JointContact) +{ + struct ContactSetup + { + dWorldID world; + dBodyID body1; + dBodyID body2; + dJointID joint; + + ContactSetup() + { + world = dWorldCreate(); + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetPosition(body1, -1, 0, 0); + dBodySetPosition(body2, 1, 0, 0); + } + + ~ContactSetup() + { + dBodyDestroy(body1); + dBodyDestroy(body2); + dWorldDestroy(world); + } + }; + + TEST_FIXTURE(ContactSetup, + test_ZeroMu) + { + dxJoint::Info1 info1; + dReal dummy_J[3][16] = {{0}}; + int dummy_findex[3]; + + dReal info2_fps = 100; + dReal info2_erp = 0; + dReal *J1 = dummy_J[0]; + dReal *J2 = dummy_J[0] + 8; + dReal *rhscfm = dummy_J[0] + 6; + dReal *lohi = dummy_J[0] + 14; + unsigned rowskip = 16; + int *findex = dummy_findex; + +#define ZERO_ALL do { \ + memset(dummy_J, 0, sizeof dummy_J); \ + std::fill(dummy_findex, dummy_findex+3, -1); \ + } \ + while (0) + + dContact contact; + contact.surface.mode = dContactMu2 | dContactFDir1 | dContactApprox1; + + contact.geom.pos[0] = 0; + contact.geom.pos[1] = 0; + contact.geom.pos[2] = 0; + + // normal points into body1 + contact.geom.normal[0] = -1; + contact.geom.normal[1] = 0; + contact.geom.normal[2] = 0; + + contact.geom.depth = 0; + + contact.geom.g1 = 0; + contact.geom.g2 = 0; + + // we ask for fdir1 = +Y, so fdir2 = normal x fdir1 = -Z + contact.fdir1[0] = 0; + contact.fdir1[1] = 1; + contact.fdir1[2] = 0; + + /* + * First, test with mu = 0, mu2 = 1 + * Because there is no friction on the first direction (+Y) the body + * is allowed to translate in the Y axis and rotate around the Z axis. + * + * That is, the only constraint will be for the second dir (-Z): + * so J[1] = [ 0 0 -1 0 1 0 0 0 1 0 1 0 ] + */ + contact.surface.mu = 0; + contact.surface.mu2 = 1; + joint = dJointCreateContact(world, 0, &contact); + dJointAttach(joint, body1, body2); + joint->getInfo1(&info1); + CHECK_EQUAL(2, (int)info1.m); + ZERO_ALL; + joint->getInfo2(info2_fps, info2_erp, rowskip, J1, J2, + rowskip, rhscfm, lohi, findex); + CHECK_CLOSE(0, dummy_J[1][0], 1e-6); + CHECK_CLOSE(0, dummy_J[1][1], 1e-6); + CHECK_CLOSE(-1, dummy_J[1][2], 1e-6); + CHECK_CLOSE(0, dummy_J[1][3], 1e-6); + CHECK_CLOSE(1, dummy_J[1][4], 1e-6); + CHECK_CLOSE(0, dummy_J[1][5], 1e-6); + CHECK_CLOSE(0, dummy_J[1][8], 1e-6); + CHECK_CLOSE(0, dummy_J[1][9], 1e-6); + CHECK_CLOSE(1, dummy_J[1][10], 1e-6); + CHECK_CLOSE(0, dummy_J[1][11], 1e-6); + CHECK_CLOSE(1, dummy_J[1][12], 1e-6); + CHECK_CLOSE(0, dummy_J[1][13], 1e-6); + CHECK_EQUAL(0, dummy_findex[1]); // because of dContactApprox1 + dJointDestroy(joint); + + + /* + * Now try with no frictino in the second direction. The Jacobian should look like: + * J[1] = [ 0 1 0 0 0 1 0 -1 0 0 0 1 ] + */ + // try again, with zero mu2 + contact.surface.mu = 1; + contact.surface.mu2 = 0; + joint = dJointCreateContact(world, 0, &contact); + dJointAttach(joint, body1, body2); + joint->getInfo1(&info1); + CHECK_EQUAL(2, (int)info1.m); + ZERO_ALL; + joint->getInfo2(info2_fps, info2_erp, rowskip, J1, J2, + rowskip, rhscfm, lohi, findex); + CHECK_CLOSE(0, dummy_J[1][0], 1e-6); + CHECK_CLOSE(1, dummy_J[1][1], 1e-6); + CHECK_CLOSE(0, dummy_J[1][2], 1e-6); + CHECK_CLOSE(0, dummy_J[1][3], 1e-6); + CHECK_CLOSE(0, dummy_J[1][4], 1e-6); + CHECK_CLOSE(1, dummy_J[1][5], 1e-6); + CHECK_CLOSE(0, dummy_J[1][8], 1e-6); + CHECK_CLOSE(-1, dummy_J[1][9], 1e-6); + CHECK_CLOSE(0, dummy_J[1][10], 1e-6); + CHECK_CLOSE(0, dummy_J[1][11], 1e-6); + CHECK_CLOSE(0, dummy_J[1][12], 1e-6); + CHECK_CLOSE(1, dummy_J[1][13], 1e-6); + CHECK_EQUAL(0, dummy_findex[1]); // because of dContactApprox1 + dJointDestroy(joint); + } + +} |