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 | |
---|
23 | /* |
---|
24 | |
---|
25 | design note: the general principle for giving a joint the option of connecting |
---|
26 | to the static environment (i.e. the absolute frame) is to check the second |
---|
27 | body (joint->node[1].body), and if it is zero then behave as if its body |
---|
28 | transform is the identity. |
---|
29 | |
---|
30 | */ |
---|
31 | |
---|
32 | #include <ode/odemath.h> |
---|
33 | #include <ode/rotation.h> |
---|
34 | #include <ode/matrix.h> |
---|
35 | #include "joint.h" |
---|
36 | |
---|
37 | //**************************************************************************** |
---|
38 | // externs |
---|
39 | |
---|
40 | extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); |
---|
41 | extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); |
---|
42 | |
---|
43 | //**************************************************************************** |
---|
44 | // utility |
---|
45 | |
---|
46 | // set three "ball-and-socket" rows in the constraint equation, and the |
---|
47 | // corresponding right hand side. |
---|
48 | |
---|
49 | static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, |
---|
50 | dVector3 anchor1, dVector3 anchor2) |
---|
51 | { |
---|
52 | // anchor points in global coordinates with respect to body PORs. |
---|
53 | dVector3 a1,a2; |
---|
54 | |
---|
55 | int s = info->rowskip; |
---|
56 | |
---|
57 | // set jacobian |
---|
58 | info->J1l[0] = 1; |
---|
59 | info->J1l[s+1] = 1; |
---|
60 | info->J1l[2*s+2] = 1; |
---|
61 | dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); |
---|
62 | dCROSSMAT (info->J1a,a1,s,-,+); |
---|
63 | if (joint->node[1].body) { |
---|
64 | info->J2l[0] = -1; |
---|
65 | info->J2l[s+1] = -1; |
---|
66 | info->J2l[2*s+2] = -1; |
---|
67 | dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); |
---|
68 | dCROSSMAT (info->J2a,a2,s,+,-); |
---|
69 | } |
---|
70 | |
---|
71 | // set right hand side |
---|
72 | dReal k = info->fps * info->erp; |
---|
73 | if (joint->node[1].body) { |
---|
74 | for (int j=0; j<3; j++) { |
---|
75 | info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] - |
---|
76 | a1[j] - joint->node[0].body->pos[j]); |
---|
77 | } |
---|
78 | } |
---|
79 | else { |
---|
80 | for (int j=0; j<3; j++) { |
---|
81 | info->c[j] = k * (anchor2[j] - a1[j] - |
---|
82 | joint->node[0].body->pos[j]); |
---|
83 | } |
---|
84 | } |
---|
85 | } |
---|
86 | |
---|
87 | |
---|
88 | // this is like setBall(), except that `axis' is a unit length vector |
---|
89 | // (in global coordinates) that should be used for the first jacobian |
---|
90 | // position row (the other two row vectors will be derived from this). |
---|
91 | // `erp1' is the erp value to use along the axis. |
---|
92 | |
---|
93 | static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, |
---|
94 | dVector3 anchor1, dVector3 anchor2, |
---|
95 | dVector3 axis, dReal erp1) |
---|
96 | { |
---|
97 | // anchor points in global coordinates with respect to body PORs. |
---|
98 | dVector3 a1,a2; |
---|
99 | |
---|
100 | int i,s = info->rowskip; |
---|
101 | |
---|
102 | // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], |
---|
103 | // [0 1 0] and [0 0 1], which makes everything much easier. |
---|
104 | dVector3 q1,q2; |
---|
105 | dPlaneSpace (axis,q1,q2); |
---|
106 | |
---|
107 | // set jacobian |
---|
108 | for (i=0; i<3; i++) info->J1l[i] = axis[i]; |
---|
109 | for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; |
---|
110 | for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; |
---|
111 | dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); |
---|
112 | dCROSS (info->J1a,=,a1,axis); |
---|
113 | dCROSS (info->J1a+s,=,a1,q1); |
---|
114 | dCROSS (info->J1a+2*s,=,a1,q2); |
---|
115 | if (joint->node[1].body) { |
---|
116 | for (i=0; i<3; i++) info->J2l[i] = -axis[i]; |
---|
117 | for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; |
---|
118 | for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; |
---|
119 | dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); |
---|
120 | dCROSS (info->J2a,= -,a2,axis); |
---|
121 | dCROSS (info->J2a+s,= -,a2,q1); |
---|
122 | dCROSS (info->J2a+2*s,= -,a2,q2); |
---|
123 | } |
---|
124 | |
---|
125 | // set right hand side - measure error along (axis,q1,q2) |
---|
126 | dReal k1 = info->fps * erp1; |
---|
127 | dReal k = info->fps * info->erp; |
---|
128 | |
---|
129 | for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; |
---|
130 | if (joint->node[1].body) { |
---|
131 | for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; |
---|
132 | info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); |
---|
133 | info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); |
---|
134 | info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); |
---|
135 | } |
---|
136 | else { |
---|
137 | info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); |
---|
138 | info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); |
---|
139 | info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); |
---|
140 | } |
---|
141 | } |
---|
142 | |
---|
143 | |
---|
144 | // set three orientation rows in the constraint equation, and the |
---|
145 | // corresponding right hand side. |
---|
146 | |
---|
147 | static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) |
---|
148 | { |
---|
149 | int s = info->rowskip; |
---|
150 | int start_index = start_row * s; |
---|
151 | |
---|
152 | // 3 rows to make body rotations equal |
---|
153 | info->J1a[start_index] = 1; |
---|
154 | info->J1a[start_index + s + 1] = 1; |
---|
155 | info->J1a[start_index + s*2+2] = 1; |
---|
156 | if (joint->node[1].body) { |
---|
157 | info->J2a[start_index] = -1; |
---|
158 | info->J2a[start_index + s+1] = -1; |
---|
159 | info->J2a[start_index + s*2+2] = -1; |
---|
160 | } |
---|
161 | |
---|
162 | // compute the right hand side. the first three elements will result in |
---|
163 | // relative angular velocity of the two bodies - this is set to bring them |
---|
164 | // back into alignment. the correcting angular velocity is |
---|
165 | // |angular_velocity| = angle/time = erp*theta / stepsize |
---|
166 | // = (erp*fps) * theta |
---|
167 | // angular_velocity = |angular_velocity| * u |
---|
168 | // = (erp*fps) * theta * u |
---|
169 | // where rotation along unit length axis u by theta brings body 2's frame |
---|
170 | // to qrel with respect to body 1's frame. using a small angle approximation |
---|
171 | // for sin(), this gives |
---|
172 | // angular_velocity = (erp*fps) * 2 * v |
---|
173 | // where the quaternion of the relative rotation between the two bodies is |
---|
174 | // q = [cos(theta/2) sin(theta/2)*u] = [s v] |
---|
175 | |
---|
176 | // get qerr = relative rotation (rotation error) between two bodies |
---|
177 | dQuaternion qerr,e; |
---|
178 | if (joint->node[1].body) { |
---|
179 | dQuaternion qq; |
---|
180 | dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); |
---|
181 | dQMultiply2 (qerr,qq,qrel); |
---|
182 | } |
---|
183 | else { |
---|
184 | dQMultiply3 (qerr,joint->node[0].body->q,qrel); |
---|
185 | } |
---|
186 | if (qerr[0] < 0) { |
---|
187 | qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small |
---|
188 | qerr[2] = -qerr[2]; |
---|
189 | qerr[3] = -qerr[3]; |
---|
190 | } |
---|
191 | dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! |
---|
192 | dReal k = info->fps * info->erp; |
---|
193 | info->c[start_row] = 2*k * e[0]; |
---|
194 | info->c[start_row+1] = 2*k * e[1]; |
---|
195 | info->c[start_row+2] = 2*k * e[2]; |
---|
196 | } |
---|
197 | |
---|
198 | |
---|
199 | // compute anchor points relative to bodies |
---|
200 | |
---|
201 | static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, |
---|
202 | dVector3 anchor1, dVector3 anchor2) |
---|
203 | { |
---|
204 | if (j->node[0].body) { |
---|
205 | dReal q[4]; |
---|
206 | q[0] = x - j->node[0].body->pos[0]; |
---|
207 | q[1] = y - j->node[0].body->pos[1]; |
---|
208 | q[2] = z - j->node[0].body->pos[2]; |
---|
209 | q[3] = 0; |
---|
210 | dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); |
---|
211 | if (j->node[1].body) { |
---|
212 | q[0] = x - j->node[1].body->pos[0]; |
---|
213 | q[1] = y - j->node[1].body->pos[1]; |
---|
214 | q[2] = z - j->node[1].body->pos[2]; |
---|
215 | q[3] = 0; |
---|
216 | dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); |
---|
217 | } |
---|
218 | else { |
---|
219 | anchor2[0] = x; |
---|
220 | anchor2[1] = y; |
---|
221 | anchor2[2] = z; |
---|
222 | } |
---|
223 | } |
---|
224 | anchor1[3] = 0; |
---|
225 | anchor2[3] = 0; |
---|
226 | } |
---|
227 | |
---|
228 | |
---|
229 | // compute axes relative to bodies. either axis1 or axis2 can be 0. |
---|
230 | |
---|
231 | static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, |
---|
232 | dVector3 axis1, dVector3 axis2) |
---|
233 | { |
---|
234 | if (j->node[0].body) { |
---|
235 | dReal q[4]; |
---|
236 | q[0] = x; |
---|
237 | q[1] = y; |
---|
238 | q[2] = z; |
---|
239 | q[3] = 0; |
---|
240 | dNormalize3 (q); |
---|
241 | if (axis1) { |
---|
242 | dMULTIPLY1_331 (axis1,j->node[0].body->R,q); |
---|
243 | axis1[3] = 0; |
---|
244 | } |
---|
245 | if (axis2) { |
---|
246 | if (j->node[1].body) { |
---|
247 | dMULTIPLY1_331 (axis2,j->node[1].body->R,q); |
---|
248 | } |
---|
249 | else { |
---|
250 | axis2[0] = x; |
---|
251 | axis2[1] = y; |
---|
252 | axis2[2] = z; |
---|
253 | } |
---|
254 | axis2[3] = 0; |
---|
255 | } |
---|
256 | } |
---|
257 | } |
---|
258 | |
---|
259 | |
---|
260 | static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) |
---|
261 | { |
---|
262 | if (j->node[0].body) { |
---|
263 | dMULTIPLY0_331 (result,j->node[0].body->R,anchor1); |
---|
264 | result[0] += j->node[0].body->pos[0]; |
---|
265 | result[1] += j->node[0].body->pos[1]; |
---|
266 | result[2] += j->node[0].body->pos[2]; |
---|
267 | } |
---|
268 | } |
---|
269 | |
---|
270 | |
---|
271 | static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) |
---|
272 | { |
---|
273 | if (j->node[1].body) { |
---|
274 | dMULTIPLY0_331 (result,j->node[1].body->R,anchor2); |
---|
275 | result[0] += j->node[1].body->pos[0]; |
---|
276 | result[1] += j->node[1].body->pos[1]; |
---|
277 | result[2] += j->node[1].body->pos[2]; |
---|
278 | } |
---|
279 | else { |
---|
280 | result[0] = anchor2[0]; |
---|
281 | result[1] = anchor2[1]; |
---|
282 | result[2] = anchor2[2]; |
---|
283 | } |
---|
284 | } |
---|
285 | |
---|
286 | |
---|
287 | static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) |
---|
288 | { |
---|
289 | if (j->node[0].body) { |
---|
290 | dMULTIPLY0_331 (result,j->node[0].body->R,axis1); |
---|
291 | } |
---|
292 | } |
---|
293 | |
---|
294 | |
---|
295 | static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) |
---|
296 | { |
---|
297 | if (j->node[1].body) { |
---|
298 | dMULTIPLY0_331 (result,j->node[1].body->R,axis2); |
---|
299 | } |
---|
300 | else { |
---|
301 | result[0] = axis2[0]; |
---|
302 | result[1] = axis2[1]; |
---|
303 | result[2] = axis2[2]; |
---|
304 | } |
---|
305 | } |
---|
306 | |
---|
307 | |
---|
308 | static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) |
---|
309 | { |
---|
310 | // the angle between the two bodies is extracted from the quaternion that |
---|
311 | // represents the relative rotation between them. recall that a quaternion |
---|
312 | // q is: |
---|
313 | // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] |
---|
314 | // where s is a scalar and v is a 3-vector. u is a unit length axis and |
---|
315 | // theta is a rotation along that axis. we can get theta/2 by: |
---|
316 | // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) |
---|
317 | // but we can't get sin(theta/2) directly, only its absolute value, i.e.: |
---|
318 | // |v| = |sin(theta/2)| * |u| |
---|
319 | // = |sin(theta/2)| |
---|
320 | // using this value will have a strange effect. recall that there are two |
---|
321 | // quaternion representations of a given rotation, q and -q. typically as |
---|
322 | // a body rotates along the axis it will go through a complete cycle using |
---|
323 | // one representation and then the next cycle will use the other |
---|
324 | // representation. this corresponds to u pointing in the direction of the |
---|
325 | // hinge axis and then in the opposite direction. the result is that theta |
---|
326 | // will appear to go "backwards" every other cycle. here is a fix: if u |
---|
327 | // points "away" from the direction of the hinge (motor) axis (i.e. more |
---|
328 | // than 90 degrees) then use -q instead of q. this represents the same |
---|
329 | // rotation, but results in the cos(theta/2) value being sign inverted. |
---|
330 | |
---|
331 | // extract the angle from the quaternion. cost2 = cos(theta/2), |
---|
332 | // sint2 = |sin(theta/2)| |
---|
333 | dReal cost2 = qrel[0]; |
---|
334 | dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); |
---|
335 | dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions |
---|
336 | (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis |
---|
337 | (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction |
---|
338 | |
---|
339 | // the angle we get will be between 0..2*pi, but we want to return angles |
---|
340 | // between -pi..pi |
---|
341 | if (theta > M_PI) theta -= 2*M_PI; |
---|
342 | |
---|
343 | // the angle we've just extracted has the wrong sign |
---|
344 | theta = -theta; |
---|
345 | |
---|
346 | return theta; |
---|
347 | } |
---|
348 | |
---|
349 | |
---|
350 | // given two bodies (body1,body2), the hinge axis that they are connected by |
---|
351 | // w.r.t. body1 (axis), and the initial relative orientation between them |
---|
352 | // (q_initial), return the relative rotation angle. the initial relative |
---|
353 | // orientation corresponds to an angle of zero. if body2 is 0 then measure the |
---|
354 | // angle between body1 and the static frame. |
---|
355 | // |
---|
356 | // this will not return the correct angle if the bodies rotate along any axis |
---|
357 | // other than the given hinge axis. |
---|
358 | |
---|
359 | static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, |
---|
360 | dQuaternion q_initial) |
---|
361 | { |
---|
362 | // get qrel = relative rotation between the two bodies |
---|
363 | dQuaternion qrel; |
---|
364 | if (body2) { |
---|
365 | dQuaternion qq; |
---|
366 | dQMultiply1 (qq,body1->q,body2->q); |
---|
367 | dQMultiply2 (qrel,qq,q_initial); |
---|
368 | } |
---|
369 | else { |
---|
370 | // pretend body2->q is the identity |
---|
371 | dQMultiply3 (qrel,body1->q,q_initial); |
---|
372 | } |
---|
373 | |
---|
374 | return getHingeAngleFromRelativeQuat (qrel,axis); |
---|
375 | } |
---|
376 | |
---|
377 | //**************************************************************************** |
---|
378 | // dxJointLimitMotor |
---|
379 | |
---|
380 | void dxJointLimitMotor::init (dxWorld *world) |
---|
381 | { |
---|
382 | vel = 0; |
---|
383 | fmax = 0; |
---|
384 | lostop = -dInfinity; |
---|
385 | histop = dInfinity; |
---|
386 | fudge_factor = 1; |
---|
387 | normal_cfm = world->global_cfm; |
---|
388 | stop_erp = world->global_erp; |
---|
389 | stop_cfm = world->global_cfm; |
---|
390 | bounce = 0; |
---|
391 | limit = 0; |
---|
392 | limit_err = 0; |
---|
393 | } |
---|
394 | |
---|
395 | |
---|
396 | void dxJointLimitMotor::set (int num, dReal value) |
---|
397 | { |
---|
398 | switch (num) { |
---|
399 | case dParamLoStop: |
---|
400 | if (value <= histop) lostop = value; |
---|
401 | break; |
---|
402 | case dParamHiStop: |
---|
403 | if (value >= lostop) histop = value; |
---|
404 | break; |
---|
405 | case dParamVel: |
---|
406 | vel = value; |
---|
407 | break; |
---|
408 | case dParamFMax: |
---|
409 | if (value >= 0) fmax = value; |
---|
410 | break; |
---|
411 | case dParamFudgeFactor: |
---|
412 | if (value >= 0 && value <= 1) fudge_factor = value; |
---|
413 | break; |
---|
414 | case dParamBounce: |
---|
415 | bounce = value; |
---|
416 | break; |
---|
417 | case dParamCFM: |
---|
418 | normal_cfm = value; |
---|
419 | break; |
---|
420 | case dParamStopERP: |
---|
421 | stop_erp = value; |
---|
422 | break; |
---|
423 | case dParamStopCFM: |
---|
424 | stop_cfm = value; |
---|
425 | break; |
---|
426 | } |
---|
427 | } |
---|
428 | |
---|
429 | |
---|
430 | dReal dxJointLimitMotor::get (int num) |
---|
431 | { |
---|
432 | switch (num) { |
---|
433 | case dParamLoStop: return lostop; |
---|
434 | case dParamHiStop: return histop; |
---|
435 | case dParamVel: return vel; |
---|
436 | case dParamFMax: return fmax; |
---|
437 | case dParamFudgeFactor: return fudge_factor; |
---|
438 | case dParamBounce: return bounce; |
---|
439 | case dParamCFM: return normal_cfm; |
---|
440 | case dParamStopERP: return stop_erp; |
---|
441 | case dParamStopCFM: return stop_cfm; |
---|
442 | default: return 0; |
---|
443 | } |
---|
444 | } |
---|
445 | |
---|
446 | |
---|
447 | int dxJointLimitMotor::testRotationalLimit (dReal angle) |
---|
448 | { |
---|
449 | if (angle <= lostop) { |
---|
450 | limit = 1; |
---|
451 | limit_err = angle - lostop; |
---|
452 | return 1; |
---|
453 | } |
---|
454 | else if (angle >= histop) { |
---|
455 | limit = 2; |
---|
456 | limit_err = angle - histop; |
---|
457 | return 1; |
---|
458 | } |
---|
459 | else { |
---|
460 | limit = 0; |
---|
461 | return 0; |
---|
462 | } |
---|
463 | } |
---|
464 | |
---|
465 | |
---|
466 | int dxJointLimitMotor::addLimot (dxJoint *joint, |
---|
467 | dxJoint::Info2 *info, int row, |
---|
468 | dVector3 ax1, int rotational) |
---|
469 | { |
---|
470 | int srow = row * info->rowskip; |
---|
471 | |
---|
472 | // if the joint is powered, or has joint limits, add in the extra row |
---|
473 | int powered = fmax > 0; |
---|
474 | if (powered || limit) { |
---|
475 | dReal *J1 = rotational ? info->J1a : info->J1l; |
---|
476 | dReal *J2 = rotational ? info->J2a : info->J2l; |
---|
477 | |
---|
478 | J1[srow+0] = ax1[0]; |
---|
479 | J1[srow+1] = ax1[1]; |
---|
480 | J1[srow+2] = ax1[2]; |
---|
481 | if (joint->node[1].body) { |
---|
482 | J2[srow+0] = -ax1[0]; |
---|
483 | J2[srow+1] = -ax1[1]; |
---|
484 | J2[srow+2] = -ax1[2]; |
---|
485 | } |
---|
486 | |
---|
487 | // linear limot torque decoupling step: |
---|
488 | // |
---|
489 | // if this is a linear limot (e.g. from a slider), we have to be careful |
---|
490 | // that the linear constraint forces (+/- ax1) applied to the two bodies |
---|
491 | // do not create a torque couple. in other words, the points that the |
---|
492 | // constraint force is applied at must lie along the same ax1 axis. |
---|
493 | // a torque couple will result in powered or limited slider-jointed free |
---|
494 | // bodies from gaining angular momentum. |
---|
495 | // the solution used here is to apply the constraint forces at the point |
---|
496 | // halfway between the body centers. there is no penalty (other than an |
---|
497 | // extra tiny bit of computation) in doing this adjustment. note that we |
---|
498 | // only need to do this if the constraint connects two bodies. |
---|
499 | |
---|
500 | dVector3 ltd; // Linear Torque Decoupling vector (a torque) |
---|
501 | if (!rotational && joint->node[1].body) { |
---|
502 | dVector3 c; |
---|
503 | c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]); |
---|
504 | c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]); |
---|
505 | c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]); |
---|
506 | dCROSS (ltd,=,c,ax1); |
---|
507 | info->J1a[srow+0] = ltd[0]; |
---|
508 | info->J1a[srow+1] = ltd[1]; |
---|
509 | info->J1a[srow+2] = ltd[2]; |
---|
510 | info->J2a[srow+0] = ltd[0]; |
---|
511 | info->J2a[srow+1] = ltd[1]; |
---|
512 | info->J2a[srow+2] = ltd[2]; |
---|
513 | } |
---|
514 | |
---|
515 | // if we're limited low and high simultaneously, the joint motor is |
---|
516 | // ineffective |
---|
517 | if (limit && (lostop == histop)) powered = 0; |
---|
518 | |
---|
519 | if (powered) { |
---|
520 | info->cfm[row] = normal_cfm; |
---|
521 | if (! limit) { |
---|
522 | info->c[row] = vel; |
---|
523 | info->lo[row] = -fmax; |
---|
524 | info->hi[row] = fmax; |
---|
525 | } |
---|
526 | else { |
---|
527 | // the joint is at a limit, AND is being powered. if the joint is |
---|
528 | // being powered into the limit then we apply the maximum motor force |
---|
529 | // in that direction, because the motor is working against the |
---|
530 | // immovable limit. if the joint is being powered away from the limit |
---|
531 | // then we have problems because actually we need *two* lcp |
---|
532 | // constraints to handle this case. so we fake it and apply some |
---|
533 | // fraction of the maximum force. the fraction to use can be set as |
---|
534 | // a fudge factor. |
---|
535 | |
---|
536 | dReal fm = fmax; |
---|
537 | if (vel > 0) fm = -fm; |
---|
538 | |
---|
539 | // if we're powering away from the limit, apply the fudge factor |
---|
540 | if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; |
---|
541 | |
---|
542 | if (rotational) { |
---|
543 | dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], |
---|
544 | -fm*ax1[2]); |
---|
545 | if (joint->node[1].body) |
---|
546 | dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); |
---|
547 | } |
---|
548 | else { |
---|
549 | dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); |
---|
550 | if (joint->node[1].body) { |
---|
551 | dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); |
---|
552 | |
---|
553 | // linear limot torque decoupling step: refer to above discussion |
---|
554 | dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], |
---|
555 | -fm*ltd[2]); |
---|
556 | dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], |
---|
557 | -fm*ltd[2]); |
---|
558 | } |
---|
559 | } |
---|
560 | } |
---|
561 | } |
---|
562 | |
---|
563 | if (limit) { |
---|
564 | dReal k = info->fps * stop_erp; |
---|
565 | info->c[row] = -k * limit_err; |
---|
566 | info->cfm[row] = stop_cfm; |
---|
567 | |
---|
568 | if (lostop == histop) { |
---|
569 | // limited low and high simultaneously |
---|
570 | info->lo[row] = -dInfinity; |
---|
571 | info->hi[row] = dInfinity; |
---|
572 | } |
---|
573 | else { |
---|
574 | if (limit == 1) { |
---|
575 | // low limit |
---|
576 | info->lo[row] = 0; |
---|
577 | info->hi[row] = dInfinity; |
---|
578 | } |
---|
579 | else { |
---|
580 | // high limit |
---|
581 | info->lo[row] = -dInfinity; |
---|
582 | info->hi[row] = 0; |
---|
583 | } |
---|
584 | |
---|
585 | // deal with bounce |
---|
586 | if (bounce > 0) { |
---|
587 | // calculate joint velocity |
---|
588 | dReal vel; |
---|
589 | if (rotational) { |
---|
590 | vel = dDOT(joint->node[0].body->avel,ax1); |
---|
591 | if (joint->node[1].body) |
---|
592 | vel -= dDOT(joint->node[1].body->avel,ax1); |
---|
593 | } |
---|
594 | else { |
---|
595 | vel = dDOT(joint->node[0].body->lvel,ax1); |
---|
596 | if (joint->node[1].body) |
---|
597 | vel -= dDOT(joint->node[1].body->lvel,ax1); |
---|
598 | } |
---|
599 | |
---|
600 | // only apply bounce if the velocity is incoming, and if the |
---|
601 | // resulting c[] exceeds what we already have. |
---|
602 | if (limit == 1) { |
---|
603 | // low limit |
---|
604 | if (vel < 0) { |
---|
605 | dReal newc = -bounce * vel; |
---|
606 | if (newc > info->c[row]) info->c[row] = newc; |
---|
607 | } |
---|
608 | } |
---|
609 | else { |
---|
610 | // high limit - all those computations are reversed |
---|
611 | if (vel > 0) { |
---|
612 | dReal newc = -bounce * vel; |
---|
613 | if (newc < info->c[row]) info->c[row] = newc; |
---|
614 | } |
---|
615 | } |
---|
616 | } |
---|
617 | } |
---|
618 | } |
---|
619 | return 1; |
---|
620 | } |
---|
621 | else return 0; |
---|
622 | } |
---|
623 | |
---|
624 | //**************************************************************************** |
---|
625 | // ball and socket |
---|
626 | |
---|
627 | static void ballInit (dxJointBall *j) |
---|
628 | { |
---|
629 | dSetZero (j->anchor1,4); |
---|
630 | dSetZero (j->anchor2,4); |
---|
631 | } |
---|
632 | |
---|
633 | |
---|
634 | static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) |
---|
635 | { |
---|
636 | info->m = 3; |
---|
637 | info->nub = 3; |
---|
638 | } |
---|
639 | |
---|
640 | |
---|
641 | static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) |
---|
642 | { |
---|
643 | setBall (joint,info,joint->anchor1,joint->anchor2); |
---|
644 | } |
---|
645 | |
---|
646 | |
---|
647 | extern "C" void dJointSetBallAnchor (dxJointBall *joint, |
---|
648 | dReal x, dReal y, dReal z) |
---|
649 | { |
---|
650 | dUASSERT(joint,"bad joint argument"); |
---|
651 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
652 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
653 | } |
---|
654 | |
---|
655 | |
---|
656 | extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result) |
---|
657 | { |
---|
658 | dUASSERT(joint,"bad joint argument"); |
---|
659 | dUASSERT(result,"bad result argument"); |
---|
660 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
661 | if (joint->flags & dJOINT_REVERSE) |
---|
662 | getAnchor2 (joint,result,joint->anchor2); |
---|
663 | else |
---|
664 | getAnchor (joint,result,joint->anchor1); |
---|
665 | } |
---|
666 | |
---|
667 | |
---|
668 | extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result) |
---|
669 | { |
---|
670 | dUASSERT(joint,"bad joint argument"); |
---|
671 | dUASSERT(result,"bad result argument"); |
---|
672 | dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); |
---|
673 | if (joint->flags & dJOINT_REVERSE) |
---|
674 | getAnchor (joint,result,joint->anchor1); |
---|
675 | else |
---|
676 | getAnchor2 (joint,result,joint->anchor2); |
---|
677 | } |
---|
678 | |
---|
679 | |
---|
680 | dxJoint::Vtable __dball_vtable = { |
---|
681 | sizeof(dxJointBall), |
---|
682 | (dxJoint::init_fn*) ballInit, |
---|
683 | (dxJoint::getInfo1_fn*) ballGetInfo1, |
---|
684 | (dxJoint::getInfo2_fn*) ballGetInfo2, |
---|
685 | dJointTypeBall}; |
---|
686 | |
---|
687 | //**************************************************************************** |
---|
688 | // hinge |
---|
689 | |
---|
690 | static void hingeInit (dxJointHinge *j) |
---|
691 | { |
---|
692 | dSetZero (j->anchor1,4); |
---|
693 | dSetZero (j->anchor2,4); |
---|
694 | dSetZero (j->axis1,4); |
---|
695 | j->axis1[0] = 1; |
---|
696 | dSetZero (j->axis2,4); |
---|
697 | j->axis2[0] = 1; |
---|
698 | dSetZero (j->qrel,4); |
---|
699 | j->limot.init (j->world); |
---|
700 | } |
---|
701 | |
---|
702 | |
---|
703 | static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) |
---|
704 | { |
---|
705 | info->nub = 5; |
---|
706 | |
---|
707 | // see if joint is powered |
---|
708 | if (j->limot.fmax > 0) |
---|
709 | info->m = 6; // powered hinge needs an extra constraint row |
---|
710 | else info->m = 5; |
---|
711 | |
---|
712 | // see if we're at a joint limit. |
---|
713 | if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && |
---|
714 | j->limot.lostop <= j->limot.histop) { |
---|
715 | dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, |
---|
716 | j->qrel); |
---|
717 | if (j->limot.testRotationalLimit (angle)) info->m = 6; |
---|
718 | } |
---|
719 | } |
---|
720 | |
---|
721 | |
---|
722 | static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) |
---|
723 | { |
---|
724 | // set the three ball-and-socket rows |
---|
725 | setBall (joint,info,joint->anchor1,joint->anchor2); |
---|
726 | |
---|
727 | // set the two hinge rows. the hinge axis should be the only unconstrained |
---|
728 | // rotational axis, the angular velocity of the two bodies perpendicular to |
---|
729 | // the hinge axis should be equal. thus the constraint equations are |
---|
730 | // p*w1 - p*w2 = 0 |
---|
731 | // q*w1 - q*w2 = 0 |
---|
732 | // where p and q are unit vectors normal to the hinge axis, and w1 and w2 |
---|
733 | // are the angular velocity vectors of the two bodies. |
---|
734 | |
---|
735 | dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body |
---|
736 | dVector3 p,q; // plane space vectors for ax1 |
---|
737 | dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); |
---|
738 | dPlaneSpace (ax1,p,q); |
---|
739 | |
---|
740 | int s3=3*info->rowskip; |
---|
741 | int s4=4*info->rowskip; |
---|
742 | |
---|
743 | info->J1a[s3+0] = p[0]; |
---|
744 | info->J1a[s3+1] = p[1]; |
---|
745 | info->J1a[s3+2] = p[2]; |
---|
746 | info->J1a[s4+0] = q[0]; |
---|
747 | info->J1a[s4+1] = q[1]; |
---|
748 | info->J1a[s4+2] = q[2]; |
---|
749 | |
---|
750 | if (joint->node[1].body) { |
---|
751 | info->J2a[s3+0] = -p[0]; |
---|
752 | info->J2a[s3+1] = -p[1]; |
---|
753 | info->J2a[s3+2] = -p[2]; |
---|
754 | info->J2a[s4+0] = -q[0]; |
---|
755 | info->J2a[s4+1] = -q[1]; |
---|
756 | info->J2a[s4+2] = -q[2]; |
---|
757 | } |
---|
758 | |
---|
759 | // compute the right hand side of the constraint equation. set relative |
---|
760 | // body velocities along p and q to bring the hinge back into alignment. |
---|
761 | // if ax1,ax2 are the unit length hinge axes as computed from body1 and |
---|
762 | // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). |
---|
763 | // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
---|
764 | // along u to cover angle erp*theta in one step : |
---|
765 | // |angular_velocity| = angle/time = erp*theta / stepsize |
---|
766 | // = (erp*fps) * theta |
---|
767 | // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
---|
768 | // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
---|
769 | // ...as ax1 and ax2 are unit length. if theta is smallish, |
---|
770 | // theta ~= sin(theta), so |
---|
771 | // angular_velocity = (erp*fps) * (ax1 x ax2) |
---|
772 | // ax1 x ax2 is in the plane space of ax1, so we project the angular |
---|
773 | // velocity to p and q to find the right hand side. |
---|
774 | |
---|
775 | dVector3 ax2,b; |
---|
776 | if (joint->node[1].body) { |
---|
777 | dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); |
---|
778 | } |
---|
779 | else { |
---|
780 | ax2[0] = joint->axis2[0]; |
---|
781 | ax2[1] = joint->axis2[1]; |
---|
782 | ax2[2] = joint->axis2[2]; |
---|
783 | } |
---|
784 | dCROSS (b,=,ax1,ax2); |
---|
785 | dReal k = info->fps * info->erp; |
---|
786 | info->c[3] = k * dDOT(b,p); |
---|
787 | info->c[4] = k * dDOT(b,q); |
---|
788 | |
---|
789 | // if the hinge is powered, or has joint limits, add in the stuff |
---|
790 | joint->limot.addLimot (joint,info,5,ax1,1); |
---|
791 | } |
---|
792 | |
---|
793 | |
---|
794 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
795 | |
---|
796 | static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) |
---|
797 | { |
---|
798 | if (joint->node[0].body) { |
---|
799 | if (joint->node[1].body) { |
---|
800 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
801 | } |
---|
802 | else { |
---|
803 | // set joint->qrel to the transpose of the first body q |
---|
804 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
805 | for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
806 | } |
---|
807 | } |
---|
808 | } |
---|
809 | |
---|
810 | |
---|
811 | extern "C" void dJointSetHingeAnchor (dxJointHinge *joint, |
---|
812 | dReal x, dReal y, dReal z) |
---|
813 | { |
---|
814 | dUASSERT(joint,"bad joint argument"); |
---|
815 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
816 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
817 | hingeComputeInitialRelativeRotation (joint); |
---|
818 | } |
---|
819 | |
---|
820 | |
---|
821 | extern "C" void dJointSetHingeAxis (dxJointHinge *joint, |
---|
822 | dReal x, dReal y, dReal z) |
---|
823 | { |
---|
824 | dUASSERT(joint,"bad joint argument"); |
---|
825 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
826 | setAxes (joint,x,y,z,joint->axis1,joint->axis2); |
---|
827 | hingeComputeInitialRelativeRotation (joint); |
---|
828 | } |
---|
829 | |
---|
830 | |
---|
831 | extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result) |
---|
832 | { |
---|
833 | dUASSERT(joint,"bad joint argument"); |
---|
834 | dUASSERT(result,"bad result argument"); |
---|
835 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
836 | if (joint->flags & dJOINT_REVERSE) |
---|
837 | getAnchor2 (joint,result,joint->anchor2); |
---|
838 | else |
---|
839 | getAnchor (joint,result,joint->anchor1); |
---|
840 | } |
---|
841 | |
---|
842 | |
---|
843 | extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result) |
---|
844 | { |
---|
845 | dUASSERT(joint,"bad joint argument"); |
---|
846 | dUASSERT(result,"bad result argument"); |
---|
847 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
848 | if (joint->flags & dJOINT_REVERSE) |
---|
849 | getAnchor (joint,result,joint->anchor1); |
---|
850 | else |
---|
851 | getAnchor2 (joint,result,joint->anchor2); |
---|
852 | } |
---|
853 | |
---|
854 | |
---|
855 | extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result) |
---|
856 | { |
---|
857 | dUASSERT(joint,"bad joint argument"); |
---|
858 | dUASSERT(result,"bad result argument"); |
---|
859 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
860 | getAxis (joint,result,joint->axis1); |
---|
861 | } |
---|
862 | |
---|
863 | |
---|
864 | extern "C" void dJointSetHingeParam (dxJointHinge *joint, |
---|
865 | int parameter, dReal value) |
---|
866 | { |
---|
867 | dUASSERT(joint,"bad joint argument"); |
---|
868 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
869 | joint->limot.set (parameter,value); |
---|
870 | } |
---|
871 | |
---|
872 | |
---|
873 | extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter) |
---|
874 | { |
---|
875 | dUASSERT(joint,"bad joint argument"); |
---|
876 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
877 | return joint->limot.get (parameter); |
---|
878 | } |
---|
879 | |
---|
880 | |
---|
881 | extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint) |
---|
882 | { |
---|
883 | dAASSERT(joint); |
---|
884 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); |
---|
885 | if (joint->node[0].body) { |
---|
886 | dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, |
---|
887 | joint->qrel); |
---|
888 | if (joint->flags & dJOINT_REVERSE) |
---|
889 | return -ang; |
---|
890 | else |
---|
891 | return ang; |
---|
892 | } |
---|
893 | else return 0; |
---|
894 | } |
---|
895 | |
---|
896 | |
---|
897 | extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) |
---|
898 | { |
---|
899 | dAASSERT(joint); |
---|
900 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); |
---|
901 | if (joint->node[0].body) { |
---|
902 | dVector3 axis; |
---|
903 | dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); |
---|
904 | dReal rate = dDOT(axis,joint->node[0].body->avel); |
---|
905 | if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
---|
906 | if (joint->flags & dJOINT_REVERSE) rate = - rate; |
---|
907 | return rate; |
---|
908 | } |
---|
909 | else return 0; |
---|
910 | } |
---|
911 | |
---|
912 | |
---|
913 | extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque) |
---|
914 | { |
---|
915 | dVector3 axis; |
---|
916 | dAASSERT(joint); |
---|
917 | dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); |
---|
918 | |
---|
919 | if (joint->flags & dJOINT_REVERSE) |
---|
920 | torque = -torque; |
---|
921 | |
---|
922 | getAxis (joint,axis,joint->axis1); |
---|
923 | axis[0] *= torque; |
---|
924 | axis[1] *= torque; |
---|
925 | axis[2] *= torque; |
---|
926 | |
---|
927 | if (joint->node[0].body != 0) |
---|
928 | dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); |
---|
929 | if (joint->node[1].body != 0) |
---|
930 | dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
---|
931 | } |
---|
932 | |
---|
933 | |
---|
934 | dxJoint::Vtable __dhinge_vtable = { |
---|
935 | sizeof(dxJointHinge), |
---|
936 | (dxJoint::init_fn*) hingeInit, |
---|
937 | (dxJoint::getInfo1_fn*) hingeGetInfo1, |
---|
938 | (dxJoint::getInfo2_fn*) hingeGetInfo2, |
---|
939 | dJointTypeHinge}; |
---|
940 | |
---|
941 | //**************************************************************************** |
---|
942 | // slider |
---|
943 | |
---|
944 | static void sliderInit (dxJointSlider *j) |
---|
945 | { |
---|
946 | dSetZero (j->axis1,4); |
---|
947 | j->axis1[0] = 1; |
---|
948 | dSetZero (j->qrel,4); |
---|
949 | dSetZero (j->offset,4); |
---|
950 | j->limot.init (j->world); |
---|
951 | } |
---|
952 | |
---|
953 | |
---|
954 | extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint) |
---|
955 | { |
---|
956 | dUASSERT(joint,"bad joint argument"); |
---|
957 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
958 | |
---|
959 | // get axis1 in global coordinates |
---|
960 | dVector3 ax1,q; |
---|
961 | dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); |
---|
962 | |
---|
963 | if (joint->node[1].body) { |
---|
964 | // get body2 + offset point in global coordinates |
---|
965 | dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset); |
---|
966 | for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - |
---|
967 | joint->node[1].body->pos[i]; |
---|
968 | } |
---|
969 | else { |
---|
970 | for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - |
---|
971 | joint->offset[i]; |
---|
972 | |
---|
973 | } |
---|
974 | return dDOT(ax1,q); |
---|
975 | } |
---|
976 | |
---|
977 | |
---|
978 | extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) |
---|
979 | { |
---|
980 | dUASSERT(joint,"bad joint argument"); |
---|
981 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
982 | |
---|
983 | // get axis1 in global coordinates |
---|
984 | dVector3 ax1; |
---|
985 | dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); |
---|
986 | |
---|
987 | if (joint->node[1].body) { |
---|
988 | return dDOT(ax1,joint->node[0].body->lvel) - |
---|
989 | dDOT(ax1,joint->node[1].body->lvel); |
---|
990 | } |
---|
991 | else { |
---|
992 | return dDOT(ax1,joint->node[0].body->lvel); |
---|
993 | } |
---|
994 | } |
---|
995 | |
---|
996 | |
---|
997 | static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) |
---|
998 | { |
---|
999 | info->nub = 5; |
---|
1000 | |
---|
1001 | // see if joint is powered |
---|
1002 | if (j->limot.fmax > 0) |
---|
1003 | info->m = 6; // powered slider needs an extra constraint row |
---|
1004 | else info->m = 5; |
---|
1005 | |
---|
1006 | // see if we're at a joint limit. |
---|
1007 | j->limot.limit = 0; |
---|
1008 | if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && |
---|
1009 | j->limot.lostop <= j->limot.histop) { |
---|
1010 | // measure joint position |
---|
1011 | dReal pos = dJointGetSliderPosition (j); |
---|
1012 | if (pos <= j->limot.lostop) { |
---|
1013 | j->limot.limit = 1; |
---|
1014 | j->limot.limit_err = pos - j->limot.lostop; |
---|
1015 | info->m = 6; |
---|
1016 | } |
---|
1017 | else if (pos >= j->limot.histop) { |
---|
1018 | j->limot.limit = 2; |
---|
1019 | j->limot.limit_err = pos - j->limot.histop; |
---|
1020 | info->m = 6; |
---|
1021 | } |
---|
1022 | } |
---|
1023 | } |
---|
1024 | |
---|
1025 | |
---|
1026 | static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) |
---|
1027 | { |
---|
1028 | int i,s = info->rowskip; |
---|
1029 | int s2=2*s,s3=3*s,s4=4*s; |
---|
1030 | |
---|
1031 | // pull out pos and R for both bodies. also get the `connection' |
---|
1032 | // vector pos2-pos1. |
---|
1033 | |
---|
1034 | dReal *pos1,*pos2,*R1,*R2; |
---|
1035 | dVector3 c; |
---|
1036 | pos1 = joint->node[0].body->pos; |
---|
1037 | R1 = joint->node[0].body->R; |
---|
1038 | if (joint->node[1].body) { |
---|
1039 | pos2 = joint->node[1].body->pos; |
---|
1040 | R2 = joint->node[1].body->R; |
---|
1041 | for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; |
---|
1042 | } |
---|
1043 | else { |
---|
1044 | pos2 = 0; |
---|
1045 | R2 = 0; |
---|
1046 | } |
---|
1047 | |
---|
1048 | // 3 rows to make body rotations equal |
---|
1049 | setFixedOrientation(joint, info, joint->qrel, 0); |
---|
1050 | |
---|
1051 | // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would |
---|
1052 | // result in three equations, so we project along the planespace vectors |
---|
1053 | // so that sliding along the slider axis is disregarded. for symmetry we |
---|
1054 | // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. |
---|
1055 | |
---|
1056 | dVector3 ax1; // joint axis in global coordinates (unit length) |
---|
1057 | dVector3 p,q; // plane space of ax1 |
---|
1058 | dMULTIPLY0_331 (ax1,R1,joint->axis1); |
---|
1059 | dPlaneSpace (ax1,p,q); |
---|
1060 | if (joint->node[1].body) { |
---|
1061 | dVector3 tmp; |
---|
1062 | dCROSS (tmp, = REAL(0.5) * ,c,p); |
---|
1063 | for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; |
---|
1064 | for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; |
---|
1065 | dCROSS (tmp, = REAL(0.5) * ,c,q); |
---|
1066 | for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; |
---|
1067 | for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; |
---|
1068 | for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; |
---|
1069 | for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; |
---|
1070 | } |
---|
1071 | for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; |
---|
1072 | for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; |
---|
1073 | |
---|
1074 | // compute last two elements of right hand side. we want to align the offset |
---|
1075 | // point (in body 2's frame) with the center of body 1. |
---|
1076 | dReal k = info->fps * info->erp; |
---|
1077 | if (joint->node[1].body) { |
---|
1078 | dVector3 ofs; // offset point in global coordinates |
---|
1079 | dMULTIPLY0_331 (ofs,R2,joint->offset); |
---|
1080 | for (i=0; i<3; i++) c[i] += ofs[i]; |
---|
1081 | info->c[3] = k * dDOT(p,c); |
---|
1082 | info->c[4] = k * dDOT(q,c); |
---|
1083 | } |
---|
1084 | else { |
---|
1085 | dVector3 ofs; // offset point in global coordinates |
---|
1086 | for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; |
---|
1087 | info->c[3] = k * dDOT(p,ofs); |
---|
1088 | info->c[4] = k * dDOT(q,ofs); |
---|
1089 | } |
---|
1090 | |
---|
1091 | // if the slider is powered, or has joint limits, add in the extra row |
---|
1092 | joint->limot.addLimot (joint,info,5,ax1,0); |
---|
1093 | } |
---|
1094 | |
---|
1095 | |
---|
1096 | extern "C" void dJointSetSliderAxis (dxJointSlider *joint, |
---|
1097 | dReal x, dReal y, dReal z) |
---|
1098 | { |
---|
1099 | int i; |
---|
1100 | dUASSERT(joint,"bad joint argument"); |
---|
1101 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1102 | setAxes (joint,x,y,z,joint->axis1,0); |
---|
1103 | |
---|
1104 | // compute initial relative rotation body1 -> body2, or env -> body1 |
---|
1105 | // also compute center of body1 w.r.t body 2 |
---|
1106 | if (joint->node[1].body) { |
---|
1107 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
1108 | dVector3 c; |
---|
1109 | for (i=0; i<3; i++) |
---|
1110 | c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; |
---|
1111 | dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); |
---|
1112 | } |
---|
1113 | else { |
---|
1114 | // set joint->qrel to the transpose of the first body's q |
---|
1115 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
1116 | for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
1117 | for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i]; |
---|
1118 | } |
---|
1119 | } |
---|
1120 | |
---|
1121 | |
---|
1122 | extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result) |
---|
1123 | { |
---|
1124 | dUASSERT(joint,"bad joint argument"); |
---|
1125 | dUASSERT(result,"bad result argument"); |
---|
1126 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1127 | getAxis (joint,result,joint->axis1); |
---|
1128 | } |
---|
1129 | |
---|
1130 | |
---|
1131 | extern "C" void dJointSetSliderParam (dxJointSlider *joint, |
---|
1132 | int parameter, dReal value) |
---|
1133 | { |
---|
1134 | dUASSERT(joint,"bad joint argument"); |
---|
1135 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1136 | joint->limot.set (parameter,value); |
---|
1137 | } |
---|
1138 | |
---|
1139 | |
---|
1140 | extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter) |
---|
1141 | { |
---|
1142 | dUASSERT(joint,"bad joint argument"); |
---|
1143 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1144 | return joint->limot.get (parameter); |
---|
1145 | } |
---|
1146 | |
---|
1147 | |
---|
1148 | extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force) |
---|
1149 | { |
---|
1150 | dVector3 axis; |
---|
1151 | dUASSERT(joint,"bad joint argument"); |
---|
1152 | dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); |
---|
1153 | |
---|
1154 | if (joint->flags & dJOINT_REVERSE) |
---|
1155 | force -= force; |
---|
1156 | |
---|
1157 | getAxis (joint,axis,joint->axis1); |
---|
1158 | axis[0] *= force; |
---|
1159 | axis[1] *= force; |
---|
1160 | axis[2] *= force; |
---|
1161 | |
---|
1162 | if (joint->node[0].body != 0) |
---|
1163 | dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); |
---|
1164 | if (joint->node[1].body != 0) |
---|
1165 | dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
---|
1166 | } |
---|
1167 | |
---|
1168 | |
---|
1169 | dxJoint::Vtable __dslider_vtable = { |
---|
1170 | sizeof(dxJointSlider), |
---|
1171 | (dxJoint::init_fn*) sliderInit, |
---|
1172 | (dxJoint::getInfo1_fn*) sliderGetInfo1, |
---|
1173 | (dxJoint::getInfo2_fn*) sliderGetInfo2, |
---|
1174 | dJointTypeSlider}; |
---|
1175 | |
---|
1176 | //**************************************************************************** |
---|
1177 | // contact |
---|
1178 | |
---|
1179 | static void contactInit (dxJointContact *j) |
---|
1180 | { |
---|
1181 | // default frictionless contact. hmmm, this info gets overwritten straight |
---|
1182 | // away anyway, so why bother? |
---|
1183 | #if 0 /* so don't bother ;) */ |
---|
1184 | j->contact.surface.mode = 0; |
---|
1185 | j->contact.surface.mu = 0; |
---|
1186 | dSetZero (j->contact.geom.pos,4); |
---|
1187 | dSetZero (j->contact.geom.normal,4); |
---|
1188 | j->contact.geom.depth = 0; |
---|
1189 | #endif |
---|
1190 | } |
---|
1191 | |
---|
1192 | |
---|
1193 | static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) |
---|
1194 | { |
---|
1195 | // make sure mu's >= 0, then calculate number of constraint rows and number |
---|
1196 | // of unbounded rows. |
---|
1197 | int m = 1, nub=0; |
---|
1198 | if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; |
---|
1199 | if (j->contact.surface.mode & dContactMu2) { |
---|
1200 | if (j->contact.surface.mu > 0) m++; |
---|
1201 | if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; |
---|
1202 | if (j->contact.surface.mu2 > 0) m++; |
---|
1203 | if (j->contact.surface.mu == dInfinity) nub ++; |
---|
1204 | if (j->contact.surface.mu2 == dInfinity) nub ++; |
---|
1205 | } |
---|
1206 | else { |
---|
1207 | if (j->contact.surface.mu > 0) m += 2; |
---|
1208 | if (j->contact.surface.mu == dInfinity) nub += 2; |
---|
1209 | } |
---|
1210 | |
---|
1211 | j->the_m = m; |
---|
1212 | info->m = m; |
---|
1213 | info->nub = nub; |
---|
1214 | } |
---|
1215 | |
---|
1216 | |
---|
1217 | static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) |
---|
1218 | { |
---|
1219 | int i,s = info->rowskip; |
---|
1220 | int s2 = 2*s; |
---|
1221 | |
---|
1222 | // get normal, with sign adjusted for body1/body2 polarity |
---|
1223 | dVector3 normal; |
---|
1224 | if (j->flags & dJOINT_REVERSE) { |
---|
1225 | normal[0] = - j->contact.geom.normal[0]; |
---|
1226 | normal[1] = - j->contact.geom.normal[1]; |
---|
1227 | normal[2] = - j->contact.geom.normal[2]; |
---|
1228 | } |
---|
1229 | else { |
---|
1230 | normal[0] = j->contact.geom.normal[0]; |
---|
1231 | normal[1] = j->contact.geom.normal[1]; |
---|
1232 | normal[2] = j->contact.geom.normal[2]; |
---|
1233 | } |
---|
1234 | normal[3] = 0; // @@@ hmmm |
---|
1235 | |
---|
1236 | // c1,c2 = contact points with respect to body PORs |
---|
1237 | dVector3 c1,c2; |
---|
1238 | for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; |
---|
1239 | |
---|
1240 | // set jacobian for normal |
---|
1241 | info->J1l[0] = normal[0]; |
---|
1242 | info->J1l[1] = normal[1]; |
---|
1243 | info->J1l[2] = normal[2]; |
---|
1244 | dCROSS (info->J1a,=,c1,normal); |
---|
1245 | if (j->node[1].body) { |
---|
1246 | for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - |
---|
1247 | j->node[1].body->pos[i]; |
---|
1248 | info->J2l[0] = -normal[0]; |
---|
1249 | info->J2l[1] = -normal[1]; |
---|
1250 | info->J2l[2] = -normal[2]; |
---|
1251 | dCROSS (info->J2a,= -,c2,normal); |
---|
1252 | } |
---|
1253 | |
---|
1254 | // set right hand side and cfm value for normal |
---|
1255 | dReal erp = info->erp; |
---|
1256 | if (j->contact.surface.mode & dContactSoftERP) |
---|
1257 | erp = j->contact.surface.soft_erp; |
---|
1258 | dReal k = info->fps * erp; |
---|
1259 | info->c[0] = k*j->contact.geom.depth; |
---|
1260 | if (j->contact.surface.mode & dContactSoftCFM) |
---|
1261 | info->cfm[0] = j->contact.surface.soft_cfm; |
---|
1262 | |
---|
1263 | // deal with bounce |
---|
1264 | if (j->contact.surface.mode & dContactBounce) { |
---|
1265 | // calculate outgoing velocity (-ve for incoming contact) |
---|
1266 | dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + |
---|
1267 | dDOT(info->J1a,j->node[0].body->avel); |
---|
1268 | if (j->node[1].body) { |
---|
1269 | outgoing += dDOT(info->J2l,j->node[1].body->lvel) + |
---|
1270 | dDOT(info->J2a,j->node[1].body->avel); |
---|
1271 | } |
---|
1272 | // only apply bounce if the outgoing velocity is greater than the |
---|
1273 | // threshold, and if the resulting c[0] exceeds what we already have. |
---|
1274 | if (j->contact.surface.bounce_vel >= 0 && |
---|
1275 | (-outgoing) > j->contact.surface.bounce_vel) { |
---|
1276 | dReal newc = - j->contact.surface.bounce * outgoing; |
---|
1277 | if (newc > info->c[0]) info->c[0] = newc; |
---|
1278 | } |
---|
1279 | } |
---|
1280 | |
---|
1281 | // set LCP limits for normal |
---|
1282 | info->lo[0] = 0; |
---|
1283 | info->hi[0] = dInfinity; |
---|
1284 | |
---|
1285 | // now do jacobian for tangential forces |
---|
1286 | dVector3 t1,t2; // two vectors tangential to normal |
---|
1287 | |
---|
1288 | // first friction direction |
---|
1289 | if (j->the_m >= 2) { |
---|
1290 | if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? |
---|
1291 | t1[0] = j->contact.fdir1[0]; |
---|
1292 | t1[1] = j->contact.fdir1[1]; |
---|
1293 | t1[2] = j->contact.fdir1[2]; |
---|
1294 | dCROSS (t2,=,normal,t1); |
---|
1295 | } |
---|
1296 | else { |
---|
1297 | dPlaneSpace (normal,t1,t2); |
---|
1298 | } |
---|
1299 | info->J1l[s+0] = t1[0]; |
---|
1300 | info->J1l[s+1] = t1[1]; |
---|
1301 | info->J1l[s+2] = t1[2]; |
---|
1302 | dCROSS (info->J1a+s,=,c1,t1); |
---|
1303 | if (j->node[1].body) { |
---|
1304 | info->J2l[s+0] = -t1[0]; |
---|
1305 | info->J2l[s+1] = -t1[1]; |
---|
1306 | info->J2l[s+2] = -t1[2]; |
---|
1307 | dCROSS (info->J2a+s,= -,c2,t1); |
---|
1308 | } |
---|
1309 | // set right hand side |
---|
1310 | if (j->contact.surface.mode & dContactMotion1) { |
---|
1311 | info->c[1] = j->contact.surface.motion1; |
---|
1312 | } |
---|
1313 | // set LCP bounds and friction index. this depends on the approximation |
---|
1314 | // mode |
---|
1315 | info->lo[1] = -j->contact.surface.mu; |
---|
1316 | info->hi[1] = j->contact.surface.mu; |
---|
1317 | if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; |
---|
1318 | |
---|
1319 | // set slip (constraint force mixing) |
---|
1320 | if (j->contact.surface.mode & dContactSlip1) |
---|
1321 | info->cfm[1] = j->contact.surface.slip1; |
---|
1322 | } |
---|
1323 | |
---|
1324 | // second friction direction |
---|
1325 | if (j->the_m >= 3) { |
---|
1326 | info->J1l[s2+0] = t2[0]; |
---|
1327 | info->J1l[s2+1] = t2[1]; |
---|
1328 | info->J1l[s2+2] = t2[2]; |
---|
1329 | dCROSS (info->J1a+s2,=,c1,t2); |
---|
1330 | if (j->node[1].body) { |
---|
1331 | info->J2l[s2+0] = -t2[0]; |
---|
1332 | info->J2l[s2+1] = -t2[1]; |
---|
1333 | info->J2l[s2+2] = -t2[2]; |
---|
1334 | dCROSS (info->J2a+s2,= -,c2,t2); |
---|
1335 | } |
---|
1336 | // set right hand side |
---|
1337 | if (j->contact.surface.mode & dContactMotion2) { |
---|
1338 | info->c[2] = j->contact.surface.motion2; |
---|
1339 | } |
---|
1340 | // set LCP bounds and friction index. this depends on the approximation |
---|
1341 | // mode |
---|
1342 | if (j->contact.surface.mode & dContactMu2) { |
---|
1343 | info->lo[2] = -j->contact.surface.mu2; |
---|
1344 | info->hi[2] = j->contact.surface.mu2; |
---|
1345 | } |
---|
1346 | else { |
---|
1347 | info->lo[2] = -j->contact.surface.mu; |
---|
1348 | info->hi[2] = j->contact.surface.mu; |
---|
1349 | } |
---|
1350 | if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; |
---|
1351 | |
---|
1352 | // set slip (constraint force mixing) |
---|
1353 | if (j->contact.surface.mode & dContactSlip2) |
---|
1354 | info->cfm[2] = j->contact.surface.slip2; |
---|
1355 | } |
---|
1356 | } |
---|
1357 | |
---|
1358 | |
---|
1359 | dxJoint::Vtable __dcontact_vtable = { |
---|
1360 | sizeof(dxJointContact), |
---|
1361 | (dxJoint::init_fn*) contactInit, |
---|
1362 | (dxJoint::getInfo1_fn*) contactGetInfo1, |
---|
1363 | (dxJoint::getInfo2_fn*) contactGetInfo2, |
---|
1364 | dJointTypeContact}; |
---|
1365 | |
---|
1366 | //**************************************************************************** |
---|
1367 | // hinge 2. note that this joint must be attached to two bodies for it to work |
---|
1368 | |
---|
1369 | static dReal measureHinge2Angle (dxJointHinge2 *joint) |
---|
1370 | { |
---|
1371 | dVector3 a1,a2; |
---|
1372 | dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2); |
---|
1373 | dMULTIPLY1_331 (a2,joint->node[0].body->R,a1); |
---|
1374 | dReal x = dDOT(joint->v1,a2); |
---|
1375 | dReal y = dDOT(joint->v2,a2); |
---|
1376 | return -dAtan2 (y,x); |
---|
1377 | } |
---|
1378 | |
---|
1379 | |
---|
1380 | static void hinge2Init (dxJointHinge2 *j) |
---|
1381 | { |
---|
1382 | dSetZero (j->anchor1,4); |
---|
1383 | dSetZero (j->anchor2,4); |
---|
1384 | dSetZero (j->axis1,4); |
---|
1385 | j->axis1[0] = 1; |
---|
1386 | dSetZero (j->axis2,4); |
---|
1387 | j->axis2[1] = 1; |
---|
1388 | j->c0 = 0; |
---|
1389 | j->s0 = 0; |
---|
1390 | |
---|
1391 | dSetZero (j->v1,4); |
---|
1392 | j->v1[0] = 1; |
---|
1393 | dSetZero (j->v2,4); |
---|
1394 | j->v2[1] = 1; |
---|
1395 | |
---|
1396 | j->limot1.init (j->world); |
---|
1397 | j->limot2.init (j->world); |
---|
1398 | |
---|
1399 | j->susp_erp = j->world->global_erp; |
---|
1400 | j->susp_cfm = j->world->global_cfm; |
---|
1401 | |
---|
1402 | j->flags |= dJOINT_TWOBODIES; |
---|
1403 | } |
---|
1404 | |
---|
1405 | |
---|
1406 | static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) |
---|
1407 | { |
---|
1408 | info->m = 4; |
---|
1409 | info->nub = 4; |
---|
1410 | |
---|
1411 | // see if we're powered or at a joint limit for axis 1 |
---|
1412 | int atlimit=0; |
---|
1413 | if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && |
---|
1414 | j->limot1.lostop <= j->limot1.histop) { |
---|
1415 | dReal angle = measureHinge2Angle (j); |
---|
1416 | if (j->limot1.testRotationalLimit (angle)) atlimit = 1; |
---|
1417 | } |
---|
1418 | if (atlimit || j->limot1.fmax > 0) info->m++; |
---|
1419 | |
---|
1420 | // see if we're powering axis 2 (we currently never limit this axis) |
---|
1421 | j->limot2.limit = 0; |
---|
1422 | if (j->limot2.fmax > 0) info->m++; |
---|
1423 | } |
---|
1424 | |
---|
1425 | |
---|
1426 | // macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are |
---|
1427 | // relative to body 1 and 2 initially) and then computes the constrained |
---|
1428 | // rotational axis as the cross product of ax1 and ax2. |
---|
1429 | // the sin and cos of the angle between axis 1 and 2 is computed, this comes |
---|
1430 | // from dot and cross product rules. |
---|
1431 | |
---|
1432 | #define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ |
---|
1433 | dVector3 ax1,ax2; \ |
---|
1434 | dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \ |
---|
1435 | dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \ |
---|
1436 | dCROSS (axis,=,ax1,ax2); \ |
---|
1437 | sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ |
---|
1438 | cos_angle = dDOT (ax1,ax2); |
---|
1439 | |
---|
1440 | |
---|
1441 | static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) |
---|
1442 | { |
---|
1443 | // get information we need to set the hinge row |
---|
1444 | dReal s,c; |
---|
1445 | dVector3 q; |
---|
1446 | HINGE2_GET_AXIS_INFO (q,s,c); |
---|
1447 | dNormalize3 (q); // @@@ quicker: divide q by s ? |
---|
1448 | |
---|
1449 | // set the three ball-and-socket rows (aligned to the suspension axis ax1) |
---|
1450 | setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); |
---|
1451 | |
---|
1452 | // set the hinge row |
---|
1453 | int s3=3*info->rowskip; |
---|
1454 | info->J1a[s3+0] = q[0]; |
---|
1455 | info->J1a[s3+1] = q[1]; |
---|
1456 | info->J1a[s3+2] = q[2]; |
---|
1457 | if (joint->node[1].body) { |
---|
1458 | info->J2a[s3+0] = -q[0]; |
---|
1459 | info->J2a[s3+1] = -q[1]; |
---|
1460 | info->J2a[s3+2] = -q[2]; |
---|
1461 | } |
---|
1462 | |
---|
1463 | // compute the right hand side for the constrained rotational DOF. |
---|
1464 | // axis 1 and axis 2 are separated by an angle `theta'. the desired |
---|
1465 | // separation angle is theta0. sin(theta0) and cos(theta0) are recorded |
---|
1466 | // in the joint structure. the correcting angular velocity is: |
---|
1467 | // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize |
---|
1468 | // = (erp*fps) * (theta0-theta) |
---|
1469 | // (theta0-theta) can be computed using the following small-angle-difference |
---|
1470 | // approximation: |
---|
1471 | // theta0-theta ~= tan(theta0-theta) |
---|
1472 | // = sin(theta0-theta)/cos(theta0-theta) |
---|
1473 | // = (c*s0 - s*c0) / (c*c0 + s*s0) |
---|
1474 | // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 |
---|
1475 | // where c = cos(theta), s = sin(theta) |
---|
1476 | // c0 = cos(theta0), s0 = sin(theta0) |
---|
1477 | |
---|
1478 | dReal k = info->fps * info->erp; |
---|
1479 | info->c[3] = k * (joint->c0 * s - joint->s0 * c); |
---|
1480 | |
---|
1481 | // if the axis1 hinge is powered, or has joint limits, add in more stuff |
---|
1482 | int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); |
---|
1483 | |
---|
1484 | // if the axis2 hinge is powered, add in more stuff |
---|
1485 | joint->limot2.addLimot (joint,info,row,ax2,1); |
---|
1486 | |
---|
1487 | // set parameter for the suspension |
---|
1488 | info->cfm[0] = joint->susp_cfm; |
---|
1489 | } |
---|
1490 | |
---|
1491 | |
---|
1492 | // compute vectors v1 and v2 (embedded in body1), used to measure angle |
---|
1493 | // between body 1 and body 2 |
---|
1494 | |
---|
1495 | static void makeHinge2V1andV2 (dxJointHinge2 *joint) |
---|
1496 | { |
---|
1497 | if (joint->node[0].body) { |
---|
1498 | // get axis 1 and 2 in global coords |
---|
1499 | dVector3 ax1,ax2,v; |
---|
1500 | dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); |
---|
1501 | dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); |
---|
1502 | |
---|
1503 | // don't do anything if the axis1 or axis2 vectors are zero or the same |
---|
1504 | if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || |
---|
1505 | (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || |
---|
1506 | (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; |
---|
1507 | |
---|
1508 | // modify axis 2 so it's perpendicular to axis 1 |
---|
1509 | dReal k = dDOT(ax1,ax2); |
---|
1510 | for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; |
---|
1511 | dNormalize3 (ax2); |
---|
1512 | |
---|
1513 | // make v1 = modified axis2, v2 = axis1 x (modified axis2) |
---|
1514 | dCROSS (v,=,ax1,ax2); |
---|
1515 | dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2); |
---|
1516 | dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v); |
---|
1517 | } |
---|
1518 | } |
---|
1519 | |
---|
1520 | |
---|
1521 | extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint, |
---|
1522 | dReal x, dReal y, dReal z) |
---|
1523 | { |
---|
1524 | dUASSERT(joint,"bad joint argument"); |
---|
1525 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1526 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
1527 | makeHinge2V1andV2 (joint); |
---|
1528 | } |
---|
1529 | |
---|
1530 | |
---|
1531 | extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint, |
---|
1532 | dReal x, dReal y, dReal z) |
---|
1533 | { |
---|
1534 | dUASSERT(joint,"bad joint argument"); |
---|
1535 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1536 | if (joint->node[0].body) { |
---|
1537 | dReal q[4]; |
---|
1538 | q[0] = x; |
---|
1539 | q[1] = y; |
---|
1540 | q[2] = z; |
---|
1541 | q[3] = 0; |
---|
1542 | dNormalize3 (q); |
---|
1543 | dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); |
---|
1544 | joint->axis1[3] = 0; |
---|
1545 | |
---|
1546 | // compute the sin and cos of the angle between axis 1 and axis 2 |
---|
1547 | dVector3 ax; |
---|
1548 | HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); |
---|
1549 | } |
---|
1550 | makeHinge2V1andV2 (joint); |
---|
1551 | } |
---|
1552 | |
---|
1553 | |
---|
1554 | extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint, |
---|
1555 | dReal x, dReal y, dReal z) |
---|
1556 | { |
---|
1557 | dUASSERT(joint,"bad joint argument"); |
---|
1558 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1559 | if (joint->node[1].body) { |
---|
1560 | dReal q[4]; |
---|
1561 | q[0] = x; |
---|
1562 | q[1] = y; |
---|
1563 | q[2] = z; |
---|
1564 | q[3] = 0; |
---|
1565 | dNormalize3 (q); |
---|
1566 | dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); |
---|
1567 | joint->axis1[3] = 0; |
---|
1568 | |
---|
1569 | // compute the sin and cos of the angle between axis 1 and axis 2 |
---|
1570 | dVector3 ax; |
---|
1571 | HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); |
---|
1572 | } |
---|
1573 | makeHinge2V1andV2 (joint); |
---|
1574 | } |
---|
1575 | |
---|
1576 | |
---|
1577 | extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint, |
---|
1578 | int parameter, dReal value) |
---|
1579 | { |
---|
1580 | dUASSERT(joint,"bad joint argument"); |
---|
1581 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1582 | if ((parameter & 0xff00) == 0x100) { |
---|
1583 | joint->limot2.set (parameter & 0xff,value); |
---|
1584 | } |
---|
1585 | else { |
---|
1586 | if (parameter == dParamSuspensionERP) joint->susp_erp = value; |
---|
1587 | else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; |
---|
1588 | else joint->limot1.set (parameter,value); |
---|
1589 | } |
---|
1590 | } |
---|
1591 | |
---|
1592 | |
---|
1593 | extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result) |
---|
1594 | { |
---|
1595 | dUASSERT(joint,"bad joint argument"); |
---|
1596 | dUASSERT(result,"bad result argument"); |
---|
1597 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1598 | if (joint->flags & dJOINT_REVERSE) |
---|
1599 | getAnchor2 (joint,result,joint->anchor2); |
---|
1600 | else |
---|
1601 | getAnchor (joint,result,joint->anchor1); |
---|
1602 | } |
---|
1603 | |
---|
1604 | |
---|
1605 | extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result) |
---|
1606 | { |
---|
1607 | dUASSERT(joint,"bad joint argument"); |
---|
1608 | dUASSERT(result,"bad result argument"); |
---|
1609 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1610 | if (joint->flags & dJOINT_REVERSE) |
---|
1611 | getAnchor (joint,result,joint->anchor1); |
---|
1612 | else |
---|
1613 | getAnchor2 (joint,result,joint->anchor2); |
---|
1614 | } |
---|
1615 | |
---|
1616 | |
---|
1617 | extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) |
---|
1618 | { |
---|
1619 | dUASSERT(joint,"bad joint argument"); |
---|
1620 | dUASSERT(result,"bad result argument"); |
---|
1621 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1622 | if (joint->node[0].body) { |
---|
1623 | dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1); |
---|
1624 | } |
---|
1625 | } |
---|
1626 | |
---|
1627 | |
---|
1628 | extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result) |
---|
1629 | { |
---|
1630 | dUASSERT(joint,"bad joint argument"); |
---|
1631 | dUASSERT(result,"bad result argument"); |
---|
1632 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1633 | if (joint->node[1].body) { |
---|
1634 | dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2); |
---|
1635 | } |
---|
1636 | } |
---|
1637 | |
---|
1638 | |
---|
1639 | extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter) |
---|
1640 | { |
---|
1641 | dUASSERT(joint,"bad joint argument"); |
---|
1642 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1643 | if ((parameter & 0xff00) == 0x100) { |
---|
1644 | return joint->limot2.get (parameter & 0xff); |
---|
1645 | } |
---|
1646 | else { |
---|
1647 | if (parameter == dParamSuspensionERP) return joint->susp_erp; |
---|
1648 | else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; |
---|
1649 | else return joint->limot1.get (parameter); |
---|
1650 | } |
---|
1651 | } |
---|
1652 | |
---|
1653 | |
---|
1654 | extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint) |
---|
1655 | { |
---|
1656 | dUASSERT(joint,"bad joint argument"); |
---|
1657 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1658 | if (joint->node[0].body) return measureHinge2Angle (joint); |
---|
1659 | else return 0; |
---|
1660 | } |
---|
1661 | |
---|
1662 | |
---|
1663 | extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint) |
---|
1664 | { |
---|
1665 | dUASSERT(joint,"bad joint argument"); |
---|
1666 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1667 | if (joint->node[0].body) { |
---|
1668 | dVector3 axis; |
---|
1669 | dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); |
---|
1670 | dReal rate = dDOT(axis,joint->node[0].body->avel); |
---|
1671 | if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
---|
1672 | return rate; |
---|
1673 | } |
---|
1674 | else return 0; |
---|
1675 | } |
---|
1676 | |
---|
1677 | |
---|
1678 | extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint) |
---|
1679 | { |
---|
1680 | dUASSERT(joint,"bad joint argument"); |
---|
1681 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1682 | if (joint->node[0].body && joint->node[1].body) { |
---|
1683 | dVector3 axis; |
---|
1684 | dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2); |
---|
1685 | dReal rate = dDOT(axis,joint->node[0].body->avel); |
---|
1686 | if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
---|
1687 | return rate; |
---|
1688 | } |
---|
1689 | else return 0; |
---|
1690 | } |
---|
1691 | |
---|
1692 | |
---|
1693 | extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2) |
---|
1694 | { |
---|
1695 | dVector3 axis1, axis2; |
---|
1696 | dUASSERT(joint,"bad joint argument"); |
---|
1697 | dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); |
---|
1698 | |
---|
1699 | if (joint->node[0].body && joint->node[1].body) { |
---|
1700 | dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1); |
---|
1701 | dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2); |
---|
1702 | axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; |
---|
1703 | axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; |
---|
1704 | axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; |
---|
1705 | dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); |
---|
1706 | dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); |
---|
1707 | } |
---|
1708 | } |
---|
1709 | |
---|
1710 | |
---|
1711 | dxJoint::Vtable __dhinge2_vtable = { |
---|
1712 | sizeof(dxJointHinge2), |
---|
1713 | (dxJoint::init_fn*) hinge2Init, |
---|
1714 | (dxJoint::getInfo1_fn*) hinge2GetInfo1, |
---|
1715 | (dxJoint::getInfo2_fn*) hinge2GetInfo2, |
---|
1716 | dJointTypeHinge2}; |
---|
1717 | |
---|
1718 | //**************************************************************************** |
---|
1719 | // universal |
---|
1720 | |
---|
1721 | // I just realized that the universal joint is equivalent to a hinge 2 joint with |
---|
1722 | // perfectly stiff suspension. By comparing the hinge 2 implementation to |
---|
1723 | // the universal implementation, you may be able to improve this |
---|
1724 | // implementation (or, less likely, the hinge2 implementation). |
---|
1725 | |
---|
1726 | static void universalInit (dxJointUniversal *j) |
---|
1727 | { |
---|
1728 | dSetZero (j->anchor1,4); |
---|
1729 | dSetZero (j->anchor2,4); |
---|
1730 | dSetZero (j->axis1,4); |
---|
1731 | j->axis1[0] = 1; |
---|
1732 | dSetZero (j->axis2,4); |
---|
1733 | j->axis2[1] = 1; |
---|
1734 | dSetZero(j->qrel1,4); |
---|
1735 | dSetZero(j->qrel2,4); |
---|
1736 | j->limot1.init (j->world); |
---|
1737 | j->limot2.init (j->world); |
---|
1738 | } |
---|
1739 | |
---|
1740 | |
---|
1741 | static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) |
---|
1742 | { |
---|
1743 | // This says "ax1 = joint->node[0].body->R * joint->axis1" |
---|
1744 | dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); |
---|
1745 | |
---|
1746 | if (joint->node[1].body) { |
---|
1747 | dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); |
---|
1748 | } |
---|
1749 | else { |
---|
1750 | ax2[0] = joint->axis2[0]; |
---|
1751 | ax2[1] = joint->axis2[1]; |
---|
1752 | ax2[2] = joint->axis2[2]; |
---|
1753 | } |
---|
1754 | } |
---|
1755 | |
---|
1756 | |
---|
1757 | static dReal getUniversalAngle1(dxJointUniversal *joint) |
---|
1758 | { |
---|
1759 | if (joint->node[0].body) { |
---|
1760 | // length 1 joint axis in global coordinates, from each body |
---|
1761 | dVector3 ax1, ax2; |
---|
1762 | dMatrix3 R; |
---|
1763 | dQuaternion qcross, qq, qrel; |
---|
1764 | |
---|
1765 | getUniversalAxes (joint,ax1,ax2); |
---|
1766 | |
---|
1767 | // It should be possible to get both angles without explicitly |
---|
1768 | // constructing the rotation matrix of the cross. Basically, |
---|
1769 | // orientation of the cross about axis1 comes from body 2, |
---|
1770 | // about axis 2 comes from body 1, and the perpendicular |
---|
1771 | // axis can come from the two bodies somehow. (We don't really |
---|
1772 | // want to assume it's 90 degrees, because in general the |
---|
1773 | // constraints won't be perfectly satisfied, or even very well |
---|
1774 | // satisfied.) |
---|
1775 | // |
---|
1776 | // However, we'd need a version of getHingeAngleFromRElativeQuat() |
---|
1777 | // that CAN handle when its relative quat is rotated along a direction |
---|
1778 | // other than the given axis. What I have here works, |
---|
1779 | // although it's probably much slower than need be. |
---|
1780 | |
---|
1781 | dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
---|
1782 | dRtoQ (R,qcross); |
---|
1783 | |
---|
1784 | // This code is essential the same as getHingeAngle(), see the comments |
---|
1785 | // there for details. |
---|
1786 | |
---|
1787 | // get qrel = relative rotation between node[0] and the cross |
---|
1788 | dQMultiply1 (qq,joint->node[0].body->q,qcross); |
---|
1789 | dQMultiply2 (qrel,qq,joint->qrel1); |
---|
1790 | |
---|
1791 | return getHingeAngleFromRelativeQuat(qrel, joint->axis1); |
---|
1792 | } |
---|
1793 | return 0; |
---|
1794 | } |
---|
1795 | |
---|
1796 | |
---|
1797 | static dReal getUniversalAngle2(dxJointUniversal *joint) |
---|
1798 | { |
---|
1799 | if (joint->node[0].body) { |
---|
1800 | // length 1 joint axis in global coordinates, from each body |
---|
1801 | dVector3 ax1, ax2; |
---|
1802 | dMatrix3 R; |
---|
1803 | dQuaternion qcross, qq, qrel; |
---|
1804 | |
---|
1805 | getUniversalAxes (joint,ax1,ax2); |
---|
1806 | |
---|
1807 | // It should be possible to get both angles without explicitly |
---|
1808 | // constructing the rotation matrix of the cross. Basically, |
---|
1809 | // orientation of the cross about axis1 comes from body 2, |
---|
1810 | // about axis 2 comes from body 1, and the perpendicular |
---|
1811 | // axis can come from the two bodies somehow. (We don't really |
---|
1812 | // want to assume it's 90 degrees, because in general the |
---|
1813 | // constraints won't be perfectly satisfied, or even very well |
---|
1814 | // satisfied.) |
---|
1815 | // |
---|
1816 | // However, we'd need a version of getHingeAngleFromRElativeQuat() |
---|
1817 | // that CAN handle when its relative quat is rotated along a direction |
---|
1818 | // other than the given axis. What I have here works, |
---|
1819 | // although it's probably much slower than need be. |
---|
1820 | |
---|
1821 | dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
---|
1822 | dRtoQ(R, qcross); |
---|
1823 | |
---|
1824 | if (joint->node[1].body) { |
---|
1825 | dQMultiply1 (qq, joint->node[1].body->q, qcross); |
---|
1826 | dQMultiply2 (qrel,qq,joint->qrel2); |
---|
1827 | } |
---|
1828 | else { |
---|
1829 | // pretend joint->node[1].body->q is the identity |
---|
1830 | dQMultiply2 (qrel,qcross, joint->qrel2); |
---|
1831 | } |
---|
1832 | |
---|
1833 | return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); |
---|
1834 | } |
---|
1835 | return 0; |
---|
1836 | } |
---|
1837 | |
---|
1838 | |
---|
1839 | static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) |
---|
1840 | { |
---|
1841 | info->nub = 4; |
---|
1842 | info->m = 4; |
---|
1843 | |
---|
1844 | // see if we're powered or at a joint limit. |
---|
1845 | bool constraint1 = j->limot1.fmax > 0; |
---|
1846 | bool constraint2 = j->limot2.fmax > 0; |
---|
1847 | |
---|
1848 | bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && |
---|
1849 | j->limot1.lostop <= j->limot1.histop; |
---|
1850 | bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && |
---|
1851 | j->limot2.lostop <= j->limot2.histop; |
---|
1852 | |
---|
1853 | // We need to call testRotationLimit() even if we're motored, since it |
---|
1854 | // records the result. |
---|
1855 | if (limiting1 || limiting2) { |
---|
1856 | dReal angle1, angle2; |
---|
1857 | angle1 = getUniversalAngle1(j); |
---|
1858 | angle2 = getUniversalAngle2(j); |
---|
1859 | if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; |
---|
1860 | if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; |
---|
1861 | } |
---|
1862 | if (constraint1) |
---|
1863 | info->m++; |
---|
1864 | if (constraint2) |
---|
1865 | info->m++; |
---|
1866 | } |
---|
1867 | |
---|
1868 | |
---|
1869 | static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) |
---|
1870 | { |
---|
1871 | // set the three ball-and-socket rows |
---|
1872 | setBall (joint,info,joint->anchor1,joint->anchor2); |
---|
1873 | |
---|
1874 | // set the universal joint row. the angular velocity about an axis |
---|
1875 | // perpendicular to both joint axes should be equal. thus the constraint |
---|
1876 | // equation is |
---|
1877 | // p*w1 - p*w2 = 0 |
---|
1878 | // where p is a vector normal to both joint axes, and w1 and w2 |
---|
1879 | // are the angular velocity vectors of the two bodies. |
---|
1880 | |
---|
1881 | // length 1 joint axis in global coordinates, from each body |
---|
1882 | dVector3 ax1, ax2; |
---|
1883 | dVector3 ax2_temp; |
---|
1884 | // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate |
---|
1885 | // about this. |
---|
1886 | dVector3 p; |
---|
1887 | dReal k; |
---|
1888 | |
---|
1889 | getUniversalAxes(joint, ax1, ax2); |
---|
1890 | k = dDOT(ax1, ax2); |
---|
1891 | ax2_temp[0] = ax2[0] - k*ax1[0]; |
---|
1892 | ax2_temp[1] = ax2[1] - k*ax1[1]; |
---|
1893 | ax2_temp[2] = ax2[2] - k*ax1[2]; |
---|
1894 | dCROSS(p, =, ax1, ax2_temp); |
---|
1895 | dNormalize3(p); |
---|
1896 | |
---|
1897 | int s3=3*info->rowskip; |
---|
1898 | |
---|
1899 | info->J1a[s3+0] = p[0]; |
---|
1900 | info->J1a[s3+1] = p[1]; |
---|
1901 | info->J1a[s3+2] = p[2]; |
---|
1902 | |
---|
1903 | if (joint->node[1].body) { |
---|
1904 | info->J2a[s3+0] = -p[0]; |
---|
1905 | info->J2a[s3+1] = -p[1]; |
---|
1906 | info->J2a[s3+2] = -p[2]; |
---|
1907 | } |
---|
1908 | |
---|
1909 | // compute the right hand side of the constraint equation. set relative |
---|
1910 | // body velocities along p to bring the axes back to perpendicular. |
---|
1911 | // If ax1, ax2 are unit length joint axes as computed from body1 and |
---|
1912 | // body2, we need to rotate both bodies along the axis p. If theta |
---|
1913 | // is the angle between ax1 and ax2, we need an angular velocity |
---|
1914 | // along p to cover the angle erp * (theta - Pi/2) in one step: |
---|
1915 | // |
---|
1916 | // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize |
---|
1917 | // = (erp*fps) * (theta - Pi/2) |
---|
1918 | // |
---|
1919 | // if theta is close to Pi/2, |
---|
1920 | // theta - Pi/2 ~= cos(theta), so |
---|
1921 | // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) |
---|
1922 | |
---|
1923 | info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); |
---|
1924 | |
---|
1925 | // if the first angle is powered, or has joint limits, add in the stuff |
---|
1926 | int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); |
---|
1927 | |
---|
1928 | // if the second angle is powered, or has joint limits, add in more stuff |
---|
1929 | joint->limot2.addLimot (joint,info,row,ax2,1); |
---|
1930 | } |
---|
1931 | |
---|
1932 | |
---|
1933 | static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) |
---|
1934 | { |
---|
1935 | if (joint->node[0].body) { |
---|
1936 | dVector3 ax1, ax2; |
---|
1937 | dMatrix3 R; |
---|
1938 | dQuaternion qcross; |
---|
1939 | |
---|
1940 | getUniversalAxes(joint, ax1, ax2); |
---|
1941 | |
---|
1942 | // Axis 1. |
---|
1943 | dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
---|
1944 | dRtoQ(R, qcross); |
---|
1945 | dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); |
---|
1946 | |
---|
1947 | // Axis 2. |
---|
1948 | dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
---|
1949 | dRtoQ(R, qcross); |
---|
1950 | if (joint->node[1].body) { |
---|
1951 | dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); |
---|
1952 | } |
---|
1953 | else { |
---|
1954 | // set joint->qrel to qcross |
---|
1955 | for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; |
---|
1956 | } |
---|
1957 | } |
---|
1958 | } |
---|
1959 | |
---|
1960 | |
---|
1961 | extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint, |
---|
1962 | dReal x, dReal y, dReal z) |
---|
1963 | { |
---|
1964 | dUASSERT(joint,"bad joint argument"); |
---|
1965 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
1966 | setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
---|
1967 | universalComputeInitialRelativeRotations(joint); |
---|
1968 | } |
---|
1969 | |
---|
1970 | |
---|
1971 | extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint, |
---|
1972 | dReal x, dReal y, dReal z) |
---|
1973 | { |
---|
1974 | dUASSERT(joint,"bad joint argument"); |
---|
1975 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
1976 | if (joint->flags & dJOINT_REVERSE) |
---|
1977 | setAxes (joint,x,y,z,NULL,joint->axis2); |
---|
1978 | else |
---|
1979 | setAxes (joint,x,y,z,joint->axis1,NULL); |
---|
1980 | universalComputeInitialRelativeRotations(joint); |
---|
1981 | } |
---|
1982 | |
---|
1983 | |
---|
1984 | extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint, |
---|
1985 | dReal x, dReal y, dReal z) |
---|
1986 | { |
---|
1987 | dUASSERT(joint,"bad joint argument"); |
---|
1988 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
1989 | if (joint->flags & dJOINT_REVERSE) |
---|
1990 | setAxes (joint,x,y,z,joint->axis1,NULL); |
---|
1991 | else |
---|
1992 | setAxes (joint,x,y,z,NULL,joint->axis2); |
---|
1993 | universalComputeInitialRelativeRotations(joint); |
---|
1994 | } |
---|
1995 | |
---|
1996 | |
---|
1997 | extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, |
---|
1998 | dVector3 result) |
---|
1999 | { |
---|
2000 | dUASSERT(joint,"bad joint argument"); |
---|
2001 | dUASSERT(result,"bad result argument"); |
---|
2002 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2003 | if (joint->flags & dJOINT_REVERSE) |
---|
2004 | getAnchor2 (joint,result,joint->anchor2); |
---|
2005 | else |
---|
2006 | getAnchor (joint,result,joint->anchor1); |
---|
2007 | } |
---|
2008 | |
---|
2009 | |
---|
2010 | extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint, |
---|
2011 | dVector3 result) |
---|
2012 | { |
---|
2013 | dUASSERT(joint,"bad joint argument"); |
---|
2014 | dUASSERT(result,"bad result argument"); |
---|
2015 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2016 | if (joint->flags & dJOINT_REVERSE) |
---|
2017 | getAnchor (joint,result,joint->anchor1); |
---|
2018 | else |
---|
2019 | getAnchor2 (joint,result,joint->anchor2); |
---|
2020 | } |
---|
2021 | |
---|
2022 | |
---|
2023 | extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, |
---|
2024 | dVector3 result) |
---|
2025 | { |
---|
2026 | dUASSERT(joint,"bad joint argument"); |
---|
2027 | dUASSERT(result,"bad result argument"); |
---|
2028 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2029 | if (joint->flags & dJOINT_REVERSE) |
---|
2030 | getAxis2 (joint,result,joint->axis2); |
---|
2031 | else |
---|
2032 | getAxis (joint,result,joint->axis1); |
---|
2033 | } |
---|
2034 | |
---|
2035 | |
---|
2036 | extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, |
---|
2037 | dVector3 result) |
---|
2038 | { |
---|
2039 | dUASSERT(joint,"bad joint argument"); |
---|
2040 | dUASSERT(result,"bad result argument"); |
---|
2041 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2042 | if (joint->flags & dJOINT_REVERSE) |
---|
2043 | getAxis (joint,result,joint->axis1); |
---|
2044 | else |
---|
2045 | getAxis2 (joint,result,joint->axis2); |
---|
2046 | } |
---|
2047 | |
---|
2048 | |
---|
2049 | extern "C" void dJointSetUniversalParam (dxJointUniversal *joint, |
---|
2050 | int parameter, dReal value) |
---|
2051 | { |
---|
2052 | dUASSERT(joint,"bad joint argument"); |
---|
2053 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2054 | if ((parameter & 0xff00) == 0x100) { |
---|
2055 | joint->limot2.set (parameter & 0xff,value); |
---|
2056 | } |
---|
2057 | else { |
---|
2058 | joint->limot1.set (parameter,value); |
---|
2059 | } |
---|
2060 | } |
---|
2061 | |
---|
2062 | |
---|
2063 | extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter) |
---|
2064 | { |
---|
2065 | dUASSERT(joint,"bad joint argument"); |
---|
2066 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2067 | if ((parameter & 0xff00) == 0x100) { |
---|
2068 | return joint->limot2.get (parameter & 0xff); |
---|
2069 | } |
---|
2070 | else { |
---|
2071 | return joint->limot1.get (parameter); |
---|
2072 | } |
---|
2073 | } |
---|
2074 | |
---|
2075 | |
---|
2076 | extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint) |
---|
2077 | { |
---|
2078 | dUASSERT(joint,"bad joint argument"); |
---|
2079 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2080 | if (joint->flags & dJOINT_REVERSE) |
---|
2081 | return getUniversalAngle2 (joint); |
---|
2082 | else |
---|
2083 | return getUniversalAngle1 (joint); |
---|
2084 | } |
---|
2085 | |
---|
2086 | |
---|
2087 | extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint) |
---|
2088 | { |
---|
2089 | dUASSERT(joint,"bad joint argument"); |
---|
2090 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2091 | if (joint->flags & dJOINT_REVERSE) |
---|
2092 | return getUniversalAngle1 (joint); |
---|
2093 | else |
---|
2094 | return getUniversalAngle2 (joint); |
---|
2095 | } |
---|
2096 | |
---|
2097 | |
---|
2098 | extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint) |
---|
2099 | { |
---|
2100 | dUASSERT(joint,"bad joint argument"); |
---|
2101 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2102 | |
---|
2103 | if (joint->node[0].body) { |
---|
2104 | dVector3 axis; |
---|
2105 | |
---|
2106 | if (joint->flags & dJOINT_REVERSE) |
---|
2107 | getAxis2 (joint,axis,joint->axis2); |
---|
2108 | else |
---|
2109 | getAxis (joint,axis,joint->axis1); |
---|
2110 | |
---|
2111 | dReal rate = dDOT(axis, joint->node[0].body->avel); |
---|
2112 | if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); |
---|
2113 | return rate; |
---|
2114 | } |
---|
2115 | return 0; |
---|
2116 | } |
---|
2117 | |
---|
2118 | |
---|
2119 | extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint) |
---|
2120 | { |
---|
2121 | dUASSERT(joint,"bad joint argument"); |
---|
2122 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2123 | |
---|
2124 | if (joint->node[0].body) { |
---|
2125 | dVector3 axis; |
---|
2126 | |
---|
2127 | if (joint->flags & dJOINT_REVERSE) |
---|
2128 | getAxis (joint,axis,joint->axis1); |
---|
2129 | else |
---|
2130 | getAxis2 (joint,axis,joint->axis2); |
---|
2131 | |
---|
2132 | dReal rate = dDOT(axis, joint->node[0].body->avel); |
---|
2133 | if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); |
---|
2134 | return rate; |
---|
2135 | } |
---|
2136 | return 0; |
---|
2137 | } |
---|
2138 | |
---|
2139 | |
---|
2140 | extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2) |
---|
2141 | { |
---|
2142 | dVector3 axis1, axis2; |
---|
2143 | dAASSERT(joint); |
---|
2144 | dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); |
---|
2145 | |
---|
2146 | if (joint->flags & dJOINT_REVERSE) { |
---|
2147 | dReal temp = torque1; |
---|
2148 | torque1 = - torque2; |
---|
2149 | torque2 = - temp; |
---|
2150 | } |
---|
2151 | |
---|
2152 | getAxis (joint,axis1,joint->axis1); |
---|
2153 | getAxis2 (joint,axis2,joint->axis2); |
---|
2154 | axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; |
---|
2155 | axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; |
---|
2156 | axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; |
---|
2157 | |
---|
2158 | if (joint->node[0].body != 0) |
---|
2159 | dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); |
---|
2160 | if (joint->node[1].body != 0) |
---|
2161 | dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); |
---|
2162 | } |
---|
2163 | |
---|
2164 | |
---|
2165 | |
---|
2166 | |
---|
2167 | |
---|
2168 | dxJoint::Vtable __duniversal_vtable = { |
---|
2169 | sizeof(dxJointUniversal), |
---|
2170 | (dxJoint::init_fn*) universalInit, |
---|
2171 | (dxJoint::getInfo1_fn*) universalGetInfo1, |
---|
2172 | (dxJoint::getInfo2_fn*) universalGetInfo2, |
---|
2173 | dJointTypeUniversal}; |
---|
2174 | |
---|
2175 | //**************************************************************************** |
---|
2176 | // angular motor |
---|
2177 | |
---|
2178 | static void amotorInit (dxJointAMotor *j) |
---|
2179 | { |
---|
2180 | int i; |
---|
2181 | j->num = 0; |
---|
2182 | j->mode = dAMotorUser; |
---|
2183 | for (i=0; i<3; i++) { |
---|
2184 | j->rel[i] = 0; |
---|
2185 | dSetZero (j->axis[i],4); |
---|
2186 | j->limot[i].init (j->world); |
---|
2187 | j->angle[i] = 0; |
---|
2188 | } |
---|
2189 | dSetZero (j->reference1,4); |
---|
2190 | dSetZero (j->reference2,4); |
---|
2191 | |
---|
2192 | j->flags |= dJOINT_TWOBODIES; |
---|
2193 | } |
---|
2194 | |
---|
2195 | |
---|
2196 | // compute the 3 axes in global coordinates |
---|
2197 | |
---|
2198 | static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) |
---|
2199 | { |
---|
2200 | if (joint->mode == dAMotorEuler) { |
---|
2201 | // special handling for euler mode |
---|
2202 | dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]); |
---|
2203 | dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]); |
---|
2204 | dCROSS (ax[1],=,ax[2],ax[0]); |
---|
2205 | dNormalize3 (ax[1]); |
---|
2206 | } |
---|
2207 | else { |
---|
2208 | for (int i=0; i < joint->num; i++) { |
---|
2209 | if (joint->rel[i] == 1) { |
---|
2210 | // relative to b1 |
---|
2211 | dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); |
---|
2212 | } |
---|
2213 | if (joint->rel[i] == 2) { |
---|
2214 | // relative to b2 |
---|
2215 | dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); |
---|
2216 | } |
---|
2217 | else { |
---|
2218 | // global - just copy it |
---|
2219 | ax[i][0] = joint->axis[i][0]; |
---|
2220 | ax[i][1] = joint->axis[i][1]; |
---|
2221 | ax[i][2] = joint->axis[i][2]; |
---|
2222 | } |
---|
2223 | } |
---|
2224 | } |
---|
2225 | } |
---|
2226 | |
---|
2227 | |
---|
2228 | static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) |
---|
2229 | { |
---|
2230 | // assumptions: |
---|
2231 | // global axes already calculated --> ax |
---|
2232 | // axis[0] is relative to body 1 --> global ax[0] |
---|
2233 | // axis[2] is relative to body 2 --> global ax[2] |
---|
2234 | // ax[1] = ax[2] x ax[0] |
---|
2235 | // original ax[0] and ax[2] are perpendicular |
---|
2236 | // reference1 is perpendicular to ax[0] (in body 1 frame) |
---|
2237 | // reference2 is perpendicular to ax[2] (in body 2 frame) |
---|
2238 | // all ax[] and reference vectors are unit length |
---|
2239 | |
---|
2240 | // calculate references in global frame |
---|
2241 | dVector3 ref1,ref2; |
---|
2242 | dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1); |
---|
2243 | dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2); |
---|
2244 | |
---|
2245 | // get q perpendicular to both ax[0] and ref1, get first euler angle |
---|
2246 | dVector3 q; |
---|
2247 | dCROSS (q,=,ax[0],ref1); |
---|
2248 | joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); |
---|
2249 | |
---|
2250 | // get q perpendicular to both ax[0] and ax[1], get second euler angle |
---|
2251 | dCROSS (q,=,ax[0],ax[1]); |
---|
2252 | joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); |
---|
2253 | |
---|
2254 | // get q perpendicular to both ax[1] and ax[2], get third euler angle |
---|
2255 | dCROSS (q,=,ax[1],ax[2]); |
---|
2256 | joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); |
---|
2257 | } |
---|
2258 | |
---|
2259 | |
---|
2260 | // set the reference vectors as follows: |
---|
2261 | // * reference1 = current axis[2] relative to body 1 |
---|
2262 | // * reference2 = current axis[0] relative to body 2 |
---|
2263 | // this assumes that: |
---|
2264 | // * axis[0] is relative to body 1 |
---|
2265 | // * axis[2] is relative to body 2 |
---|
2266 | |
---|
2267 | static void amotorSetEulerReferenceVectors (dxJointAMotor *j) |
---|
2268 | { |
---|
2269 | if (j->node[0].body && j->node[1].body) { |
---|
2270 | dVector3 r; // axis[2] and axis[0] in global coordinates |
---|
2271 | dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]); |
---|
2272 | dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); |
---|
2273 | dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); |
---|
2274 | dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r); |
---|
2275 | } |
---|
2276 | } |
---|
2277 | |
---|
2278 | |
---|
2279 | static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) |
---|
2280 | { |
---|
2281 | info->m = 0; |
---|
2282 | info->nub = 0; |
---|
2283 | |
---|
2284 | // compute the axes and angles, if in euler mode |
---|
2285 | if (j->mode == dAMotorEuler) { |
---|
2286 | dVector3 ax[3]; |
---|
2287 | amotorComputeGlobalAxes (j,ax); |
---|
2288 | amotorComputeEulerAngles (j,ax); |
---|
2289 | } |
---|
2290 | |
---|
2291 | // see if we're powered or at a joint limit for each axis |
---|
2292 | for (int i=0; i < j->num; i++) { |
---|
2293 | if (j->limot[i].testRotationalLimit (j->angle[i]) || |
---|
2294 | j->limot[i].fmax > 0) { |
---|
2295 | info->m++; |
---|
2296 | } |
---|
2297 | } |
---|
2298 | } |
---|
2299 | |
---|
2300 | |
---|
2301 | static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) |
---|
2302 | { |
---|
2303 | int i; |
---|
2304 | |
---|
2305 | // compute the axes (if not global) |
---|
2306 | dVector3 ax[3]; |
---|
2307 | amotorComputeGlobalAxes (joint,ax); |
---|
2308 | |
---|
2309 | // in euler angle mode we do not actually constrain the angular velocity |
---|
2310 | // along the axes axis[0] and axis[2] (although we do use axis[1]) : |
---|
2311 | // |
---|
2312 | // to get constrain w2-w1 along ...not |
---|
2313 | // ------ --------------------- ------ |
---|
2314 | // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] |
---|
2315 | // d(angle[1])/dt = 0 ax[1] |
---|
2316 | // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] |
---|
2317 | // |
---|
2318 | // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. |
---|
2319 | // to prove the result for angle[0], write the expression for angle[0] from |
---|
2320 | // GetInfo1 then take the derivative. to prove this for angle[2] it is |
---|
2321 | // easier to take the euler rate expression for d(angle[2])/dt with respect |
---|
2322 | // to the components of w and set that to 0. |
---|
2323 | |
---|
2324 | dVector3 *axptr[3]; |
---|
2325 | axptr[0] = &ax[0]; |
---|
2326 | axptr[1] = &ax[1]; |
---|
2327 | axptr[2] = &ax[2]; |
---|
2328 | |
---|
2329 | dVector3 ax0_cross_ax1; |
---|
2330 | dVector3 ax1_cross_ax2; |
---|
2331 | if (joint->mode == dAMotorEuler) { |
---|
2332 | dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); |
---|
2333 | axptr[2] = &ax0_cross_ax1; |
---|
2334 | dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); |
---|
2335 | axptr[0] = &ax1_cross_ax2; |
---|
2336 | } |
---|
2337 | |
---|
2338 | int row=0; |
---|
2339 | for (i=0; i < joint->num; i++) { |
---|
2340 | row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); |
---|
2341 | } |
---|
2342 | } |
---|
2343 | |
---|
2344 | |
---|
2345 | extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num) |
---|
2346 | { |
---|
2347 | dAASSERT(joint && num >= 0 && num <= 3); |
---|
2348 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2349 | if (joint->mode == dAMotorEuler) { |
---|
2350 | joint->num = 3; |
---|
2351 | } |
---|
2352 | else { |
---|
2353 | if (num < 0) num = 0; |
---|
2354 | if (num > 3) num = 3; |
---|
2355 | joint->num = num; |
---|
2356 | } |
---|
2357 | } |
---|
2358 | |
---|
2359 | |
---|
2360 | extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, |
---|
2361 | dReal x, dReal y, dReal z) |
---|
2362 | { |
---|
2363 | dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); |
---|
2364 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2365 | if (anum < 0) anum = 0; |
---|
2366 | if (anum > 2) anum = 2; |
---|
2367 | joint->rel[anum] = rel; |
---|
2368 | |
---|
2369 | // x,y,z is always in global coordinates regardless of rel, so we may have |
---|
2370 | // to convert it to be relative to a body |
---|
2371 | dVector3 r; |
---|
2372 | r[0] = x; |
---|
2373 | r[1] = y; |
---|
2374 | r[2] = z; |
---|
2375 | r[3] = 0; |
---|
2376 | if (rel > 0) { |
---|
2377 | if (rel==1) { |
---|
2378 | dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r); |
---|
2379 | } |
---|
2380 | else { |
---|
2381 | dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r); |
---|
2382 | } |
---|
2383 | } |
---|
2384 | else { |
---|
2385 | joint->axis[anum][0] = r[0]; |
---|
2386 | joint->axis[anum][1] = r[1]; |
---|
2387 | joint->axis[anum][2] = r[2]; |
---|
2388 | } |
---|
2389 | dNormalize3 (joint->axis[anum]); |
---|
2390 | if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); |
---|
2391 | } |
---|
2392 | |
---|
2393 | |
---|
2394 | extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum, |
---|
2395 | dReal angle) |
---|
2396 | { |
---|
2397 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
2398 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2399 | if (joint->mode == dAMotorUser) { |
---|
2400 | if (anum < 0) anum = 0; |
---|
2401 | if (anum > 3) anum = 3; |
---|
2402 | joint->angle[anum] = angle; |
---|
2403 | } |
---|
2404 | } |
---|
2405 | |
---|
2406 | |
---|
2407 | extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter, |
---|
2408 | dReal value) |
---|
2409 | { |
---|
2410 | dAASSERT(joint); |
---|
2411 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2412 | int anum = parameter >> 8; |
---|
2413 | if (anum < 0) anum = 0; |
---|
2414 | if (anum > 2) anum = 2; |
---|
2415 | parameter &= 0xff; |
---|
2416 | joint->limot[anum].set (parameter, value); |
---|
2417 | } |
---|
2418 | |
---|
2419 | |
---|
2420 | extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode) |
---|
2421 | { |
---|
2422 | dAASSERT(joint); |
---|
2423 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2424 | joint->mode = mode; |
---|
2425 | if (joint->mode == dAMotorEuler) { |
---|
2426 | joint->num = 3; |
---|
2427 | amotorSetEulerReferenceVectors (joint); |
---|
2428 | } |
---|
2429 | } |
---|
2430 | |
---|
2431 | |
---|
2432 | extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint) |
---|
2433 | { |
---|
2434 | dAASSERT(joint); |
---|
2435 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2436 | return joint->num; |
---|
2437 | } |
---|
2438 | |
---|
2439 | |
---|
2440 | extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, |
---|
2441 | dVector3 result) |
---|
2442 | { |
---|
2443 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
2444 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2445 | if (anum < 0) anum = 0; |
---|
2446 | if (anum > 2) anum = 2; |
---|
2447 | if (joint->rel[anum] > 0) { |
---|
2448 | if (joint->rel[anum]==1) { |
---|
2449 | dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]); |
---|
2450 | } |
---|
2451 | else { |
---|
2452 | dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]); |
---|
2453 | } |
---|
2454 | } |
---|
2455 | else { |
---|
2456 | result[0] = joint->axis[anum][0]; |
---|
2457 | result[1] = joint->axis[anum][1]; |
---|
2458 | result[2] = joint->axis[anum][2]; |
---|
2459 | } |
---|
2460 | } |
---|
2461 | |
---|
2462 | |
---|
2463 | extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum) |
---|
2464 | { |
---|
2465 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
2466 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2467 | if (anum < 0) anum = 0; |
---|
2468 | if (anum > 2) anum = 2; |
---|
2469 | return joint->rel[anum]; |
---|
2470 | } |
---|
2471 | |
---|
2472 | |
---|
2473 | extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum) |
---|
2474 | { |
---|
2475 | dAASSERT(joint && anum >= 0 && anum < 3); |
---|
2476 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2477 | if (anum < 0) anum = 0; |
---|
2478 | if (anum > 3) anum = 3; |
---|
2479 | return joint->angle[anum]; |
---|
2480 | } |
---|
2481 | |
---|
2482 | |
---|
2483 | extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum) |
---|
2484 | { |
---|
2485 | // @@@ |
---|
2486 | dDebug (0,"not yet implemented"); |
---|
2487 | return 0; |
---|
2488 | } |
---|
2489 | |
---|
2490 | |
---|
2491 | extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter) |
---|
2492 | { |
---|
2493 | dAASSERT(joint); |
---|
2494 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2495 | int anum = parameter >> 8; |
---|
2496 | if (anum < 0) anum = 0; |
---|
2497 | if (anum > 2) anum = 2; |
---|
2498 | parameter &= 0xff; |
---|
2499 | return joint->limot[anum].get (parameter); |
---|
2500 | } |
---|
2501 | |
---|
2502 | |
---|
2503 | extern "C" int dJointGetAMotorMode (dxJointAMotor *joint) |
---|
2504 | { |
---|
2505 | dAASSERT(joint); |
---|
2506 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2507 | return joint->mode; |
---|
2508 | } |
---|
2509 | |
---|
2510 | |
---|
2511 | extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3) |
---|
2512 | { |
---|
2513 | dVector3 axes[3]; |
---|
2514 | dAASSERT(joint); |
---|
2515 | dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); |
---|
2516 | |
---|
2517 | if (joint->num == 0) |
---|
2518 | return; |
---|
2519 | dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); |
---|
2520 | |
---|
2521 | amotorComputeGlobalAxes (joint,axes); |
---|
2522 | axes[0][0] *= torque1; |
---|
2523 | axes[0][1] *= torque1; |
---|
2524 | axes[0][2] *= torque1; |
---|
2525 | if (joint->num >= 2) { |
---|
2526 | axes[0][0] += axes[1][0] * torque2; |
---|
2527 | axes[0][1] += axes[1][0] * torque2; |
---|
2528 | axes[0][2] += axes[1][0] * torque2; |
---|
2529 | if (joint->num >= 3) { |
---|
2530 | axes[0][0] += axes[2][0] * torque3; |
---|
2531 | axes[0][1] += axes[2][0] * torque3; |
---|
2532 | axes[0][2] += axes[2][0] * torque3; |
---|
2533 | } |
---|
2534 | } |
---|
2535 | |
---|
2536 | if (joint->node[0].body != 0) |
---|
2537 | dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); |
---|
2538 | if (joint->node[1].body != 0) |
---|
2539 | dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); |
---|
2540 | } |
---|
2541 | |
---|
2542 | |
---|
2543 | dxJoint::Vtable __damotor_vtable = { |
---|
2544 | sizeof(dxJointAMotor), |
---|
2545 | (dxJoint::init_fn*) amotorInit, |
---|
2546 | (dxJoint::getInfo1_fn*) amotorGetInfo1, |
---|
2547 | (dxJoint::getInfo2_fn*) amotorGetInfo2, |
---|
2548 | dJointTypeAMotor}; |
---|
2549 | |
---|
2550 | //**************************************************************************** |
---|
2551 | // fixed joint |
---|
2552 | |
---|
2553 | static void fixedInit (dxJointFixed *j) |
---|
2554 | { |
---|
2555 | dSetZero (j->offset,4); |
---|
2556 | dSetZero (j->qrel,4); |
---|
2557 | } |
---|
2558 | |
---|
2559 | |
---|
2560 | static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) |
---|
2561 | { |
---|
2562 | info->m = 6; |
---|
2563 | info->nub = 6; |
---|
2564 | } |
---|
2565 | |
---|
2566 | |
---|
2567 | static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) |
---|
2568 | { |
---|
2569 | int s = info->rowskip; |
---|
2570 | |
---|
2571 | // Three rows for orientation |
---|
2572 | setFixedOrientation(joint, info, joint->qrel, 3); |
---|
2573 | |
---|
2574 | // Three rows for position. |
---|
2575 | // set jacobian |
---|
2576 | info->J1l[0] = 1; |
---|
2577 | info->J1l[s+1] = 1; |
---|
2578 | info->J1l[2*s+2] = 1; |
---|
2579 | |
---|
2580 | dVector3 ofs; |
---|
2581 | dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); |
---|
2582 | if (joint->node[1].body) { |
---|
2583 | dCROSSMAT (info->J1a,ofs,s,+,-); |
---|
2584 | info->J2l[0] = -1; |
---|
2585 | info->J2l[s+1] = -1; |
---|
2586 | info->J2l[2*s+2] = -1; |
---|
2587 | } |
---|
2588 | |
---|
2589 | // set right hand side for the first three rows (linear) |
---|
2590 | dReal k = info->fps * info->erp; |
---|
2591 | if (joint->node[1].body) { |
---|
2592 | for (int j=0; j<3; j++) |
---|
2593 | info->c[j] = k * (joint->node[1].body->pos[j] - |
---|
2594 | joint->node[0].body->pos[j] + ofs[j]); |
---|
2595 | } |
---|
2596 | else { |
---|
2597 | for (int j=0; j<3; j++) |
---|
2598 | info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); |
---|
2599 | } |
---|
2600 | } |
---|
2601 | |
---|
2602 | |
---|
2603 | extern "C" void dJointSetFixed (dxJointFixed *joint) |
---|
2604 | { |
---|
2605 | dUASSERT(joint,"bad joint argument"); |
---|
2606 | dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); |
---|
2607 | int i; |
---|
2608 | |
---|
2609 | // This code is taken from sJointSetSliderAxis(), we should really put the |
---|
2610 | // common code in its own function. |
---|
2611 | // compute the offset between the bodies |
---|
2612 | if (joint->node[0].body) { |
---|
2613 | if (joint->node[1].body) { |
---|
2614 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
---|
2615 | dReal ofs[4]; |
---|
2616 | for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; |
---|
2617 | for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; |
---|
2618 | dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); |
---|
2619 | } |
---|
2620 | else { |
---|
2621 | // set joint->qrel to the transpose of the first body's q |
---|
2622 | joint->qrel[0] = joint->node[0].body->q[0]; |
---|
2623 | for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
---|
2624 | for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; |
---|
2625 | } |
---|
2626 | } |
---|
2627 | } |
---|
2628 | |
---|
2629 | |
---|
2630 | dxJoint::Vtable __dfixed_vtable = { |
---|
2631 | sizeof(dxJointFixed), |
---|
2632 | (dxJoint::init_fn*) fixedInit, |
---|
2633 | (dxJoint::getInfo1_fn*) fixedGetInfo1, |
---|
2634 | (dxJoint::getInfo2_fn*) fixedGetInfo2, |
---|
2635 | dJointTypeFixed}; |
---|
2636 | |
---|
2637 | //**************************************************************************** |
---|
2638 | // null joint |
---|
2639 | |
---|
2640 | static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) |
---|
2641 | { |
---|
2642 | info->m = 0; |
---|
2643 | info->nub = 0; |
---|
2644 | } |
---|
2645 | |
---|
2646 | |
---|
2647 | static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) |
---|
2648 | { |
---|
2649 | dDebug (0,"this should never get called"); |
---|
2650 | } |
---|
2651 | |
---|
2652 | |
---|
2653 | dxJoint::Vtable __dnull_vtable = { |
---|
2654 | sizeof(dxJointNull), |
---|
2655 | (dxJoint::init_fn*) 0, |
---|
2656 | (dxJoint::getInfo1_fn*) nullGetInfo1, |
---|
2657 | (dxJoint::getInfo2_fn*) nullGetInfo2, |
---|
2658 | dJointTypeNull}; |
---|
2659 | |
---|
2660 | /******************** breakable joint contribution ***********************/ |
---|
2661 | extern "C" void dJointSetBreakable (dxJoint *joint, int b) { |
---|
2662 | dAASSERT(joint); |
---|
2663 | if (b) { |
---|
2664 | // we want this joint to be breakable but we must first check if it |
---|
2665 | // was already breakable |
---|
2666 | if (!joint->breakInfo) { |
---|
2667 | // allocate a dxJointBreakInfo struct |
---|
2668 | joint->breakInfo = new dxJointBreakInfo; |
---|
2669 | joint->breakInfo->flags = 0; |
---|
2670 | for (int i = 0; i < 3; i++) { |
---|
2671 | joint->breakInfo->b1MaxF[0] = 0; |
---|
2672 | joint->breakInfo->b1MaxT[0] = 0; |
---|
2673 | joint->breakInfo->b2MaxF[0] = 0; |
---|
2674 | joint->breakInfo->b2MaxT[0] = 0; |
---|
2675 | } |
---|
2676 | joint->breakInfo->callback = 0; |
---|
2677 | } |
---|
2678 | else { |
---|
2679 | // the joint was already breakable |
---|
2680 | return; |
---|
2681 | } |
---|
2682 | } |
---|
2683 | else { |
---|
2684 | // we want this joint to be unbreakable mut we must first check if |
---|
2685 | // it is alreay unbreakable |
---|
2686 | if (joint->breakInfo) { |
---|
2687 | // deallocate the dxJointBreakInfo struct |
---|
2688 | delete joint->breakInfo; |
---|
2689 | joint->breakInfo = 0; |
---|
2690 | } |
---|
2691 | else { |
---|
2692 | // the joint was already unbreakable |
---|
2693 | return; |
---|
2694 | } |
---|
2695 | } |
---|
2696 | } |
---|
2697 | |
---|
2698 | extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) { |
---|
2699 | dAASSERT(joint); |
---|
2700 | # ifndef dNODEBUG |
---|
2701 | // only works for a breakable joint |
---|
2702 | if (!joint->breakInfo) { |
---|
2703 | dDebug (0, "dJointSetBreakCallback called on unbreakable joint"); |
---|
2704 | } |
---|
2705 | # endif |
---|
2706 | joint->breakInfo->callback = callbackFunc; |
---|
2707 | } |
---|
2708 | |
---|
2709 | extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) { |
---|
2710 | dAASSERT(joint); |
---|
2711 | # ifndef dNODEBUG |
---|
2712 | // only works for a breakable joint |
---|
2713 | if (!joint->breakInfo) { |
---|
2714 | dDebug (0, "dJointSetBreakMode called on unbreakable joint"); |
---|
2715 | } |
---|
2716 | # endif |
---|
2717 | joint->breakInfo->flags = mode; |
---|
2718 | } |
---|
2719 | |
---|
2720 | extern "C" int dJointGetBreakMode (dxJoint *joint) { |
---|
2721 | dAASSERT(joint); |
---|
2722 | # ifndef dNODEBUG |
---|
2723 | // only works for a breakable joint |
---|
2724 | if (!joint->breakInfo) { |
---|
2725 | dDebug (0, "dJointGetBreakMode called on unbreakable joint"); |
---|
2726 | } |
---|
2727 | # endif |
---|
2728 | return joint->breakInfo->flags; |
---|
2729 | } |
---|
2730 | |
---|
2731 | extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) { |
---|
2732 | dAASSERT(joint); |
---|
2733 | # ifndef dNODEBUG |
---|
2734 | // only works for a breakable joint |
---|
2735 | if (!joint->breakInfo) { |
---|
2736 | dDebug (0, "dJointSetBreakForce called on unbreakable joint"); |
---|
2737 | } |
---|
2738 | # endif |
---|
2739 | if (body) { |
---|
2740 | joint->breakInfo->b2MaxF[0] = x; |
---|
2741 | joint->breakInfo->b2MaxF[1] = y; |
---|
2742 | joint->breakInfo->b2MaxF[2] = z; |
---|
2743 | } |
---|
2744 | else { |
---|
2745 | joint->breakInfo->b1MaxF[0] = x; |
---|
2746 | joint->breakInfo->b1MaxF[1] = y; |
---|
2747 | joint->breakInfo->b1MaxF[2] = z; |
---|
2748 | } |
---|
2749 | } |
---|
2750 | |
---|
2751 | extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) { |
---|
2752 | dAASSERT(joint); |
---|
2753 | # ifndef dNODEBUG |
---|
2754 | // only works for a breakable joint |
---|
2755 | if (!joint->breakInfo) { |
---|
2756 | dDebug (0, "dJointSetBreakTorque called on unbreakable joint"); |
---|
2757 | } |
---|
2758 | # endif |
---|
2759 | if (body) { |
---|
2760 | joint->breakInfo->b2MaxT[0] = x; |
---|
2761 | joint->breakInfo->b2MaxT[1] = y; |
---|
2762 | joint->breakInfo->b2MaxT[2] = z; |
---|
2763 | } |
---|
2764 | else { |
---|
2765 | joint->breakInfo->b1MaxT[0] = x; |
---|
2766 | joint->breakInfo->b1MaxT[1] = y; |
---|
2767 | joint->breakInfo->b1MaxT[2] = z; |
---|
2768 | } |
---|
2769 | } |
---|
2770 | |
---|
2771 | extern "C" int dJointIsBreakable (dxJoint *joint) { |
---|
2772 | dAASSERT(joint); |
---|
2773 | return joint->breakInfo != 0; |
---|
2774 | } |
---|
2775 | |
---|
2776 | extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) { |
---|
2777 | dAASSERT(joint); |
---|
2778 | # ifndef dNODEBUG |
---|
2779 | // only works for a breakable joint |
---|
2780 | if (!joint->breakInfo) { |
---|
2781 | dDebug (0, "dJointGetBreakForce called on unbreakable joint"); |
---|
2782 | } |
---|
2783 | # endif |
---|
2784 | if (body) |
---|
2785 | for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i]; |
---|
2786 | else |
---|
2787 | for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i]; |
---|
2788 | } |
---|
2789 | |
---|
2790 | extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) { |
---|
2791 | dAASSERT(joint); |
---|
2792 | # ifndef dNODEBUG |
---|
2793 | // only works for a breakable joint |
---|
2794 | if (!joint->breakInfo) { |
---|
2795 | dDebug (0, "dJointGetBreakTorque called on unbreakable joint"); |
---|
2796 | } |
---|
2797 | # endif |
---|
2798 | if (body) |
---|
2799 | for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i]; |
---|
2800 | else |
---|
2801 | for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i]; |
---|
2802 | } |
---|
2803 | /*************************************************************************/ |
---|