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/joinst/fixed.cpp
28 //
29 //
30 ////////////////////////////////////////////////////////////////////////////////
31
32 #include <UnitTest++.h>
33 #include <ode/ode.h>
34
35 #include "config.h"
36 #include "../../ode/src/joints/amotor.h"
37
38 const dReal tol = 1e-5;
39
SUITE(TestdxJointAMotor)40 SUITE (TestdxJointAMotor)
41 {
42 struct FixtureBase {
43 dWorldID world;
44 dBodyID body;
45 dJointID joint;
46
47 FixtureBase()
48 {
49 world = dWorldCreate();
50 body = dBodyCreate(world);
51 joint = dJointCreateAMotor(world, 0);
52 }
53
54 ~FixtureBase()
55 {
56 dJointDestroy(joint);
57 dBodyDestroy(body);
58 dWorldDestroy(world);
59 }
60 };
61
62
63 struct FixtureXUser: FixtureBase {
64 FixtureXUser()
65 {
66 // body only allowed to rotate around X axis
67 dBodySetFiniteRotationMode(body, 1);
68 dBodySetFiniteRotationAxis(body, 1, 0, 0);
69 dJointAttach(joint, body, 0);
70 dJointSetAMotorNumAxes(joint, 2);
71 dJointSetAMotorAxis(joint, 0, 2, 0, 1, 0);
72 dJointSetAMotorAxis(joint, 1, 2, 0, 0, 1);
73 dJointSetAMotorParam(joint, dParamVel, 0);
74 dJointSetAMotorParam(joint, dParamFMax, dInfinity);
75 dJointSetAMotorParam(joint, dParamVel2, 0);
76 dJointSetAMotorParam(joint, dParamFMax2, dInfinity);
77 }
78 };
79
80 TEST_FIXTURE(FixtureXUser, rotate_x)
81 {
82 const dReal h = 1;
83 const dReal v = 1;
84 dMatrix3 identity = {1, 0, 0, 0,
85 0, 1, 0, 0,
86 0, 0, 1, 0};
87 dBodySetRotation(body, identity);
88 dBodySetAngularVel(body, v, 0, 0);
89 dWorldQuickStep(world, h);
90 const dReal* rot = dBodyGetRotation(body);
91 CHECK_CLOSE(1, rot[0], tol);
92 CHECK_CLOSE(0, rot[4], tol);
93 CHECK_CLOSE(0, rot[8], tol);
94
95 CHECK_CLOSE(0, rot[1], tol);
96 CHECK_CLOSE(dCos(v*h), rot[5], tol);
97 CHECK_CLOSE(dSin(v*h), rot[9], tol);
98
99 CHECK_CLOSE(0, rot[2], tol);
100 CHECK_CLOSE(-dSin(v*h), rot[6], tol);
101 CHECK_CLOSE( dCos(v*h), rot[10], tol);
102 }
103
104 TEST_FIXTURE(FixtureXUser, rotate_yz)
105 {
106 const dReal h = 1;
107 const dReal v = 1;
108 dMatrix3 identity = {1, 0, 0, 0,
109 0, 1, 0, 0,
110 0, 0, 1, 0};
111 dBodySetRotation(body, identity);
112
113 dVector3 axis_y;
114 dJointGetAMotorAxis(joint, 0, axis_y);
115 CHECK_CLOSE(0, axis_y[0], tol);
116 CHECK_CLOSE(1, axis_y[1], tol);
117 CHECK_CLOSE(0, axis_y[2], tol);
118
119 dVector3 axis_z;
120 dJointGetAMotorAxis(joint, 1, axis_z);
121 CHECK_CLOSE(0, axis_z[0], tol);
122 CHECK_CLOSE(0, axis_z[1], tol);
123 CHECK_CLOSE(1, axis_z[2], tol);
124
125 dBodySetAngularVel(body, 0, v, v);
126 dWorldStep(world, h);
127 const dReal* rot = dBodyGetRotation(body);
128 CHECK_CLOSE(1, rot[0], tol);
129 CHECK_CLOSE(0, rot[4], tol);
130 CHECK_CLOSE(0, rot[8], tol);
131
132 CHECK_CLOSE(0, rot[1], tol);
133 CHECK_CLOSE(1, rot[5], tol);
134 CHECK_CLOSE(0, rot[9], tol);
135
136 CHECK_CLOSE(0, rot[2], tol);
137 CHECK_CLOSE(0, rot[6], tol);
138 CHECK_CLOSE(1, rot[10], tol);
139 }
140
141
142 TEST_FIXTURE(FixtureBase, sanity_check)
143 {
144 dMatrix3 R;
145 dRFromAxisAndAngle(R, 1, 1, 1, 10*M_PI/180);
146 dBodySetRotation(body, R);
147
148 dVector3 res;
149
150 dJointAttach(joint, body, 0);
151 dJointSetAMotorNumAxes(joint, 3);
152 CHECK_EQUAL(3, dJointGetAMotorNumAxes(joint));
153
154 // axes relative to world
155 dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
156 dJointGetAMotorAxis(joint, 0, res);
157 CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0));
158 CHECK_CLOSE(1, res[0], tol);
159 CHECK_CLOSE(0, res[1], tol);
160 CHECK_CLOSE(0, res[2], tol);
161
162 dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0);
163 dJointGetAMotorAxis(joint, 1, res);
164 CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1));
165 CHECK_CLOSE(0, res[0], tol);
166 CHECK_CLOSE(1, res[1], tol);
167 CHECK_CLOSE(0, res[2], tol);
168
169 dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
170 dJointGetAMotorAxis(joint, 2, res);
171 CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2));
172 CHECK_CLOSE(0, res[0], tol);
173 CHECK_CLOSE(0, res[1], tol);
174 CHECK_CLOSE(1, res[2], tol);
175
176 // axes relative to body1
177 dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0);
178 dJointGetAMotorAxis(joint, 0, res);
179 CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0));
180 CHECK_CLOSE(1, res[0], tol);
181 CHECK_CLOSE(0, res[1], tol);
182 CHECK_CLOSE(0, res[2], tol);
183
184 dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0);
185 dJointGetAMotorAxis(joint, 1, res);
186 CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1));
187 CHECK_CLOSE(0, res[0], tol);
188 CHECK_CLOSE(1, res[1], tol);
189 CHECK_CLOSE(0, res[2], tol);
190
191 dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1);
192 dJointGetAMotorAxis(joint, 2, res);
193 CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2));
194 CHECK_CLOSE(0, res[0], tol);
195 CHECK_CLOSE(0, res[1], tol);
196 CHECK_CLOSE(1, res[2], tol);
197
198 // axes relative to body2
199 dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0);
200 dJointGetAMotorAxis(joint, 0, res);
201 CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0));
202 CHECK_CLOSE(1, res[0], tol);
203 CHECK_CLOSE(0, res[1], tol);
204 CHECK_CLOSE(0, res[2], tol);
205
206 dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0);
207 dJointGetAMotorAxis(joint, 1, res);
208 CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1));
209 CHECK_CLOSE(0, res[0], tol);
210 CHECK_CLOSE(1, res[1], tol);
211 CHECK_CLOSE(0, res[2], tol);
212
213 dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1);
214 dJointGetAMotorAxis(joint, 2, res);
215 CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2));
216 CHECK_CLOSE(0, res[0], tol);
217 CHECK_CLOSE(0, res[1], tol);
218 CHECK_CLOSE(1, res[2], tol);
219
220 // reverse attachment to force internal reversal
221 dJointAttach(joint, 0, body);
222 // axes relative to world
223 dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
224 dJointGetAMotorAxis(joint, 0, res);
225 CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0));
226 CHECK_CLOSE(1, res[0], tol);
227 CHECK_CLOSE(0, res[1], tol);
228 CHECK_CLOSE(0, res[2], tol);
229
230 dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0);
231 dJointGetAMotorAxis(joint, 1, res);
232 CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1));
233 CHECK_CLOSE(0, res[0], tol);
234 CHECK_CLOSE(1, res[1], tol);
235 CHECK_CLOSE(0, res[2], tol);
236
237 dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
238 dJointGetAMotorAxis(joint, 2, res);
239 CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2));
240 CHECK_CLOSE(0, res[0], tol);
241 CHECK_CLOSE(0, res[1], tol);
242 CHECK_CLOSE(1, res[2], tol);
243
244 // axes relative to body1
245 dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0);
246 dJointGetAMotorAxis(joint, 0, res);
247 CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0));
248 CHECK_CLOSE(1, res[0], tol);
249 CHECK_CLOSE(0, res[1], tol);
250 CHECK_CLOSE(0, res[2], tol);
251
252 dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0);
253 dJointGetAMotorAxis(joint, 1, res);
254 CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1));
255 CHECK_CLOSE(0, res[0], tol);
256 CHECK_CLOSE(1, res[1], tol);
257 CHECK_CLOSE(0, res[2], tol);
258
259 dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1);
260 dJointGetAMotorAxis(joint, 2, res);
261 CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2));
262 CHECK_CLOSE(0, res[0], tol);
263 CHECK_CLOSE(0, res[1], tol);
264 CHECK_CLOSE(1, res[2], tol);
265
266 // axes relative to body2
267 dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0);
268 dJointGetAMotorAxis(joint, 0, res);
269 CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0));
270 CHECK_CLOSE(1, res[0], tol);
271 CHECK_CLOSE(0, res[1], tol);
272 CHECK_CLOSE(0, res[2], tol);
273
274 dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0);
275 dJointGetAMotorAxis(joint, 1, res);
276 CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1));
277 CHECK_CLOSE(0, res[0], tol);
278 CHECK_CLOSE(1, res[1], tol);
279 CHECK_CLOSE(0, res[2], tol);
280
281 dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1);
282 dJointGetAMotorAxis(joint, 2, res);
283 CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2));
284 CHECK_CLOSE(0, res[0], tol);
285 CHECK_CLOSE(0, res[1], tol);
286 CHECK_CLOSE(1, res[2], tol);
287 }
288
289
290 struct FixtureXEuler : FixtureBase {
291 FixtureXEuler()
292 {
293 // body only allowed to rotate around X axis
294 dJointAttach(joint, 0, body);
295 dJointSetAMotorMode(joint, dAMotorEuler);
296 dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
297 dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
298 }
299 };
300
301
302 TEST_FIXTURE(FixtureXEuler, check_axes)
303 {
304 // test patch #181 bug fix
305 dVector3 axis_x;
306 dJointGetAMotorAxis(joint, 0, axis_x);
307 CHECK_CLOSE(1, axis_x[0], tol);
308 CHECK_CLOSE(0, axis_x[1], tol);
309 CHECK_CLOSE(0, axis_x[2], tol);
310
311 dVector3 axis_y;
312 dJointGetAMotorAxis(joint, 1, axis_y);
313 CHECK_CLOSE(0, axis_y[0], tol);
314 CHECK_CLOSE(1, axis_y[1], tol);
315 CHECK_CLOSE(0, axis_y[2], tol);
316
317 dVector3 axis_z;
318 dJointGetAMotorAxis(joint, 2, axis_z);
319 CHECK_CLOSE(0, axis_z[0], tol);
320 CHECK_CLOSE(0, axis_z[1], tol);
321 CHECK_CLOSE(1, axis_z[2], tol);
322 }
323
324 } // End of SUITE TestdxJointAMotor
325