1 /*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22 //234567890123456789012345678901234567890123456789012345678901234567890123456789
23 // 1 2 3 4 5 6 7
24
25 ////////////////////////////////////////////////////////////////////////////////
26 // This file create unit test for some of the functions found in:
27 // ode/src/joint.cpp
28 //
29 //
30 ////////////////////////////////////////////////////////////////////////////////
31 #include <algorithm>
32 #include <UnitTest++.h>
33 #include <ode/ode.h>
34 #include "../ode/src/config.h"
35 #include "../ode/src/joints/joints.h"
36
37
38 /*
39 * Tests for contact friction
40 */
41
SUITE(JointContact)42 SUITE(JointContact)
43 {
44 struct ContactSetup
45 {
46 dWorldID world;
47 dBodyID body1;
48 dBodyID body2;
49 dJointID joint;
50
51 ContactSetup()
52 {
53 world = dWorldCreate();
54 body1 = dBodyCreate(world);
55 body2 = dBodyCreate(world);
56
57 dBodySetPosition(body1, -1, 0, 0);
58 dBodySetPosition(body2, 1, 0, 0);
59 }
60
61 ~ContactSetup()
62 {
63 dBodyDestroy(body1);
64 dBodyDestroy(body2);
65 dWorldDestroy(world);
66 }
67 };
68
69 TEST_FIXTURE(ContactSetup,
70 test_ZeroMu)
71 {
72 dxJoint::Info1 info1;
73 dxJoint::Info2Descr info2;
74 dReal dummy_J[3][12] = {{0}};
75 dReal dummy_c[3];
76 dReal dummy_cfm[3];
77 dReal dummy_lo[3];
78 dReal dummy_hi[3];
79 int dummy_findex[3];
80
81 dReal info2_fps = 100;
82 dReal info2_erp = 0;
83 info2.J1l = dummy_J[0];
84 info2.J1a = dummy_J[0] + 3;
85 info2.J2l = dummy_J[0] + 6;
86 info2.J2a = dummy_J[0] + 9;
87 info2.rowskip = 12;
88 info2.c = dummy_c;
89 info2.cfm = dummy_cfm;
90 info2.lo = dummy_lo;
91 info2.hi = dummy_hi;
92 info2.findex = dummy_findex;
93
94 #define ZERO_ALL do { \
95 memset(dummy_J, 0, sizeof dummy_J); \
96 memset(dummy_c, 0, sizeof dummy_c); \
97 memset(dummy_cfm, 0, sizeof dummy_cfm); \
98 memset(dummy_lo, 0, sizeof dummy_lo); \
99 memset(dummy_hi, 0, sizeof dummy_hi); \
100 std::fill(dummy_findex, dummy_findex+3, -1);; \
101 } \
102 while (0)
103
104 dContact contact;
105 contact.surface.mode = dContactMu2 | dContactFDir1 | dContactApprox1;
106
107 contact.geom.pos[0] = 0;
108 contact.geom.pos[1] = 0;
109 contact.geom.pos[2] = 0;
110
111 // normal points into body1
112 contact.geom.normal[0] = -1;
113 contact.geom.normal[1] = 0;
114 contact.geom.normal[2] = 0;
115
116 contact.geom.depth = 0;
117
118 contact.geom.g1 = 0;
119 contact.geom.g2 = 0;
120
121 // we ask for fdir1 = +Y, so fdir2 = normal x fdir1 = -Z
122 contact.fdir1[0] = 0;
123 contact.fdir1[1] = 1;
124 contact.fdir1[2] = 0;
125
126 /*
127 * First, test with mu = 0, mu2 = 1
128 * Because there is no friction on the first direction (+Y) the body
129 * is allowed to translate in the Y axis and rotate around the Z axis.
130 *
131 * That is, the only constraint will be for the second dir (-Z):
132 * so J[1] = [ 0 0 -1 0 1 0 0 0 1 0 1 0 ]
133 */
134 contact.surface.mu = 0;
135 contact.surface.mu2 = 1;
136 joint = dJointCreateContact(world, 0, &contact);
137 dJointAttach(joint, body1, body2);
138 joint->getInfo1(&info1);
139 CHECK_EQUAL(2, (int)info1.m);
140 ZERO_ALL;
141 joint->getInfo2(info2_fps, info2_erp, &info2);
142 CHECK_CLOSE(0, dummy_J[1][0], 1e-6);
143 CHECK_CLOSE(0, dummy_J[1][1], 1e-6);
144 CHECK_CLOSE(-1, dummy_J[1][2], 1e-6);
145 CHECK_CLOSE(0, dummy_J[1][3], 1e-6);
146 CHECK_CLOSE(1, dummy_J[1][4], 1e-6);
147 CHECK_CLOSE(0, dummy_J[1][5], 1e-6);
148 CHECK_CLOSE(0, dummy_J[1][6], 1e-6);
149 CHECK_CLOSE(0, dummy_J[1][7], 1e-6);
150 CHECK_CLOSE(1, dummy_J[1][8], 1e-6);
151 CHECK_CLOSE(0, dummy_J[1][9], 1e-6);
152 CHECK_CLOSE(1, dummy_J[1][10], 1e-6);
153 CHECK_CLOSE(0, dummy_J[1][11], 1e-6);
154 CHECK_EQUAL(0, dummy_findex[1]); // because of dContactApprox1
155 dJointDestroy(joint);
156
157
158 /*
159 * Now try with no frictino in the second direction. The Jacobian should look like:
160 * J[1] = [ 0 1 0 0 0 1 0 -1 0 0 0 1 ]
161 */
162 // try again, with zero mu2
163 contact.surface.mu = 1;
164 contact.surface.mu2 = 0;
165 joint = dJointCreateContact(world, 0, &contact);
166 dJointAttach(joint, body1, body2);
167 joint->getInfo1(&info1);
168 CHECK_EQUAL(2, (int)info1.m);
169 ZERO_ALL;
170 joint->getInfo2(info2_fps, info2_erp, &info2);
171 CHECK_CLOSE(0, dummy_J[1][0], 1e-6);
172 CHECK_CLOSE(1, dummy_J[1][1], 1e-6);
173 CHECK_CLOSE(0, dummy_J[1][2], 1e-6);
174 CHECK_CLOSE(0, dummy_J[1][3], 1e-6);
175 CHECK_CLOSE(0, dummy_J[1][4], 1e-6);
176 CHECK_CLOSE(1, dummy_J[1][5], 1e-6);
177 CHECK_CLOSE(0, dummy_J[1][6], 1e-6);
178 CHECK_CLOSE(-1, dummy_J[1][7], 1e-6);
179 CHECK_CLOSE(0, dummy_J[1][8], 1e-6);
180 CHECK_CLOSE(0, dummy_J[1][9], 1e-6);
181 CHECK_CLOSE(0, dummy_J[1][10], 1e-6);
182 CHECK_CLOSE(1, dummy_J[1][11], 1e-6);
183 CHECK_EQUAL(0, dummy_findex[1]); // because of dContactApprox1
184 dJointDestroy(joint);
185 }
186
187 }
188