|
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/ode.h> |
|
33 #include <ode/odemath.h> |
|
34 #include <ode/rotation.h> |
|
35 #include <ode/matrix.h> |
|
36 #include "joint.h" |
|
37 |
|
38 #define dCROSSMUL(a,op,b,c) \ |
|
39 do { \ |
|
40 (a)[0] op dMUL(REAL(0.5),(dMUL((b)[1],(c)[2]) - dMUL((b)[2],(c)[1]))); \ |
|
41 (a)[1] op dMUL(REAL(0.5),(dMUL((b)[2],(c)[0]) - dMUL((b)[0],(c)[2]))); \ |
|
42 (a)[2] op dMUL(REAL(0.5),(dMUL((b)[0],(c)[1]) - dMUL((b)[1],(c)[0]))); \ |
|
43 } while(0) |
|
44 |
|
45 //**************************************************************************** |
|
46 // externs |
|
47 |
|
48 // extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); |
|
49 // extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); |
|
50 |
|
51 //**************************************************************************** |
|
52 // utility |
|
53 |
|
54 // set three "ball-and-socket" rows in the constraint equation, and the |
|
55 // corresponding right hand side. |
|
56 |
|
57 static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, |
|
58 dVector3 anchor1, dVector3 anchor2) |
|
59 { |
|
60 // anchor points in global coordinates with respect to body PORs. |
|
61 dVector3 a1,a2; |
|
62 |
|
63 int s = info->rowskip; |
|
64 |
|
65 // set jacobian |
|
66 info->J1l[0] = REAL(1.0); |
|
67 info->J1l[s+1] = REAL(1.0); |
|
68 info->J1l[2*s+2] = REAL(1.0); |
|
69 dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); |
|
70 dCROSSMAT (info->J1a,a1,s,-,+); |
|
71 if (joint->node[1].body) { |
|
72 info->J2l[0] = -REAL(1.0); |
|
73 info->J2l[s+1] = -REAL(1.0); |
|
74 info->J2l[2*s+2] = -REAL(1.0); |
|
75 dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); |
|
76 dCROSSMAT (info->J2a,a2,s,+,-); |
|
77 } |
|
78 |
|
79 // set right hand side |
|
80 dReal k = dMUL(info->fps,info->erp); |
|
81 if (joint->node[1].body) { |
|
82 for (int j=0; j<3; j++) { |
|
83 info->c[j] = dMUL(k,(a2[j] + joint->node[1].body->posr.pos[j] - |
|
84 a1[j] - joint->node[0].body->posr.pos[j])); |
|
85 } |
|
86 } |
|
87 else { |
|
88 for (int j=0; j<3; j++) { |
|
89 info->c[j] = dMUL(k,(anchor2[j] - a1[j] - |
|
90 joint->node[0].body->posr.pos[j])); |
|
91 } |
|
92 } |
|
93 } |
|
94 |
|
95 |
|
96 // this is like setBall(), except that `axis' is a unit length vector |
|
97 // (in global coordinates) that should be used for the first jacobian |
|
98 // position row (the other two row vectors will be derived from this). |
|
99 // `erp1' is the erp value to use along the axis. |
|
100 |
|
101 static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, |
|
102 dVector3 anchor1, dVector3 anchor2, |
|
103 dVector3 axis, dReal erp1) |
|
104 { |
|
105 // anchor points in global coordinates with respect to body PORs. |
|
106 dVector3 a1,a2; |
|
107 |
|
108 int i,s = info->rowskip; |
|
109 |
|
110 // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], |
|
111 // [0 1 0] and [0 0 1], which makes everything much easier. |
|
112 dVector3 q1,q2; |
|
113 dPlaneSpace (axis,q1,q2); |
|
114 |
|
115 // set jacobian |
|
116 for (i=0; i<3; i++) info->J1l[i] = axis[i]; |
|
117 for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; |
|
118 for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; |
|
119 dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); |
|
120 dCROSS (info->J1a,=,a1,axis); |
|
121 dCROSS (info->J1a+s,=,a1,q1); |
|
122 dCROSS (info->J1a+2*s,=,a1,q2); |
|
123 if (joint->node[1].body) { |
|
124 for (i=0; i<3; i++) info->J2l[i] = -axis[i]; |
|
125 for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; |
|
126 for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; |
|
127 dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); |
|
128 dCROSS (info->J2a,= -,a2,axis); |
|
129 dCROSS (info->J2a+s,= -,a2,q1); |
|
130 dCROSS (info->J2a+2*s,= -,a2,q2); |
|
131 } |
|
132 |
|
133 // set right hand side - measure error along (axis,q1,q2) |
|
134 dReal k1 = dMUL(info->fps,erp1); |
|
135 dReal k = dMUL(info->fps,info->erp); |
|
136 |
|
137 for (i=0; i<3; i++) a1[i] += joint->node[0].body->posr.pos[i]; |
|
138 if (joint->node[1].body) { |
|
139 for (i=0; i<3; i++) a2[i] += joint->node[1].body->posr.pos[i]; |
|
140 info->c[0] = dMUL(k1,(dDOT(axis,a2) - dDOT(axis,a1))); |
|
141 info->c[1] = dMUL(k,(dDOT(q1,a2) - dDOT(q1,a1))); |
|
142 info->c[2] = dMUL(k,(dDOT(q2,a2) - dDOT(q2,a1))); |
|
143 } |
|
144 else { |
|
145 info->c[0] = dMUL(k1,(dDOT(axis,anchor2) - dDOT(axis,a1))); |
|
146 info->c[1] = dMUL(k,(dDOT(q1,anchor2) - dDOT(q1,a1))); |
|
147 info->c[2] = dMUL(k,(dDOT(q2,anchor2) - dDOT(q2,a1))); |
|
148 } |
|
149 } |
|
150 |
|
151 |
|
152 // set three orientation rows in the constraint equation, and the |
|
153 // corresponding right hand side. |
|
154 |
|
155 static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) |
|
156 { |
|
157 int s = info->rowskip; |
|
158 int start_index = start_row * s; |
|
159 |
|
160 // 3 rows to make body rotations equal |
|
161 info->J1a[start_index] = REAL(1.0); |
|
162 info->J1a[start_index + s + 1] = REAL(1.0); |
|
163 info->J1a[start_index + s*2+2] = REAL(1.0); |
|
164 if (joint->node[1].body) { |
|
165 info->J2a[start_index] = REAL(-1.0); |
|
166 info->J2a[start_index + s+1] = REAL(-1.0); |
|
167 info->J2a[start_index + s*2+2] = REAL(-1.0); |
|
168 } |
|
169 |
|
170 // compute the right hand side. the first three elements will result in |
|
171 // relative angular velocity of the two bodies - this is set to bring them |
|
172 // back into alignment. the correcting angular velocity is |
|
173 // |angular_velocity| = angle/time = erp*theta / stepsize |
|
174 // = (erp*fps) * theta |
|
175 // angular_velocity = |angular_velocity| * u |
|
176 // = (erp*fps) * theta * u |
|
177 // where rotation along unit length axis u by theta brings body 2's frame |
|
178 // to qrel with respect to body 1's frame. using a small angle approximation |
|
179 // for sin(), this gives |
|
180 // angular_velocity = (erp*fps) * 2 * v |
|
181 // where the quaternion of the relative rotation between the two bodies is |
|
182 // q = [cos(theta/2) sin(theta/2)*u] = [s v] |
|
183 |
|
184 // get qerr = relative rotation (rotation error) between two bodies |
|
185 dQuaternion qerr,e; |
|
186 if (joint->node[1].body) { |
|
187 dQuaternion qq; |
|
188 dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); |
|
189 dQMultiply2 (qerr,qq,qrel); |
|
190 } |
|
191 else { |
|
192 dQMultiply3 (qerr,joint->node[0].body->q,qrel); |
|
193 } |
|
194 if (qerr[0] < 0) { |
|
195 qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small |
|
196 qerr[2] = -qerr[2]; |
|
197 qerr[3] = -qerr[3]; |
|
198 } |
|
199 dMULTIPLY0_331 (e,joint->node[0].body->posr.R,qerr+1); // @@@ bad SIMD padding! |
|
200 dReal k = dMUL(info->fps,info->erp); |
|
201 info->c[start_row] = 2*dMUL(k,e[0]); |
|
202 info->c[start_row+1] = 2*dMUL(k,e[1]); |
|
203 info->c[start_row+2] = 2*dMUL(k,e[2]); |
|
204 } |
|
205 |
|
206 |
|
207 // compute anchor points relative to bodies |
|
208 |
|
209 static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, |
|
210 dVector3 anchor1, dVector3 anchor2) |
|
211 { |
|
212 if (j->node[0].body) { |
|
213 dReal q[4]; |
|
214 q[0] = x - j->node[0].body->posr.pos[0]; |
|
215 q[1] = y - j->node[0].body->posr.pos[1]; |
|
216 q[2] = z - j->node[0].body->posr.pos[2]; |
|
217 q[3] = 0; |
|
218 dMULTIPLY1_331 (anchor1,j->node[0].body->posr.R,q); |
|
219 if (j->node[1].body) { |
|
220 q[0] = x - j->node[1].body->posr.pos[0]; |
|
221 q[1] = y - j->node[1].body->posr.pos[1]; |
|
222 q[2] = z - j->node[1].body->posr.pos[2]; |
|
223 q[3] = 0; |
|
224 dMULTIPLY1_331 (anchor2,j->node[1].body->posr.R,q); |
|
225 } |
|
226 else { |
|
227 anchor2[0] = x; |
|
228 anchor2[1] = y; |
|
229 anchor2[2] = z; |
|
230 } |
|
231 } |
|
232 anchor1[3] = 0; |
|
233 anchor2[3] = 0; |
|
234 } |
|
235 |
|
236 |
|
237 // compute axes relative to bodies. either axis1 or axis2 can be 0. |
|
238 |
|
239 static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, |
|
240 dVector3 axis1, dVector3 axis2) |
|
241 { |
|
242 if (j->node[0].body) { |
|
243 dReal q[4]; |
|
244 q[0] = x; |
|
245 q[1] = y; |
|
246 q[2] = z; |
|
247 q[3] = 0; |
|
248 dNormalize3 (q); |
|
249 if (axis1) { |
|
250 dMULTIPLY1_331 (axis1,j->node[0].body->posr.R,q); |
|
251 axis1[3] = 0; |
|
252 } |
|
253 if (axis2) { |
|
254 if (j->node[1].body) { |
|
255 dMULTIPLY1_331 (axis2,j->node[1].body->posr.R,q); |
|
256 } |
|
257 else { |
|
258 axis2[0] = x; |
|
259 axis2[1] = y; |
|
260 axis2[2] = z; |
|
261 } |
|
262 axis2[3] = 0; |
|
263 } |
|
264 } |
|
265 } |
|
266 |
|
267 |
|
268 static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) |
|
269 { |
|
270 if (j->node[0].body) { |
|
271 dMULTIPLY0_331 (result,j->node[0].body->posr.R,anchor1); |
|
272 result[0] += j->node[0].body->posr.pos[0]; |
|
273 result[1] += j->node[0].body->posr.pos[1]; |
|
274 result[2] += j->node[0].body->posr.pos[2]; |
|
275 } |
|
276 } |
|
277 |
|
278 |
|
279 static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) |
|
280 { |
|
281 if (j->node[1].body) { |
|
282 dMULTIPLY0_331 (result,j->node[1].body->posr.R,anchor2); |
|
283 result[0] += j->node[1].body->posr.pos[0]; |
|
284 result[1] += j->node[1].body->posr.pos[1]; |
|
285 result[2] += j->node[1].body->posr.pos[2]; |
|
286 } |
|
287 else { |
|
288 result[0] = anchor2[0]; |
|
289 result[1] = anchor2[1]; |
|
290 result[2] = anchor2[2]; |
|
291 } |
|
292 } |
|
293 |
|
294 |
|
295 static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) |
|
296 { |
|
297 if (j->node[0].body) { |
|
298 dMULTIPLY0_331 (result,j->node[0].body->posr.R,axis1); |
|
299 } |
|
300 } |
|
301 |
|
302 |
|
303 static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) |
|
304 { |
|
305 if (j->node[1].body) { |
|
306 dMULTIPLY0_331 (result,j->node[1].body->posr.R,axis2); |
|
307 } |
|
308 else { |
|
309 result[0] = axis2[0]; |
|
310 result[1] = axis2[1]; |
|
311 result[2] = axis2[2]; |
|
312 } |
|
313 } |
|
314 |
|
315 |
|
316 static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) |
|
317 { |
|
318 // the angle between the two bodies is extracted from the quaternion that |
|
319 // represents the relative rotation between them. recall that a quaternion |
|
320 // q is: |
|
321 // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] |
|
322 // where s is a scalar and v is a 3-vector. u is a unit length axis and |
|
323 // theta is a rotation along that axis. we can get theta/2 by: |
|
324 // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) |
|
325 // but we can't get sin(theta/2) directly, only its absolute value, i.e.: |
|
326 // |v| = |sin(theta/2)| * |u| |
|
327 // = |sin(theta/2)| |
|
328 // using this value will have a strange effect. recall that there are two |
|
329 // quaternion representations of a given rotation, q and -q. typically as |
|
330 // a body rotates along the axis it will go through a complete cycle using |
|
331 // one representation and then the next cycle will use the other |
|
332 // representation. this corresponds to u pointing in the direction of the |
|
333 // hinge axis and then in the opposite direction. the result is that theta |
|
334 // will appear to go "backwards" every other cycle. here is a fix: if u |
|
335 // points "away" from the direction of the hinge (motor) axis (i.e. more |
|
336 // than 90 degrees) then use -q instead of q. this represents the same |
|
337 // rotation, but results in the cos(theta/2) value being sign inverted. |
|
338 |
|
339 // extract the angle from the quaternion. cost2 = cos(theta/2), |
|
340 // sint2 = |sin(theta/2)| |
|
341 dReal cost2 = qrel[0]; |
|
342 dReal sint2 = dSqrt (dMUL(qrel[1],qrel[1])+dMUL(qrel[2],qrel[2])+dMUL(qrel[3],qrel[3])); |
|
343 dReal theta = (dDOT(qrel+REAL(1.0),axis) >= 0) ? // @@@ padding assumptions |
|
344 (2 * dArcTan2(sint2,cost2)) : // if u points in direction of axis |
|
345 (2 * dArcTan2(sint2,-cost2)); // if u points in opposite direction |
|
346 |
|
347 // the angle we get will be between 0..2*pi, but we want to return angles |
|
348 // between -pi..pi |
|
349 if (theta > dPI) theta -= 2*dPI; |
|
350 |
|
351 // the angle we've just extracted has the wrong sign |
|
352 theta = -theta; |
|
353 |
|
354 return theta; |
|
355 } |
|
356 |
|
357 |
|
358 // given two bodies (body1,body2), the hinge axis that they are connected by |
|
359 // w.r.t. body1 (axis), and the initial relative orientation between them |
|
360 // (q_initial), return the relative rotation angle. the initial relative |
|
361 // orientation corresponds to an angle of zero. if body2 is 0 then measure the |
|
362 // angle between body1 and the static frame. |
|
363 // |
|
364 // this will not return the correct angle if the bodies rotate along any axis |
|
365 // other than the given hinge axis. |
|
366 |
|
367 static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, |
|
368 dQuaternion q_initial) |
|
369 { |
|
370 // get qrel = relative rotation between the two bodies |
|
371 dQuaternion qrel; |
|
372 if (body2) { |
|
373 dQuaternion qq; |
|
374 dQMultiply1 (qq,body1->q,body2->q); |
|
375 dQMultiply2 (qrel,qq,q_initial); |
|
376 } |
|
377 else { |
|
378 // pretend body2->q is the identity |
|
379 dQMultiply3 (qrel,body1->q,q_initial); |
|
380 } |
|
381 |
|
382 return getHingeAngleFromRelativeQuat (qrel,axis); |
|
383 } |
|
384 |
|
385 //**************************************************************************** |
|
386 // dxJointLimitMotor |
|
387 |
|
388 void dxJointLimitMotor::init (dxWorld *world) |
|
389 { |
|
390 vel = 0; |
|
391 fmax = 0; |
|
392 lostop = -dInfinity; |
|
393 histop = dInfinity; |
|
394 fudge_factor = REAL(1.0); |
|
395 normal_cfm = world->global_cfm; |
|
396 stop_erp = world->global_erp; |
|
397 stop_cfm = world->global_cfm; |
|
398 bounce = 0; |
|
399 limit = 0; |
|
400 limit_err = 0; |
|
401 } |
|
402 |
|
403 |
|
404 void dxJointLimitMotor::set (int num, dReal value) |
|
405 { |
|
406 switch (num) { |
|
407 case dParamLoStop: |
|
408 lostop = value; |
|
409 break; |
|
410 case dParamHiStop: |
|
411 histop = value; |
|
412 break; |
|
413 case dParamVel: |
|
414 vel = value; |
|
415 break; |
|
416 case dParamFMax: |
|
417 if (value >= 0) fmax = value; |
|
418 break; |
|
419 case dParamFudgeFactor: |
|
420 if (value >= 0 && value <= REAL(1.0)) fudge_factor = value; |
|
421 break; |
|
422 case dParamBounce: |
|
423 bounce = value; |
|
424 break; |
|
425 case dParamCFM: |
|
426 normal_cfm = value; |
|
427 break; |
|
428 case dParamStopERP: |
|
429 stop_erp = value; |
|
430 break; |
|
431 case dParamStopCFM: |
|
432 stop_cfm = value; |
|
433 break; |
|
434 } |
|
435 } |
|
436 |
|
437 |
|
438 dReal dxJointLimitMotor::get (int num) |
|
439 { |
|
440 switch (num) { |
|
441 case dParamLoStop: return lostop; |
|
442 case dParamHiStop: return histop; |
|
443 case dParamVel: return vel; |
|
444 case dParamFMax: return fmax; |
|
445 case dParamFudgeFactor: return fudge_factor; |
|
446 case dParamBounce: return bounce; |
|
447 case dParamCFM: return normal_cfm; |
|
448 case dParamStopERP: return stop_erp; |
|
449 case dParamStopCFM: return stop_cfm; |
|
450 default: return 0; |
|
451 } |
|
452 } |
|
453 |
|
454 |
|
455 int dxJointLimitMotor::testRotationalLimit (dReal angle) |
|
456 { |
|
457 if (angle <= lostop) { |
|
458 limit = 1; |
|
459 limit_err = angle - lostop; |
|
460 return 1; |
|
461 } |
|
462 else if (angle >= histop) { |
|
463 limit = 2; |
|
464 limit_err = angle - histop; |
|
465 return 1; |
|
466 } |
|
467 else { |
|
468 limit = 0; |
|
469 return 0; |
|
470 } |
|
471 } |
|
472 |
|
473 |
|
474 int dxJointLimitMotor::addLimot (dxJoint *joint, |
|
475 dxJoint::Info2 *info, int row, |
|
476 const dVector3 ax1, int rotational) |
|
477 { |
|
478 int srow = row * info->rowskip; |
|
479 |
|
480 // if the joint is powered, or has joint limits, add in the extra row |
|
481 int powered = fmax > 0; |
|
482 if (powered || limit) { |
|
483 dReal *J1 = rotational ? info->J1a : info->J1l; |
|
484 dReal *J2 = rotational ? info->J2a : info->J2l; |
|
485 |
|
486 J1[srow+0] = ax1[0]; |
|
487 J1[srow+1] = ax1[1]; |
|
488 J1[srow+2] = ax1[2]; |
|
489 if (joint->node[1].body) { |
|
490 J2[srow+0] = -ax1[0]; |
|
491 J2[srow+1] = -ax1[1]; |
|
492 J2[srow+2] = -ax1[2]; |
|
493 } |
|
494 |
|
495 // linear limot torque decoupling step: |
|
496 // |
|
497 // if this is a linear limot (e.g. from a slider), we have to be careful |
|
498 // that the linear constraint forces (+/- ax1) applied to the two bodies |
|
499 // do not create a torque couple. in other words, the points that the |
|
500 // constraint force is applied at must lie along the same ax1 axis. |
|
501 // a torque couple will result in powered or limited slider-jointed free |
|
502 // bodies from gaining angular momentum. |
|
503 // the solution used here is to apply the constraint forces at the point |
|
504 // halfway between the body centers. there is no penalty (other than an |
|
505 // extra tiny bit of computation) in doing this adjustment. note that we |
|
506 // only need to do this if the constraint connects two bodies. |
|
507 |
|
508 dVector3 ltd = {}; // Linear Torque Decoupling vector (a torque) |
|
509 if (!rotational && joint->node[1].body) { |
|
510 dVector3 c; |
|
511 c[0]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0])); |
|
512 c[1]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1])); |
|
513 c[2]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2])); |
|
514 dCROSS (ltd,=,c,ax1); |
|
515 info->J1a[srow+0] = ltd[0]; |
|
516 info->J1a[srow+1] = ltd[1]; |
|
517 info->J1a[srow+2] = ltd[2]; |
|
518 info->J2a[srow+0] = ltd[0]; |
|
519 info->J2a[srow+1] = ltd[1]; |
|
520 info->J2a[srow+2] = ltd[2]; |
|
521 } |
|
522 |
|
523 // if we're limited low and high simultaneously, the joint motor is |
|
524 // ineffective |
|
525 if (limit && (lostop == histop)) powered = 0; |
|
526 |
|
527 if (powered) { |
|
528 info->cfm[row] = normal_cfm; |
|
529 if (! limit) { |
|
530 info->c[row] = vel; |
|
531 info->lo[row] = -fmax; |
|
532 info->hi[row] = fmax; |
|
533 } |
|
534 else { |
|
535 // the joint is at a limit, AND is being powered. if the joint is |
|
536 // being powered into the limit then we apply the maximum motor force |
|
537 // in that direction, because the motor is working against the |
|
538 // immovable limit. if the joint is being powered away from the limit |
|
539 // then we have problems because actually we need *two* lcp |
|
540 // constraints to handle this case. so we fake it and apply some |
|
541 // fraction of the maximum force. the fraction to use can be set as |
|
542 // a fudge factor. |
|
543 |
|
544 dReal fm = fmax; |
|
545 if ((vel > 0) || (vel==0 && limit==2)) fm = -fm; |
|
546 |
|
547 // if we're powering away from the limit, apply the fudge factor |
|
548 if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; |
|
549 |
|
550 if (rotational) { |
|
551 dBodyAddTorque (joint->node[0].body,-dMUL(fm,ax1[0]),-dMUL(fm,ax1[1]), |
|
552 -dMUL(fm,ax1[2])); |
|
553 if (joint->node[1].body) |
|
554 dBodyAddTorque (joint->node[1].body,dMUL(fm,ax1[0]),dMUL(fm,ax1[1]),dMUL(fm,ax1[2])); |
|
555 } |
|
556 else { |
|
557 dBodyAddForce (joint->node[0].body,-dMUL(fm,ax1[0]),-dMUL(fm,ax1[1]),-dMUL(fm,ax1[2])); |
|
558 if (joint->node[1].body) { |
|
559 dBodyAddForce (joint->node[1].body,dMUL(fm,ax1[0]),dMUL(fm,ax1[1]),dMUL(fm,ax1[2])); |
|
560 |
|
561 // linear limot torque decoupling step: refer to above discussion |
|
562 dBodyAddTorque (joint->node[0].body,-dMUL(fm,ltd[0]),-dMUL(fm,ltd[1]), |
|
563 -dMUL(fm,ltd[2])); |
|
564 dBodyAddTorque (joint->node[1].body,-dMUL(fm,ltd[0]),-dMUL(fm,ltd[1]), |
|
565 -dMUL(fm,ltd[2])); |
|
566 } |
|
567 } |
|
568 } |
|
569 } |
|
570 |
|
571 if (limit) { |
|
572 dReal k = dMUL(info->fps,stop_erp); |
|
573 info->c[row] = -dMUL(k,limit_err); |
|
574 info->cfm[row] = stop_cfm; |
|
575 |
|
576 if (lostop == histop) { |
|
577 // limited low and high simultaneously |
|
578 info->lo[row] = -dInfinity; |
|
579 info->hi[row] = dInfinity; |
|
580 } |
|
581 else { |
|
582 if (limit == 1) { |
|
583 // low limit |
|
584 info->lo[row] = REAL(0.0); |
|
585 info->hi[row] = dInfinity; |
|
586 } |
|
587 else { |
|
588 // high limit |
|
589 info->lo[row] = -dInfinity; |
|
590 info->hi[row] = REAL(0.0); |
|
591 } |
|
592 |
|
593 // deal with bounce |
|
594 if (bounce > 0) { |
|
595 // calculate joint velocity |
|
596 dReal vel; |
|
597 if (rotational) { |
|
598 vel = dDOT(joint->node[0].body->avel,ax1); |
|
599 if (joint->node[1].body) |
|
600 vel -= dDOT(joint->node[1].body->avel,ax1); |
|
601 } |
|
602 else { |
|
603 vel = dDOT(joint->node[0].body->lvel,ax1); |
|
604 if (joint->node[1].body) |
|
605 vel -= dDOT(joint->node[1].body->lvel,ax1); |
|
606 } |
|
607 |
|
608 // only apply bounce if the velocity is incoming, and if the |
|
609 // resulting c[] exceeds what we already have. |
|
610 if (limit == 1) { |
|
611 // low limit |
|
612 if (vel < 0) { |
|
613 dReal newc = -dMUL(bounce,vel); |
|
614 if (newc > info->c[row]) info->c[row] = newc; |
|
615 } |
|
616 } |
|
617 else { |
|
618 // high limit - all those computations are reversed |
|
619 if (vel > 0) { |
|
620 dReal newc = -dMUL(bounce,vel); |
|
621 if (newc < info->c[row]) info->c[row] = newc; |
|
622 } |
|
623 } |
|
624 } |
|
625 } |
|
626 } |
|
627 return 1; |
|
628 } |
|
629 else return 0; |
|
630 } |
|
631 |
|
632 //**************************************************************************** |
|
633 // ball and socket |
|
634 |
|
635 static void ballInit (dxJointBall *j) |
|
636 { |
|
637 dSetZero (j->anchor1,4); |
|
638 dSetZero (j->anchor2,4); |
|
639 } |
|
640 |
|
641 |
|
642 static void ballGetInfo1 (dxJointBall */*j*/, dxJoint::Info1 *info) |
|
643 { |
|
644 info->m = 3; |
|
645 info->nub = 3; |
|
646 } |
|
647 |
|
648 |
|
649 static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) |
|
650 { |
|
651 setBall (joint,info,joint->anchor1,joint->anchor2); |
|
652 } |
|
653 |
|
654 |
|
655 EXPORT_C void dJointSetBallAnchor (dJointID j, dReal x, dReal y, dReal z) |
|
656 { |
|
657 dxJointBall* joint = (dxJointBall*)j; |
|
658 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
|
659 } |
|
660 |
|
661 |
|
662 EXPORT_C void dJointSetBallAnchor2 (dJointID j, dReal x, dReal y, dReal z) |
|
663 { |
|
664 dxJointBall* joint = (dxJointBall*)j; |
|
665 joint->anchor2[0] = x; |
|
666 joint->anchor2[1] = y; |
|
667 joint->anchor2[2] = z; |
|
668 joint->anchor2[3] = 0; |
|
669 |
|
670 } |
|
671 |
|
672 EXPORT_C void dJointGetBallAnchor (dJointID j, dVector3 result) |
|
673 { |
|
674 dxJointBall* joint = (dxJointBall*)j; |
|
675 if (joint->flags & dJOINT_REVERSE) |
|
676 getAnchor2 (joint,result,joint->anchor2); |
|
677 else |
|
678 getAnchor (joint,result,joint->anchor1); |
|
679 } |
|
680 |
|
681 |
|
682 EXPORT_C void dJointGetBallAnchor2 (dJointID j, dVector3 result) |
|
683 { |
|
684 dxJointBall* joint = (dxJointBall*)j; |
|
685 if (joint->flags & dJOINT_REVERSE) |
|
686 getAnchor (joint,result,joint->anchor1); |
|
687 else |
|
688 getAnchor2 (joint,result,joint->anchor2); |
|
689 } |
|
690 |
|
691 |
|
692 dxJoint::Vtable __dball_vtable = { |
|
693 sizeof(dxJointBall), |
|
694 (dxJoint::init_fn*) ballInit, |
|
695 (dxJoint::getInfo1_fn*) ballGetInfo1, |
|
696 (dxJoint::getInfo2_fn*) ballGetInfo2, |
|
697 dJointTypeBall}; |
|
698 |
|
699 //**************************************************************************** |
|
700 // hinge |
|
701 |
|
702 static void hingeInit (dxJointHinge *j) |
|
703 { |
|
704 dSetZero (j->anchor1,4); |
|
705 dSetZero (j->anchor2,4); |
|
706 dSetZero (j->axis1,4); |
|
707 j->axis1[0] = REAL(1.0); |
|
708 dSetZero (j->axis2,4); |
|
709 j->axis2[0] = REAL(1.0); |
|
710 dSetZero (j->qrel,4); |
|
711 j->limot.init (j->world); |
|
712 } |
|
713 |
|
714 |
|
715 static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) |
|
716 { |
|
717 info->nub = 5; |
|
718 |
|
719 // see if joint is powered |
|
720 if (j->limot.fmax > 0) |
|
721 info->m = 6; // powered hinge needs an extra constraint row |
|
722 else info->m = 5; |
|
723 |
|
724 // see if we're at a joint limit. |
|
725 if ((j->limot.lostop >= -dPI || j->limot.histop <= dPI) && |
|
726 j->limot.lostop <= j->limot.histop) { |
|
727 dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, |
|
728 j->qrel); |
|
729 if (j->limot.testRotationalLimit (angle)) info->m = 6; |
|
730 } |
|
731 } |
|
732 |
|
733 |
|
734 static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) |
|
735 { |
|
736 // set the three ball-and-socket rows |
|
737 setBall (joint,info,joint->anchor1,joint->anchor2); |
|
738 |
|
739 // set the two hinge rows. the hinge axis should be the only unconstrained |
|
740 // rotational axis, the angular velocity of the two bodies perpendicular to |
|
741 // the hinge axis should be equal. thus the constraint equations are |
|
742 // p*w1 - p*w2 = 0 |
|
743 // q*w1 - q*w2 = 0 |
|
744 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 |
|
745 // are the angular velocity vectors of the two bodies. |
|
746 |
|
747 dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body |
|
748 dVector3 p,q; // plane space vectors for ax1 |
|
749 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
|
750 dPlaneSpace (ax1,p,q); |
|
751 |
|
752 int s3=3*info->rowskip; |
|
753 int s4=4*info->rowskip; |
|
754 |
|
755 info->J1a[s3+0] = p[0]; |
|
756 info->J1a[s3+1] = p[1]; |
|
757 info->J1a[s3+2] = p[2]; |
|
758 info->J1a[s4+0] = q[0]; |
|
759 info->J1a[s4+1] = q[1]; |
|
760 info->J1a[s4+2] = q[2]; |
|
761 |
|
762 if (joint->node[1].body) { |
|
763 info->J2a[s3+0] = -p[0]; |
|
764 info->J2a[s3+1] = -p[1]; |
|
765 info->J2a[s3+2] = -p[2]; |
|
766 info->J2a[s4+0] = -q[0]; |
|
767 info->J2a[s4+1] = -q[1]; |
|
768 info->J2a[s4+2] = -q[2]; |
|
769 } |
|
770 |
|
771 // compute the right hand side of the constraint equation. set relative |
|
772 // body velocities along p and q to bring the hinge back into alignment. |
|
773 // if ax1,ax2 are the unit length hinge axes as computed from body1 and |
|
774 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). |
|
775 // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
|
776 // along u to cover angle erp*theta in one step : |
|
777 // |angular_velocity| = angle/time = erp*theta / stepsize |
|
778 // = (erp*fps) * theta |
|
779 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
|
780 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
|
781 // ...as ax1 and ax2 are unit length. if theta is smallish, |
|
782 // theta ~= sin(theta), so |
|
783 // angular_velocity = (erp*fps) * (ax1 x ax2) |
|
784 // ax1 x ax2 is in the plane space of ax1, so we project the angular |
|
785 // velocity to p and q to find the right hand side. |
|
786 |
|
787 dVector3 ax2,b; |
|
788 if (joint->node[1].body) { |
|
789 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); |
|
790 } |
|
791 else { |
|
792 ax2[0] = joint->axis2[0]; |
|
793 ax2[1] = joint->axis2[1]; |
|
794 ax2[2] = joint->axis2[2]; |
|
795 } |
|
796 dCROSS (b,=,ax1,ax2); |
|
797 dReal k = dMUL(info->fps,info->erp); |
|
798 info->c[3] = dMUL(k,dDOT(b,p)); |
|
799 info->c[4] = dMUL(k,dDOT(b,q)); |
|
800 |
|
801 // if the hinge is powered, or has joint limits, add in the stuff |
|
802 joint->limot.addLimot (joint,info,5,ax1,1); |
|
803 } |
|
804 |
|
805 |
|
806 // compute initial relative rotation body1 -> body2, or env -> body1 |
|
807 |
|
808 static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) |
|
809 { |
|
810 if (joint->node[0].body) { |
|
811 if (joint->node[1].body) { |
|
812 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
|
813 } |
|
814 else { |
|
815 // set joint->qrel to the transpose of the first body q |
|
816 joint->qrel[0] = joint->node[0].body->q[0]; |
|
817 for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
|
818 } |
|
819 } |
|
820 } |
|
821 |
|
822 |
|
823 EXPORT_C void dJointSetHingeAnchor (dJointID j, dReal x, dReal y, dReal z) |
|
824 { |
|
825 dxJointHinge* joint = (dxJointHinge*)j; |
|
826 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
|
827 hingeComputeInitialRelativeRotation (joint); |
|
828 } |
|
829 |
|
830 |
|
831 EXPORT_C void dJointSetHingeAnchorDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz) |
|
832 { |
|
833 dxJointHinge* joint = (dxJointHinge*)j; |
|
834 |
|
835 if (joint->node[0].body) { |
|
836 dReal q[4]; |
|
837 q[0] = x - joint->node[0].body->posr.pos[0]; |
|
838 q[1] = y - joint->node[0].body->posr.pos[1]; |
|
839 q[2] = z - joint->node[0].body->posr.pos[2]; |
|
840 q[3] = REAL(0.0); |
|
841 dMULTIPLY1_331 (joint->anchor1,joint->node[0].body->posr.R,q); |
|
842 |
|
843 if (joint->node[1].body) { |
|
844 q[0] = x - joint->node[1].body->posr.pos[0]; |
|
845 q[1] = y - joint->node[1].body->posr.pos[1]; |
|
846 q[2] = z - joint->node[1].body->posr.pos[2]; |
|
847 q[3] = 0; |
|
848 dMULTIPLY1_331 (joint->anchor2,joint->node[1].body->posr.R,q); |
|
849 } |
|
850 else { |
|
851 // Move the relative displacement between the passive body and the |
|
852 // anchor in the same direction as the passive body has just moved |
|
853 joint->anchor2[0] = x + dx; |
|
854 joint->anchor2[1] = y + dy; |
|
855 joint->anchor2[2] = z + dz; |
|
856 } |
|
857 } |
|
858 joint->anchor1[3] = 0; |
|
859 joint->anchor2[3] = 0; |
|
860 |
|
861 hingeComputeInitialRelativeRotation (joint); |
|
862 } |
|
863 |
|
864 |
|
865 |
|
866 EXPORT_C void dJointSetHingeAxis (dJointID j, dReal x, dReal y, dReal z) |
|
867 { |
|
868 dxJointHinge* joint = (dxJointHinge*)j; |
|
869 |
|
870 setAxes (joint,x,y,z,joint->axis1,joint->axis2); |
|
871 hingeComputeInitialRelativeRotation (joint); |
|
872 } |
|
873 |
|
874 |
|
875 EXPORT_C void dJointGetHingeAnchor (dJointID j, dVector3 result) |
|
876 { |
|
877 dxJointHinge* joint = (dxJointHinge*)j; |
|
878 if (joint->flags & dJOINT_REVERSE) |
|
879 getAnchor2 (joint,result,joint->anchor2); |
|
880 else |
|
881 getAnchor (joint,result,joint->anchor1); |
|
882 } |
|
883 |
|
884 |
|
885 EXPORT_C void dJointGetHingeAnchor2 (dJointID j, dVector3 result) |
|
886 { |
|
887 dxJointHinge* joint = (dxJointHinge*)j; |
|
888 |
|
889 if (joint->flags & dJOINT_REVERSE) |
|
890 getAnchor (joint,result,joint->anchor1); |
|
891 else |
|
892 getAnchor2 (joint,result,joint->anchor2); |
|
893 } |
|
894 |
|
895 |
|
896 EXPORT_C void dJointGetHingeAxis (dJointID j, dVector3 result) |
|
897 { |
|
898 dxJointHinge* joint = (dxJointHinge*)j; |
|
899 |
|
900 getAxis (joint,result,joint->axis1); |
|
901 } |
|
902 |
|
903 |
|
904 EXPORT_C void dJointSetHingeParam (dJointID j, int parameter, dReal value) |
|
905 { |
|
906 dxJointHinge* joint = (dxJointHinge*)j; |
|
907 |
|
908 joint->limot.set (parameter,value); |
|
909 } |
|
910 |
|
911 |
|
912 EXPORT_C dReal dJointGetHingeParam (dJointID j, int parameter) |
|
913 { |
|
914 dxJointHinge* joint = (dxJointHinge*)j; |
|
915 |
|
916 return joint->limot.get (parameter); |
|
917 } |
|
918 |
|
919 |
|
920 EXPORT_C dReal dJointGetHingeAngle (dJointID j) |
|
921 { |
|
922 dxJointHinge* joint = (dxJointHinge*)j; |
|
923 |
|
924 if (joint->node[0].body) { |
|
925 dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, |
|
926 joint->qrel); |
|
927 if (joint->flags & dJOINT_REVERSE) |
|
928 return -ang; |
|
929 else |
|
930 return ang; |
|
931 } |
|
932 else return 0; |
|
933 } |
|
934 |
|
935 |
|
936 EXPORT_C dReal dJointGetHingeAngleRate (dJointID j) |
|
937 { |
|
938 dxJointHinge* joint = (dxJointHinge*)j; |
|
939 |
|
940 if (joint->node[0].body) { |
|
941 dVector3 axis; |
|
942 dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); |
|
943 dReal rate = dDOT(axis,joint->node[0].body->avel); |
|
944 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
|
945 if (joint->flags & dJOINT_REVERSE) rate = - rate; |
|
946 return rate; |
|
947 } |
|
948 else return 0; |
|
949 } |
|
950 |
|
951 |
|
952 EXPORT_C void dJointAddHingeTorque (dJointID j, dReal torque) |
|
953 { |
|
954 dxJointHinge* joint = (dxJointHinge*)j; |
|
955 dVector3 axis; |
|
956 |
|
957 |
|
958 if (joint->flags & dJOINT_REVERSE) |
|
959 torque = -torque; |
|
960 |
|
961 getAxis (joint,axis,joint->axis1); |
|
962 axis[0] = dMUL(axis[0],torque); |
|
963 axis[1] = dMUL(axis[1],torque); |
|
964 axis[2] = dMUL(axis[2],torque); |
|
965 |
|
966 if (joint->node[0].body != 0) |
|
967 dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); |
|
968 if (joint->node[1].body != 0) |
|
969 dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
|
970 } |
|
971 |
|
972 |
|
973 dxJoint::Vtable __dhinge_vtable = { |
|
974 sizeof(dxJointHinge), |
|
975 (dxJoint::init_fn*) hingeInit, |
|
976 (dxJoint::getInfo1_fn*) hingeGetInfo1, |
|
977 (dxJoint::getInfo2_fn*) hingeGetInfo2, |
|
978 dJointTypeHinge}; |
|
979 |
|
980 //**************************************************************************** |
|
981 // slider |
|
982 |
|
983 static void sliderInit (dxJointSlider *j) |
|
984 { |
|
985 dSetZero (j->axis1,4); |
|
986 j->axis1[0] = REAL(1.0); |
|
987 dSetZero (j->qrel,4); |
|
988 dSetZero (j->offset,4); |
|
989 j->limot.init (j->world); |
|
990 } |
|
991 |
|
992 |
|
993 EXPORT_C dReal dJointGetSliderPosition (dJointID j) |
|
994 { |
|
995 dxJointSlider* joint = (dxJointSlider*)j; |
|
996 |
|
997 |
|
998 // get axis1 in global coordinates |
|
999 dVector3 ax1,q; |
|
1000 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
|
1001 |
|
1002 if (joint->node[1].body) { |
|
1003 // get body2 + offset point in global coordinates |
|
1004 dMULTIPLY0_331 (q,joint->node[1].body->posr.R,joint->offset); |
|
1005 for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - q[i] - |
|
1006 joint->node[1].body->posr.pos[i]; |
|
1007 } |
|
1008 else { |
|
1009 for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - |
|
1010 joint->offset[i]; |
|
1011 |
|
1012 } |
|
1013 return dDOT(ax1,q); |
|
1014 } |
|
1015 |
|
1016 |
|
1017 EXPORT_C dReal dJointGetSliderPositionRate (dJointID j) |
|
1018 { |
|
1019 dxJointSlider* joint = (dxJointSlider*)j; |
|
1020 |
|
1021 |
|
1022 // get axis1 in global coordinates |
|
1023 dVector3 ax1; |
|
1024 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
|
1025 |
|
1026 if (joint->node[1].body) { |
|
1027 return dDOT(ax1,joint->node[0].body->lvel) - |
|
1028 dDOT(ax1,joint->node[1].body->lvel); |
|
1029 } |
|
1030 else { |
|
1031 return dDOT(ax1,joint->node[0].body->lvel); |
|
1032 } |
|
1033 } |
|
1034 |
|
1035 |
|
1036 static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) |
|
1037 { |
|
1038 info->nub = 5; |
|
1039 |
|
1040 // see if joint is powered |
|
1041 if (j->limot.fmax > 0) |
|
1042 info->m = 6; // powered slider needs an extra constraint row |
|
1043 else info->m = 5; |
|
1044 |
|
1045 // see if we're at a joint limit. |
|
1046 j->limot.limit = 0; |
|
1047 if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && |
|
1048 j->limot.lostop <= j->limot.histop) { |
|
1049 // measure joint position |
|
1050 dReal pos = dJointGetSliderPosition (j); |
|
1051 if (pos <= j->limot.lostop) { |
|
1052 j->limot.limit = 1; |
|
1053 j->limot.limit_err = pos - j->limot.lostop; |
|
1054 info->m = 6; |
|
1055 } |
|
1056 else if (pos >= j->limot.histop) { |
|
1057 j->limot.limit = 2; |
|
1058 j->limot.limit_err = pos - j->limot.histop; |
|
1059 info->m = 6; |
|
1060 } |
|
1061 } |
|
1062 } |
|
1063 |
|
1064 |
|
1065 static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) |
|
1066 { |
|
1067 int i,s = info->rowskip; |
|
1068 int s3=3*s,s4=4*s; |
|
1069 |
|
1070 // pull out pos and R for both bodies. also get the `connection' |
|
1071 // vector pos2-pos1. |
|
1072 |
|
1073 dReal *pos1,*pos2,*R1,*R2; |
|
1074 dVector3 c; |
|
1075 pos1 = joint->node[0].body->posr.pos; |
|
1076 R1 = joint->node[0].body->posr.R; |
|
1077 if (joint->node[1].body) { |
|
1078 pos2 = joint->node[1].body->posr.pos; |
|
1079 R2 = joint->node[1].body->posr.R; |
|
1080 for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; |
|
1081 } |
|
1082 else { |
|
1083 pos2 = 0; |
|
1084 R2 = 0; |
|
1085 } |
|
1086 |
|
1087 // 3 rows to make body rotations equal |
|
1088 setFixedOrientation(joint, info, joint->qrel, 0); |
|
1089 |
|
1090 // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would |
|
1091 // result in three equations, so we project along the planespace vectors |
|
1092 // so that sliding along the slider axis is disregarded. for symmetry we |
|
1093 // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. |
|
1094 |
|
1095 dVector3 ax1; // joint axis in global coordinates (unit length) |
|
1096 dVector3 p,q; // plane space of ax1 |
|
1097 dMULTIPLY0_331 (ax1,R1,joint->axis1); |
|
1098 dPlaneSpace (ax1,p,q); |
|
1099 if (joint->node[1].body) { |
|
1100 dVector3 tmp; |
|
1101 dCROSSMUL (tmp, = ,c,p); |
|
1102 for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i]; |
|
1103 for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; |
|
1104 dCROSSMUL (tmp, = ,c,q); |
|
1105 for (i=0; i<3; i++) info->J1a[s4+i] = tmp[i]; |
|
1106 for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; |
|
1107 for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; |
|
1108 for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; |
|
1109 } |
|
1110 for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; |
|
1111 for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; |
|
1112 |
|
1113 // compute last two elements of right hand side. we want to align the offset |
|
1114 // point (in body 2's frame) with the center of body 1. |
|
1115 dReal k = dMUL(info->fps,info->erp); |
|
1116 if (joint->node[1].body) { |
|
1117 dVector3 ofs; // offset point in global coordinates |
|
1118 dMULTIPLY0_331 (ofs,R2,joint->offset); |
|
1119 for (i=0; i<3; i++) c[i] += ofs[i]; |
|
1120 info->c[3] = dMUL(k,dDOT(p,c)); |
|
1121 info->c[4] = dMUL(k,dDOT(q,c)); |
|
1122 } |
|
1123 else { |
|
1124 dVector3 ofs; // offset point in global coordinates |
|
1125 for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; |
|
1126 info->c[3] = dMUL(k,dDOT(p,ofs)); |
|
1127 info->c[4] = dMUL(k,dDOT(q,ofs)); |
|
1128 } |
|
1129 |
|
1130 // if the slider is powered, or has joint limits, add in the extra row |
|
1131 joint->limot.addLimot (joint,info,5,ax1,0); |
|
1132 } |
|
1133 |
|
1134 |
|
1135 EXPORT_C void dJointSetSliderAxis (dJointID j, dReal x, dReal y, dReal z) |
|
1136 { |
|
1137 dxJointSlider* joint = (dxJointSlider*)j; |
|
1138 int i; |
|
1139 |
|
1140 setAxes (joint,x,y,z,joint->axis1,0); |
|
1141 |
|
1142 // compute initial relative rotation body1 -> body2, or env -> body1 |
|
1143 // also compute center of body1 w.r.t body 2 |
|
1144 if (joint->node[1].body) { |
|
1145 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
|
1146 dVector3 c; |
|
1147 for (i=0; i<3; i++) |
|
1148 c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; |
|
1149 dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); |
|
1150 } |
|
1151 else { |
|
1152 // set joint->qrel to the transpose of the first body's q |
|
1153 joint->qrel[0] = joint->node[0].body->q[0]; |
|
1154 for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
|
1155 for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; |
|
1156 } |
|
1157 } |
|
1158 |
|
1159 |
|
1160 EXPORT_C void dJointSetSliderAxisDelta (dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz) |
|
1161 { |
|
1162 dxJointSlider* joint = (dxJointSlider*)j; |
|
1163 int i; |
|
1164 |
|
1165 setAxes (joint,x,y,z,joint->axis1,0); |
|
1166 |
|
1167 // compute initial relative rotation body1 -> body2, or env -> body1 |
|
1168 // also compute center of body1 w.r.t body 2 |
|
1169 if (joint->node[1].body) { |
|
1170 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
|
1171 dVector3 c; |
|
1172 for (i=0; i<3; i++) |
|
1173 c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; |
|
1174 dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); |
|
1175 } |
|
1176 else { |
|
1177 // set joint->qrel to the transpose of the first body's q |
|
1178 joint->qrel[0] = joint->node[0].body->q[0]; |
|
1179 |
|
1180 for (i=1; i<4; i++) |
|
1181 joint->qrel[i] = -joint->node[0].body->q[i]; |
|
1182 |
|
1183 joint->offset[0] = joint->node[0].body->posr.pos[0] + dx; |
|
1184 joint->offset[1] = joint->node[0].body->posr.pos[1] + dy; |
|
1185 joint->offset[2] = joint->node[0].body->posr.pos[2] + dz; |
|
1186 } |
|
1187 } |
|
1188 |
|
1189 |
|
1190 |
|
1191 EXPORT_C void dJointGetSliderAxis (dJointID j, dVector3 result) |
|
1192 { |
|
1193 dxJointSlider* joint = (dxJointSlider*)j; |
|
1194 |
|
1195 getAxis (joint,result,joint->axis1); |
|
1196 } |
|
1197 |
|
1198 |
|
1199 EXPORT_C void dJointSetSliderParam (dJointID j, int parameter, dReal value) |
|
1200 { |
|
1201 dxJointSlider* joint = (dxJointSlider*)j; |
|
1202 |
|
1203 joint->limot.set (parameter,value); |
|
1204 } |
|
1205 |
|
1206 |
|
1207 EXPORT_C dReal dJointGetSliderParam (dJointID j, int parameter) |
|
1208 { |
|
1209 dxJointSlider* joint = (dxJointSlider*)j; |
|
1210 |
|
1211 return joint->limot.get (parameter); |
|
1212 } |
|
1213 |
|
1214 |
|
1215 EXPORT_C void dJointAddSliderForce (dJointID j, dReal force) |
|
1216 { |
|
1217 dxJointSlider* joint = (dxJointSlider*)j; |
|
1218 dVector3 axis; |
|
1219 |
|
1220 |
|
1221 if (joint->flags & dJOINT_REVERSE) |
|
1222 force -= force; |
|
1223 |
|
1224 getAxis (joint,axis,joint->axis1); |
|
1225 axis[0] = dMUL(axis[0],force); |
|
1226 axis[1] = dMUL(axis[1],force); |
|
1227 axis[2] = dMUL(axis[2],force); |
|
1228 |
|
1229 if (joint->node[0].body != 0) |
|
1230 dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); |
|
1231 if (joint->node[1].body != 0) |
|
1232 dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
|
1233 |
|
1234 if (joint->node[0].body != 0 && joint->node[1].body != 0) { |
|
1235 // linear torque decoupling: |
|
1236 // we have to compensate the torque, that this slider force may generate |
|
1237 // if body centers are not aligned along the slider axis |
|
1238 |
|
1239 dVector3 ltd; // Linear Torque Decoupling vector (a torque) |
|
1240 |
|
1241 dVector3 c; |
|
1242 c[0]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0])); |
|
1243 c[1]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1])); |
|
1244 c[2]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2])); |
|
1245 dCROSS (ltd,=,c,axis); |
|
1246 |
|
1247 dBodyAddTorque (joint->node[0].body,ltd[0],ltd[1], ltd[2]); |
|
1248 dBodyAddTorque (joint->node[1].body,ltd[0],ltd[1], ltd[2]); |
|
1249 } |
|
1250 } |
|
1251 |
|
1252 |
|
1253 dxJoint::Vtable __dslider_vtable = { |
|
1254 sizeof(dxJointSlider), |
|
1255 (dxJoint::init_fn*) sliderInit, |
|
1256 (dxJoint::getInfo1_fn*) sliderGetInfo1, |
|
1257 (dxJoint::getInfo2_fn*) sliderGetInfo2, |
|
1258 dJointTypeSlider}; |
|
1259 |
|
1260 //**************************************************************************** |
|
1261 // contact |
|
1262 |
|
1263 static void contactInit (dxJointContact */*j*/) |
|
1264 { |
|
1265 |
|
1266 } |
|
1267 |
|
1268 |
|
1269 static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) |
|
1270 { |
|
1271 // make sure mu's >= 0, then calculate number of constraint rows and number |
|
1272 // of unbounded rows. |
|
1273 int m = 1, nub=0; |
|
1274 if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; |
|
1275 if (j->contact.surface.mode & dContactMu2) { |
|
1276 if (j->contact.surface.mu > 0) m++; |
|
1277 if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; |
|
1278 if (j->contact.surface.mu2 > 0) m++; |
|
1279 if (j->contact.surface.mu == dInfinity) nub ++; |
|
1280 if (j->contact.surface.mu2 == dInfinity) nub ++; |
|
1281 } |
|
1282 else { |
|
1283 if (j->contact.surface.mu > REAL(0.0)) m += 2; |
|
1284 if (j->contact.surface.mu == dInfinity) nub += 2; |
|
1285 } |
|
1286 |
|
1287 j->the_m = m; |
|
1288 info->m = m; |
|
1289 info->nub = nub; |
|
1290 } |
|
1291 |
|
1292 |
|
1293 static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) |
|
1294 { |
|
1295 int s = info->rowskip; |
|
1296 int s2 = 2*s; |
|
1297 |
|
1298 // get normal, with sign adjusted for body1/body2 polarity |
|
1299 dVector3 normal; |
|
1300 if (j->flags & dJOINT_REVERSE) { |
|
1301 normal[0] = - j->contact.geom.normal[0]; |
|
1302 normal[1] = - j->contact.geom.normal[1]; |
|
1303 normal[2] = - j->contact.geom.normal[2]; |
|
1304 } |
|
1305 else { |
|
1306 normal[0] = j->contact.geom.normal[0]; |
|
1307 normal[1] = j->contact.geom.normal[1]; |
|
1308 normal[2] = j->contact.geom.normal[2]; |
|
1309 } |
|
1310 normal[3] = 0; // @@@ hmmm |
|
1311 |
|
1312 // c1,c2 = contact points with respect to body PORs |
|
1313 dVector3 c1,c2 = {}; |
|
1314 c1[0] = j->contact.geom.pos[0] - j->node[0].body->posr.pos[0]; |
|
1315 c1[1] = j->contact.geom.pos[1] - j->node[0].body->posr.pos[1]; |
|
1316 c1[2] = j->contact.geom.pos[2] - j->node[0].body->posr.pos[2]; |
|
1317 |
|
1318 // set jacobian for normal |
|
1319 info->J1l[0] = normal[0]; |
|
1320 info->J1l[1] = normal[1]; |
|
1321 info->J1l[2] = normal[2]; |
|
1322 dCROSS (info->J1a,=,c1,normal); |
|
1323 if (j->node[1].body) { |
|
1324 c2[0] = j->contact.geom.pos[0] - j->node[1].body->posr.pos[0]; |
|
1325 c2[1] = j->contact.geom.pos[1] - j->node[1].body->posr.pos[1]; |
|
1326 c2[2] = j->contact.geom.pos[2] - j->node[1].body->posr.pos[2]; |
|
1327 info->J2l[0] = -normal[0]; |
|
1328 info->J2l[1] = -normal[1]; |
|
1329 info->J2l[2] = -normal[2]; |
|
1330 dCROSS (info->J2a,= -,c2,normal); |
|
1331 } |
|
1332 |
|
1333 // set right hand side and cfm value for normal |
|
1334 dReal erp = info->erp; |
|
1335 if (j->contact.surface.mode & dContactSoftERP) |
|
1336 erp = j->contact.surface.soft_erp; |
|
1337 dReal k = dMUL(info->fps,erp); |
|
1338 dReal depth = j->contact.geom.depth - j->world->contactp.min_depth; |
|
1339 if (depth < 0) depth = 0; |
|
1340 |
|
1341 const dReal maxvel = j->world->contactp.max_vel; |
|
1342 info->c[0] = dMUL(k,depth); |
|
1343 if (info->c[0] > maxvel) |
|
1344 info->c[0] = maxvel; |
|
1345 |
|
1346 if (j->contact.surface.mode & dContactSoftCFM) |
|
1347 info->cfm[0] = j->contact.surface.soft_cfm; |
|
1348 |
|
1349 // deal with bounce |
|
1350 if (j->contact.surface.mode & dContactBounce) { |
|
1351 // calculate outgoing velocity (-ve for incoming contact) |
|
1352 dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + |
|
1353 dDOT(info->J1a,j->node[0].body->avel); |
|
1354 if (j->node[1].body) { |
|
1355 outgoing += dDOT(info->J2l,j->node[1].body->lvel) + |
|
1356 dDOT(info->J2a,j->node[1].body->avel); |
|
1357 } |
|
1358 // only apply bounce if the outgoing velocity is greater than the |
|
1359 // threshold, and if the resulting c[0] exceeds what we already have. |
|
1360 if (j->contact.surface.bounce_vel >= 0 && |
|
1361 (-outgoing) > j->contact.surface.bounce_vel) { |
|
1362 dReal newc = - dMUL(j->contact.surface.bounce,outgoing); |
|
1363 if (newc > info->c[0]) info->c[0] = newc; |
|
1364 } |
|
1365 } |
|
1366 |
|
1367 // set LCP limits for normal |
|
1368 info->lo[0] = 0; |
|
1369 info->hi[0] = dInfinity; |
|
1370 |
|
1371 // now do jacobian for tangential forces |
|
1372 dVector3 t1,t2; // two vectors tangential to normal |
|
1373 |
|
1374 // first friction direction |
|
1375 if (j->the_m >= 2) { |
|
1376 if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? |
|
1377 t1[0] = j->contact.fdir1[0]; |
|
1378 t1[1] = j->contact.fdir1[1]; |
|
1379 t1[2] = j->contact.fdir1[2]; |
|
1380 dCROSS (t2,=,normal,t1); |
|
1381 } |
|
1382 else { |
|
1383 dPlaneSpace (normal,t1,t2); |
|
1384 } |
|
1385 info->J1l[s+0] = t1[0]; |
|
1386 info->J1l[s+1] = t1[1]; |
|
1387 info->J1l[s+2] = t1[2]; |
|
1388 dCROSS (info->J1a+s,=,c1,t1); |
|
1389 if (j->node[1].body) { |
|
1390 info->J2l[s+0] = -t1[0]; |
|
1391 info->J2l[s+1] = -t1[1]; |
|
1392 info->J2l[s+2] = -t1[2]; |
|
1393 dCROSS (info->J2a+s,= -,c2,t1); |
|
1394 } |
|
1395 // set right hand side |
|
1396 if (j->contact.surface.mode & dContactMotion1) { |
|
1397 info->c[1] = j->contact.surface.motion1; |
|
1398 } |
|
1399 // set LCP bounds and friction index. this depends on the approximation |
|
1400 // mode |
|
1401 info->lo[1] = -j->contact.surface.mu; |
|
1402 info->hi[1] = j->contact.surface.mu; |
|
1403 if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; |
|
1404 |
|
1405 // set slip (constraint force mixing) |
|
1406 if (j->contact.surface.mode & dContactSlip1) |
|
1407 info->cfm[1] = j->contact.surface.slip1; |
|
1408 } |
|
1409 |
|
1410 // second friction direction |
|
1411 if (j->the_m >= 3) { |
|
1412 info->J1l[s2+0] = t2[0]; |
|
1413 info->J1l[s2+1] = t2[1]; |
|
1414 info->J1l[s2+2] = t2[2]; |
|
1415 dCROSS (info->J1a+s2,=,c1,t2); |
|
1416 if (j->node[1].body) { |
|
1417 info->J2l[s2+0] = -t2[0]; |
|
1418 info->J2l[s2+1] = -t2[1]; |
|
1419 info->J2l[s2+2] = -t2[2]; |
|
1420 dCROSS (info->J2a+s2,= -,c2,t2); |
|
1421 } |
|
1422 // set right hand side |
|
1423 if (j->contact.surface.mode & dContactMotion2) { |
|
1424 info->c[2] = j->contact.surface.motion2; |
|
1425 } |
|
1426 // set LCP bounds and friction index. this depends on the approximation |
|
1427 // mode |
|
1428 if (j->contact.surface.mode & dContactMu2) { |
|
1429 info->lo[2] = -j->contact.surface.mu2; |
|
1430 info->hi[2] = j->contact.surface.mu2; |
|
1431 } |
|
1432 else { |
|
1433 info->lo[2] = -j->contact.surface.mu; |
|
1434 info->hi[2] = j->contact.surface.mu; |
|
1435 } |
|
1436 if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; |
|
1437 |
|
1438 // set slip (constraint force mixing) |
|
1439 if (j->contact.surface.mode & dContactSlip2) |
|
1440 info->cfm[2] = j->contact.surface.slip2; |
|
1441 } |
|
1442 } |
|
1443 |
|
1444 |
|
1445 dxJoint::Vtable __dcontact_vtable = { |
|
1446 sizeof(dxJointContact), |
|
1447 (dxJoint::init_fn*) contactInit, |
|
1448 (dxJoint::getInfo1_fn*) contactGetInfo1, |
|
1449 (dxJoint::getInfo2_fn*) contactGetInfo2, |
|
1450 dJointTypeContact}; |
|
1451 |
|
1452 //**************************************************************************** |
|
1453 // hinge 2. note that this joint must be attached to two bodies for it to work |
|
1454 |
|
1455 static dReal measureHinge2Angle (dxJointHinge2 *joint) |
|
1456 { |
|
1457 dVector3 a1,a2; |
|
1458 dMULTIPLY0_331 (a1,joint->node[1].body->posr.R,joint->axis2); |
|
1459 dMULTIPLY1_331 (a2,joint->node[0].body->posr.R,a1); |
|
1460 dReal x = dDOT(joint->v1,a2); |
|
1461 dReal y = dDOT(joint->v2,a2); |
|
1462 return -dArcTan2 (y,x); |
|
1463 } |
|
1464 |
|
1465 |
|
1466 static void hinge2Init (dxJointHinge2 *j) |
|
1467 { |
|
1468 dSetZero (j->anchor1,4); |
|
1469 dSetZero (j->anchor2,4); |
|
1470 dSetZero (j->axis1,4); |
|
1471 j->axis1[0] = REAL(1.0); |
|
1472 dSetZero (j->axis2,4); |
|
1473 j->axis2[1] = REAL(1.0); |
|
1474 j->c0 = 0; |
|
1475 j->s0 = 0; |
|
1476 |
|
1477 dSetZero (j->v1,4); |
|
1478 j->v1[0] = REAL(1.0); |
|
1479 dSetZero (j->v2,4); |
|
1480 j->v2[1] = REAL(1.0); |
|
1481 |
|
1482 j->limot1.init (j->world); |
|
1483 j->limot2.init (j->world); |
|
1484 |
|
1485 j->susp_erp = j->world->global_erp; |
|
1486 j->susp_cfm = j->world->global_cfm; |
|
1487 |
|
1488 j->flags |= dJOINT_TWOBODIES; |
|
1489 } |
|
1490 |
|
1491 |
|
1492 static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) |
|
1493 { |
|
1494 info->m = 4; |
|
1495 info->nub = 4; |
|
1496 |
|
1497 // see if we're powered or at a joint limit for axis 1 |
|
1498 int atlimit=0; |
|
1499 if ((j->limot1.lostop >= -dPI || j->limot1.histop <= dPI) && |
|
1500 j->limot1.lostop <= j->limot1.histop) { |
|
1501 dReal angle = measureHinge2Angle (j); |
|
1502 if (j->limot1.testRotationalLimit (angle)) atlimit = 1; |
|
1503 } |
|
1504 if (atlimit || j->limot1.fmax > 0) info->m++; |
|
1505 |
|
1506 // see if we're powering axis 2 (we currently never limit this axis) |
|
1507 j->limot2.limit = 0; |
|
1508 if (j->limot2.fmax > 0) info->m++; |
|
1509 } |
|
1510 |
|
1511 |
|
1512 // macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are |
|
1513 // relative to body 1 and 2 initially) and then computes the constrained |
|
1514 // rotational axis as the cross product of ax1 and ax2. |
|
1515 // the sin and cos of the angle between axis 1 and 2 is computed, this comes |
|
1516 // from dot and cross product rules. |
|
1517 |
|
1518 #define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ |
|
1519 dVector3 ax1,ax2; \ |
|
1520 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); \ |
|
1521 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); \ |
|
1522 dCROSS (axis,=,ax1,ax2); \ |
|
1523 sin_angle = dSqrt (dMUL(axis[0],axis[0]) + dMUL(axis[1],axis[1]) + dMUL(axis[2],axis[2])); \ |
|
1524 cos_angle = dDOT (ax1,ax2); |
|
1525 |
|
1526 |
|
1527 static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) |
|
1528 { |
|
1529 // get information we need to set the hinge row |
|
1530 dReal s,c; |
|
1531 dVector3 q; |
|
1532 HINGE2_GET_AXIS_INFO (q,s,c); |
|
1533 dNormalize3 (q); // @@@ quicker: divide q by s ? |
|
1534 |
|
1535 // set the three ball-and-socket rows (aligned to the suspension axis ax1) |
|
1536 setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); |
|
1537 |
|
1538 // set the hinge row |
|
1539 int s3=3*info->rowskip; |
|
1540 info->J1a[s3+0] = q[0]; |
|
1541 info->J1a[s3+1] = q[1]; |
|
1542 info->J1a[s3+2] = q[2]; |
|
1543 if (joint->node[1].body) { |
|
1544 info->J2a[s3+0] = -q[0]; |
|
1545 info->J2a[s3+1] = -q[1]; |
|
1546 info->J2a[s3+2] = -q[2]; |
|
1547 } |
|
1548 |
|
1549 // compute the right hand side for the constrained rotational DOF. |
|
1550 // axis 1 and axis 2 are separated by an angle `theta'. the desired |
|
1551 // separation angle is theta0. sin(theta0) and cos(theta0) are recorded |
|
1552 // in the joint structure. the correcting angular velocity is: |
|
1553 // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize |
|
1554 // = (erp*fps) * (theta0-theta) |
|
1555 // (theta0-theta) can be computed using the following small-angle-difference |
|
1556 // approximation: |
|
1557 // theta0-theta ~= tan(theta0-theta) |
|
1558 // = sin(theta0-theta)/cos(theta0-theta) |
|
1559 // = (c*s0 - s*c0) / (c*c0 + s*s0) |
|
1560 // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 |
|
1561 // where c = cos(theta), s = sin(theta) |
|
1562 // c0 = cos(theta0), s0 = sin(theta0) |
|
1563 |
|
1564 dReal k = dMUL(info->fps,info->erp); |
|
1565 info->c[3] = dMUL(k,(dMUL(joint->c0,s) - dMUL(joint->s0,c))); |
|
1566 |
|
1567 // if the axis1 hinge is powered, or has joint limits, add in more stuff |
|
1568 int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); |
|
1569 |
|
1570 // if the axis2 hinge is powered, add in more stuff |
|
1571 joint->limot2.addLimot (joint,info,row,ax2,1); |
|
1572 |
|
1573 // set parameter for the suspension |
|
1574 info->cfm[0] = joint->susp_cfm; |
|
1575 } |
|
1576 |
|
1577 |
|
1578 // compute vectors v1 and v2 (embedded in body1), used to measure angle |
|
1579 // between body 1 and body 2 |
|
1580 |
|
1581 static void makeHinge2V1andV2 (dxJointHinge2 *joint) |
|
1582 { |
|
1583 if (joint->node[0].body) { |
|
1584 // get axis 1 and 2 in global coords |
|
1585 dVector3 ax1,ax2,v; |
|
1586 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
|
1587 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); |
|
1588 |
|
1589 // don't do anything if the axis1 or axis2 vectors are zero or the same |
|
1590 if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0 || |
|
1591 (ax2[0]==0) && ax2[1]==0 && ax2[2]==0) || |
|
1592 (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; |
|
1593 |
|
1594 // modify axis 2 so it's perpendicular to axis 1 |
|
1595 dReal k = dDOT(ax1,ax2); |
|
1596 for (int i=0; i<3; i++) ax2[i] -= dMUL(k,ax1[i]); |
|
1597 dNormalize3 (ax2); |
|
1598 |
|
1599 // make v1 = modified axis2, v2 = axis1 x (modified axis2) |
|
1600 dCROSS (v,=,ax1,ax2); |
|
1601 dMULTIPLY1_331 (joint->v1,joint->node[0].body->posr.R,ax2); |
|
1602 dMULTIPLY1_331 (joint->v2,joint->node[0].body->posr.R,v); |
|
1603 } |
|
1604 } |
|
1605 |
|
1606 |
|
1607 EXPORT_C void dJointSetHinge2Anchor (dJointID j, dReal x, dReal y, dReal z) |
|
1608 { |
|
1609 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1610 |
|
1611 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
|
1612 makeHinge2V1andV2 (joint); |
|
1613 } |
|
1614 |
|
1615 |
|
1616 EXPORT_C void dJointSetHinge2Axis1 (dJointID j, dReal x, dReal y, dReal z) |
|
1617 { |
|
1618 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1619 |
|
1620 if (joint->node[0].body) { |
|
1621 dReal q[4]; |
|
1622 q[0] = x; |
|
1623 q[1] = y; |
|
1624 q[2] = z; |
|
1625 q[3] = 0; |
|
1626 dNormalize3 (q); |
|
1627 dMULTIPLY1_331 (joint->axis1,joint->node[0].body->posr.R,q); |
|
1628 joint->axis1[3] = 0; |
|
1629 |
|
1630 // compute the sin and cos of the angle between axis 1 and axis 2 |
|
1631 dVector3 ax; |
|
1632 HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); |
|
1633 } |
|
1634 makeHinge2V1andV2 (joint); |
|
1635 } |
|
1636 |
|
1637 |
|
1638 EXPORT_C void dJointSetHinge2Axis2 (dJointID j, dReal x, dReal y, dReal z) |
|
1639 { |
|
1640 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1641 |
|
1642 if (joint->node[1].body) { |
|
1643 dReal q[4]; |
|
1644 q[0] = x; |
|
1645 q[1] = y; |
|
1646 q[2] = z; |
|
1647 q[3] = 0; |
|
1648 dNormalize3 (q); |
|
1649 dMULTIPLY1_331 (joint->axis2,joint->node[1].body->posr.R,q); |
|
1650 joint->axis1[3] = 0; |
|
1651 |
|
1652 // compute the sin and cos of the angle between axis 1 and axis 2 |
|
1653 dVector3 ax; |
|
1654 HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); |
|
1655 } |
|
1656 makeHinge2V1andV2 (joint); |
|
1657 } |
|
1658 |
|
1659 |
|
1660 EXPORT_C void dJointSetHinge2Param (dJointID j, int parameter, dReal value) |
|
1661 { |
|
1662 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1663 |
|
1664 if ((parameter & 0xff00) == 0x100) { |
|
1665 joint->limot2.set (parameter & 0xff,value); |
|
1666 } |
|
1667 else { |
|
1668 if (parameter == dParamSuspensionERP) joint->susp_erp = value; |
|
1669 else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; |
|
1670 else joint->limot1.set (parameter,value); |
|
1671 } |
|
1672 } |
|
1673 |
|
1674 |
|
1675 EXPORT_C void dJointGetHinge2Anchor (dJointID j, dVector3 result) |
|
1676 { |
|
1677 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1678 |
|
1679 if (joint->flags & dJOINT_REVERSE) |
|
1680 getAnchor2 (joint,result,joint->anchor2); |
|
1681 else |
|
1682 getAnchor (joint,result,joint->anchor1); |
|
1683 } |
|
1684 |
|
1685 |
|
1686 EXPORT_C void dJointGetHinge2Anchor2 (dJointID j, dVector3 result) |
|
1687 { |
|
1688 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1689 |
|
1690 if (joint->flags & dJOINT_REVERSE) |
|
1691 getAnchor (joint,result,joint->anchor1); |
|
1692 else |
|
1693 getAnchor2 (joint,result,joint->anchor2); |
|
1694 } |
|
1695 |
|
1696 |
|
1697 EXPORT_C void dJointGetHinge2Axis1 (dJointID j, dVector3 result) |
|
1698 { |
|
1699 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1700 |
|
1701 if (joint->node[0].body) { |
|
1702 dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis1); |
|
1703 } |
|
1704 } |
|
1705 |
|
1706 |
|
1707 EXPORT_C void dJointGetHinge2Axis2 (dJointID j, dVector3 result) |
|
1708 { |
|
1709 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1710 |
|
1711 if (joint->node[1].body) { |
|
1712 dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis2); |
|
1713 } |
|
1714 } |
|
1715 |
|
1716 |
|
1717 EXPORT_C dReal dJointGetHinge2Param (dJointID j, int parameter) |
|
1718 { |
|
1719 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1720 |
|
1721 if ((parameter & 0xff00) == 0x100) { |
|
1722 return joint->limot2.get (parameter & 0xff); |
|
1723 } |
|
1724 else { |
|
1725 if (parameter == dParamSuspensionERP) return joint->susp_erp; |
|
1726 else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; |
|
1727 else return joint->limot1.get (parameter); |
|
1728 } |
|
1729 } |
|
1730 |
|
1731 |
|
1732 EXPORT_C dReal dJointGetHinge2Angle1 (dJointID j) |
|
1733 { |
|
1734 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1735 |
|
1736 if (joint->node[0].body) return measureHinge2Angle (joint); |
|
1737 else return 0; |
|
1738 } |
|
1739 |
|
1740 |
|
1741 EXPORT_C dReal dJointGetHinge2Angle1Rate (dJointID j) |
|
1742 { |
|
1743 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1744 |
|
1745 if (joint->node[0].body) { |
|
1746 dVector3 axis; |
|
1747 dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); |
|
1748 dReal rate = dDOT(axis,joint->node[0].body->avel); |
|
1749 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
|
1750 return rate; |
|
1751 } |
|
1752 else return 0; |
|
1753 } |
|
1754 |
|
1755 |
|
1756 EXPORT_C dReal dJointGetHinge2Angle2Rate (dJointID j) |
|
1757 { |
|
1758 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1759 |
|
1760 if (joint->node[0].body && joint->node[1].body) { |
|
1761 dVector3 axis; |
|
1762 dMULTIPLY0_331 (axis,joint->node[1].body->posr.R,joint->axis2); |
|
1763 dReal rate = dDOT(axis,joint->node[0].body->avel); |
|
1764 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); |
|
1765 return rate; |
|
1766 } |
|
1767 else return 0; |
|
1768 } |
|
1769 |
|
1770 |
|
1771 EXPORT_C void dJointAddHinge2Torques (dJointID j, dReal torque1, dReal torque2) |
|
1772 { |
|
1773 dxJointHinge2* joint = (dxJointHinge2*)j; |
|
1774 dVector3 axis1, axis2; |
|
1775 |
|
1776 |
|
1777 if (joint->node[0].body && joint->node[1].body) { |
|
1778 dMULTIPLY0_331 (axis1,joint->node[0].body->posr.R,joint->axis1); |
|
1779 dMULTIPLY0_331 (axis2,joint->node[1].body->posr.R,joint->axis2); |
|
1780 axis1[0] = dMUL(axis1[0],torque1) + dMUL(axis2[0],torque2); |
|
1781 axis1[1] = dMUL(axis1[1],torque1) + dMUL(axis2[1],torque2); |
|
1782 axis1[2] = dMUL(axis1[2],torque1) + dMUL(axis2[2],torque2); |
|
1783 dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); |
|
1784 dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); |
|
1785 } |
|
1786 } |
|
1787 |
|
1788 |
|
1789 dxJoint::Vtable __dhinge2_vtable = { |
|
1790 sizeof(dxJointHinge2), |
|
1791 (dxJoint::init_fn*) hinge2Init, |
|
1792 (dxJoint::getInfo1_fn*) hinge2GetInfo1, |
|
1793 (dxJoint::getInfo2_fn*) hinge2GetInfo2, |
|
1794 dJointTypeHinge2}; |
|
1795 |
|
1796 //**************************************************************************** |
|
1797 // universal |
|
1798 |
|
1799 // I just realized that the universal joint is equivalent to a hinge 2 joint with |
|
1800 // perfectly stiff suspension. By comparing the hinge 2 implementation to |
|
1801 // the universal implementation, you may be able to improve this |
|
1802 // implementation (or, less likely, the hinge2 implementation). |
|
1803 |
|
1804 static void universalInit (dxJointUniversal *j) |
|
1805 { |
|
1806 dSetZero (j->anchor1,4); |
|
1807 dSetZero (j->anchor2,4); |
|
1808 dSetZero (j->axis1,4); |
|
1809 j->axis1[0] = REAL(1.0); |
|
1810 dSetZero (j->axis2,4); |
|
1811 j->axis2[1] = REAL(1.0); |
|
1812 dSetZero(j->qrel1,4); |
|
1813 dSetZero(j->qrel2,4); |
|
1814 j->limot1.init (j->world); |
|
1815 j->limot2.init (j->world); |
|
1816 } |
|
1817 |
|
1818 |
|
1819 static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) |
|
1820 { |
|
1821 // This says "ax1 = joint->node[0].body->posr.R * joint->axis1" |
|
1822 dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); |
|
1823 |
|
1824 if (joint->node[1].body) { |
|
1825 dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); |
|
1826 } |
|
1827 else { |
|
1828 ax2[0] = joint->axis2[0]; |
|
1829 ax2[1] = joint->axis2[1]; |
|
1830 ax2[2] = joint->axis2[2]; |
|
1831 } |
|
1832 } |
|
1833 |
|
1834 static void getUniversalAngles(dxJointUniversal *joint, dReal *angle1, dReal *angle2) |
|
1835 { |
|
1836 if (joint->node[0].body) |
|
1837 { |
|
1838 // length 1 joint axis in global coordinates, from each body |
|
1839 dVector3 ax1, ax2; |
|
1840 dMatrix3 R; |
|
1841 dQuaternion qcross, qq, qrel; |
|
1842 |
|
1843 getUniversalAxes (joint,ax1,ax2); |
|
1844 |
|
1845 // It should be possible to get both angles without explicitly |
|
1846 // constructing the rotation matrix of the cross. Basically, |
|
1847 // orientation of the cross about axis1 comes from body 2, |
|
1848 // about axis 2 comes from body 1, and the perpendicular |
|
1849 // axis can come from the two bodies somehow. (We don't really |
|
1850 // want to assume it's 90 degrees, because in general the |
|
1851 // constraints won't be perfectly satisfied, or even very well |
|
1852 // satisfied.) |
|
1853 // |
|
1854 // However, we'd need a version of getHingeAngleFromRElativeQuat() |
|
1855 // that CAN handle when its relative quat is rotated along a direction |
|
1856 // other than the given axis. What I have here works, |
|
1857 // although it's probably much slower than need be. |
|
1858 |
|
1859 dRFrom2Axes (R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
|
1860 |
|
1861 dRtoQ (R, qcross); |
|
1862 |
|
1863 |
|
1864 // This code is essentialy the same as getHingeAngle(), see the comments |
|
1865 // there for details. |
|
1866 |
|
1867 // get qrel = relative rotation between node[0] and the cross |
|
1868 dQMultiply1 (qq, joint->node[0].body->q, qcross); |
|
1869 dQMultiply2 (qrel, qq, joint->qrel1); |
|
1870 |
|
1871 *angle1 = getHingeAngleFromRelativeQuat(qrel, joint->axis1); |
|
1872 |
|
1873 // This is equivalent to |
|
1874 // dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
|
1875 // You see that the R is constructed from the same 2 axis as for angle1 |
|
1876 // but the first and second axis are swapped. |
|
1877 // So we can take the first R and rapply a rotation to it. |
|
1878 // The rotation is around the axis between the 2 axes (ax1 and ax2). |
|
1879 // We do a rotation of 180deg. |
|
1880 |
|
1881 dQuaternion qcross2; |
|
1882 // Find the vector between ax1 and ax2 (i.e. in the middle) |
|
1883 // We need to turn around this vector by 180deg |
|
1884 |
|
1885 // The 2 axes should be normalize so to find the vector between the 2. |
|
1886 // Add and devide by 2 then normalize or simply normalize |
|
1887 // ax2 |
|
1888 // ^ |
|
1889 // | |
|
1890 // | |
|
1891 /// *------------> ax1 |
|
1892 // We want the vector a 45deg |
|
1893 // |
|
1894 // N.B. We don't need to normalize the ax1 and ax2 since there are |
|
1895 // normalized when we set them. |
|
1896 |
|
1897 // We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z] |
|
1898 qrel[0] = 0; // equivalent to cos(Pi/2) |
|
1899 qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1 |
|
1900 qrel[2] = ax1[1] + ax2[1]; |
|
1901 qrel[3] = ax1[2] + ax2[2]; |
|
1902 |
|
1903 dReal l = dRecip(dSqrt(dMUL(qrel[1],qrel[1]) + dMUL(qrel[2],qrel[2]) + dMUL(qrel[3],qrel[3]))); |
|
1904 qrel[1] = dMUL(qrel[1],l); |
|
1905 qrel[2] = dMUL(qrel[2],l); |
|
1906 qrel[3] = dMUL(qrel[3],l); |
|
1907 |
|
1908 dQMultiply0 (qcross2, qrel, qcross); |
|
1909 |
|
1910 if (joint->node[1].body) { |
|
1911 dQMultiply1 (qq, joint->node[1].body->q, qcross2); |
|
1912 dQMultiply2 (qrel, qq, joint->qrel2); |
|
1913 } |
|
1914 else { |
|
1915 // pretend joint->node[1].body->q is the identity |
|
1916 dQMultiply2 (qrel, qcross2, joint->qrel2); |
|
1917 } |
|
1918 |
|
1919 *angle2 = - getHingeAngleFromRelativeQuat(qrel, joint->axis2); |
|
1920 |
|
1921 } |
|
1922 else |
|
1923 { |
|
1924 *angle1 = 0; |
|
1925 *angle2 = 0; |
|
1926 } |
|
1927 } |
|
1928 |
|
1929 static dReal getUniversalAngle1(dxJointUniversal *joint) |
|
1930 { |
|
1931 if (joint->node[0].body) { |
|
1932 // length 1 joint axis in global coordinates, from each body |
|
1933 dVector3 ax1, ax2; |
|
1934 dMatrix3 R; |
|
1935 dQuaternion qcross, qq, qrel; |
|
1936 |
|
1937 getUniversalAxes (joint,ax1,ax2); |
|
1938 |
|
1939 // It should be possible to get both angles without explicitly |
|
1940 // constructing the rotation matrix of the cross. Basically, |
|
1941 // orientation of the cross about axis1 comes from body 2, |
|
1942 // about axis 2 comes from body 1, and the perpendicular |
|
1943 // axis can come from the two bodies somehow. (We don't really |
|
1944 // want to assume it's 90 degrees, because in general the |
|
1945 // constraints won't be perfectly satisfied, or even very well |
|
1946 // satisfied.) |
|
1947 // |
|
1948 // However, we'd need a version of getHingeAngleFromRElativeQuat() |
|
1949 // that CAN handle when its relative quat is rotated along a direction |
|
1950 // other than the given axis. What I have here works, |
|
1951 // although it's probably much slower than need be. |
|
1952 |
|
1953 dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
|
1954 dRtoQ (R,qcross); |
|
1955 |
|
1956 // This code is essential the same as getHingeAngle(), see the comments |
|
1957 // there for details. |
|
1958 |
|
1959 // get qrel = relative rotation between node[0] and the cross |
|
1960 dQMultiply1 (qq,joint->node[0].body->q,qcross); |
|
1961 dQMultiply2 (qrel,qq,joint->qrel1); |
|
1962 |
|
1963 return getHingeAngleFromRelativeQuat(qrel, joint->axis1); |
|
1964 } |
|
1965 return 0; |
|
1966 } |
|
1967 |
|
1968 |
|
1969 static dReal getUniversalAngle2(dxJointUniversal *joint) |
|
1970 { |
|
1971 if (joint->node[0].body) { |
|
1972 // length 1 joint axis in global coordinates, from each body |
|
1973 dVector3 ax1, ax2; |
|
1974 dMatrix3 R; |
|
1975 dQuaternion qcross, qq, qrel; |
|
1976 |
|
1977 getUniversalAxes (joint,ax1,ax2); |
|
1978 |
|
1979 // It should be possible to get both angles without explicitly |
|
1980 // constructing the rotation matrix of the cross. Basically, |
|
1981 // orientation of the cross about axis1 comes from body 2, |
|
1982 // about axis 2 comes from body 1, and the perpendicular |
|
1983 // axis can come from the two bodies somehow. (We don't really |
|
1984 // want to assume it's 90 degrees, because in general the |
|
1985 // constraints won't be perfectly satisfied, or even very well |
|
1986 // satisfied.) |
|
1987 // |
|
1988 // However, we'd need a version of getHingeAngleFromRElativeQuat() |
|
1989 // that CAN handle when its relative quat is rotated along a direction |
|
1990 // other than the given axis. What I have here works, |
|
1991 // although it's probably much slower than need be. |
|
1992 |
|
1993 dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
|
1994 dRtoQ(R, qcross); |
|
1995 |
|
1996 if (joint->node[1].body) { |
|
1997 dQMultiply1 (qq, joint->node[1].body->q, qcross); |
|
1998 dQMultiply2 (qrel,qq,joint->qrel2); |
|
1999 } |
|
2000 else { |
|
2001 // pretend joint->node[1].body->q is the identity |
|
2002 dQMultiply2 (qrel,qcross, joint->qrel2); |
|
2003 } |
|
2004 |
|
2005 return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); |
|
2006 } |
|
2007 return 0; |
|
2008 } |
|
2009 |
|
2010 |
|
2011 static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) |
|
2012 { |
|
2013 info->nub = 4; |
|
2014 info->m = 4; |
|
2015 |
|
2016 // see if we're powered or at a joint limit. |
|
2017 bool constraint1 = j->limot1.fmax > 0; |
|
2018 bool constraint2 = j->limot2.fmax > 0; |
|
2019 |
|
2020 bool limiting1 = (j->limot1.lostop >= -dPI || j->limot1.histop <= dPI) && |
|
2021 j->limot1.lostop <= j->limot1.histop; |
|
2022 bool limiting2 = (j->limot2.lostop >= -dPI || j->limot2.histop <= dPI) && |
|
2023 j->limot2.lostop <= j->limot2.histop; |
|
2024 |
|
2025 // We need to call testRotationLimit() even if we're motored, since it |
|
2026 // records the result. |
|
2027 if (limiting1 || limiting2) { |
|
2028 dReal angle1, angle2; |
|
2029 getUniversalAngles (j, &angle1, &angle2); |
|
2030 if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; |
|
2031 if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; |
|
2032 } |
|
2033 if (constraint1) |
|
2034 info->m++; |
|
2035 if (constraint2) |
|
2036 info->m++; |
|
2037 } |
|
2038 |
|
2039 |
|
2040 static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) |
|
2041 { |
|
2042 // set the three ball-and-socket rows |
|
2043 setBall (joint,info,joint->anchor1,joint->anchor2); |
|
2044 |
|
2045 // set the universal joint row. the angular velocity about an axis |
|
2046 // perpendicular to both joint axes should be equal. thus the constraint |
|
2047 // equation is |
|
2048 // p*w1 - p*w2 = 0 |
|
2049 // where p is a vector normal to both joint axes, and w1 and w2 |
|
2050 // are the angular velocity vectors of the two bodies. |
|
2051 |
|
2052 // length 1 joint axis in global coordinates, from each body |
|
2053 dVector3 ax1, ax2; |
|
2054 dVector3 ax2_temp; |
|
2055 // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate |
|
2056 // about this. |
|
2057 dVector3 p; |
|
2058 dReal k; |
|
2059 |
|
2060 getUniversalAxes(joint, ax1, ax2); |
|
2061 k = dDOT(ax1, ax2); |
|
2062 ax2_temp[0] = ax2[0] - dMUL(k,ax1[0]); |
|
2063 ax2_temp[1] = ax2[1] - dMUL(k,ax1[1]); |
|
2064 ax2_temp[2] = ax2[2] - dMUL(k,ax1[2]); |
|
2065 dCROSS(p, =, ax1, ax2_temp); |
|
2066 dNormalize3(p); |
|
2067 |
|
2068 int s3=3*info->rowskip; |
|
2069 |
|
2070 info->J1a[s3+0] = p[0]; |
|
2071 info->J1a[s3+1] = p[1]; |
|
2072 info->J1a[s3+2] = p[2]; |
|
2073 |
|
2074 if (joint->node[1].body) { |
|
2075 info->J2a[s3+0] = -p[0]; |
|
2076 info->J2a[s3+1] = -p[1]; |
|
2077 info->J2a[s3+2] = -p[2]; |
|
2078 } |
|
2079 |
|
2080 // compute the right hand side of the constraint equation. set relative |
|
2081 // body velocities along p to bring the axes back to perpendicular. |
|
2082 // If ax1, ax2 are unit length joint axes as computed from body1 and |
|
2083 // body2, we need to rotate both bodies along the axis p. If theta |
|
2084 // is the angle between ax1 and ax2, we need an angular velocity |
|
2085 // along p to cover the angle erp * (theta - Pi/2) in one step: |
|
2086 // |
|
2087 // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize |
|
2088 // = (erp*fps) * (theta - Pi/2) |
|
2089 // |
|
2090 // if theta is close to Pi/2, |
|
2091 // theta - Pi/2 ~= cos(theta), so |
|
2092 // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) |
|
2093 |
|
2094 info->c[3] = dMUL(info->fps,dMUL(info->erp,- dDOT(ax1, ax2))); |
|
2095 |
|
2096 // if the first angle is powered, or has joint limits, add in the stuff |
|
2097 int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); |
|
2098 |
|
2099 // if the second angle is powered, or has joint limits, add in more stuff |
|
2100 joint->limot2.addLimot (joint,info,row,ax2,1); |
|
2101 } |
|
2102 |
|
2103 |
|
2104 static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) |
|
2105 { |
|
2106 if (joint->node[0].body) { |
|
2107 dVector3 ax1, ax2; |
|
2108 dMatrix3 R; |
|
2109 dQuaternion qcross; |
|
2110 |
|
2111 getUniversalAxes(joint, ax1, ax2); |
|
2112 |
|
2113 // Axis 1. |
|
2114 dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); |
|
2115 dRtoQ(R, qcross); |
|
2116 dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); |
|
2117 |
|
2118 // Axis 2. |
|
2119 dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); |
|
2120 dRtoQ(R, qcross); |
|
2121 if (joint->node[1].body) { |
|
2122 dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); |
|
2123 } |
|
2124 else { |
|
2125 // set joint->qrel to qcross |
|
2126 for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; |
|
2127 } |
|
2128 } |
|
2129 } |
|
2130 |
|
2131 |
|
2132 EXPORT_C void dJointSetUniversalAnchor (dJointID j, dReal x, dReal y, dReal z) |
|
2133 { |
|
2134 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2135 |
|
2136 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); |
|
2137 universalComputeInitialRelativeRotations(joint); |
|
2138 } |
|
2139 |
|
2140 |
|
2141 EXPORT_C void dJointSetUniversalAxis1 (dJointID j, dReal x, dReal y, dReal z) |
|
2142 { |
|
2143 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2144 |
|
2145 if (joint->flags & dJOINT_REVERSE) |
|
2146 setAxes (joint,x,y,z,NULL,joint->axis2); |
|
2147 else |
|
2148 setAxes (joint,x,y,z,joint->axis1,NULL); |
|
2149 universalComputeInitialRelativeRotations(joint); |
|
2150 } |
|
2151 |
|
2152 |
|
2153 EXPORT_C void dJointSetUniversalAxis2 (dJointID j, dReal x, dReal y, dReal z) |
|
2154 { |
|
2155 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2156 |
|
2157 if (joint->flags & dJOINT_REVERSE) |
|
2158 setAxes (joint,x,y,z,joint->axis1,NULL); |
|
2159 else |
|
2160 setAxes (joint,x,y,z,NULL,joint->axis2); |
|
2161 universalComputeInitialRelativeRotations(joint); |
|
2162 } |
|
2163 |
|
2164 |
|
2165 EXPORT_C void dJointGetUniversalAnchor (dJointID j, dVector3 result) |
|
2166 { |
|
2167 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2168 |
|
2169 if (joint->flags & dJOINT_REVERSE) |
|
2170 getAnchor2 (joint,result,joint->anchor2); |
|
2171 else |
|
2172 getAnchor (joint,result,joint->anchor1); |
|
2173 } |
|
2174 |
|
2175 |
|
2176 EXPORT_C void dJointGetUniversalAnchor2 (dJointID j, dVector3 result) |
|
2177 { |
|
2178 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2179 |
|
2180 if (joint->flags & dJOINT_REVERSE) |
|
2181 getAnchor (joint,result,joint->anchor1); |
|
2182 else |
|
2183 getAnchor2 (joint,result,joint->anchor2); |
|
2184 } |
|
2185 |
|
2186 |
|
2187 EXPORT_C void dJointGetUniversalAxis1 (dJointID j, dVector3 result) |
|
2188 { |
|
2189 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2190 |
|
2191 if (joint->flags & dJOINT_REVERSE) |
|
2192 getAxis2 (joint,result,joint->axis2); |
|
2193 else |
|
2194 getAxis (joint,result,joint->axis1); |
|
2195 } |
|
2196 |
|
2197 |
|
2198 EXPORT_C void dJointGetUniversalAxis2 (dJointID j, dVector3 result) |
|
2199 { |
|
2200 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2201 |
|
2202 if (joint->flags & dJOINT_REVERSE) |
|
2203 getAxis (joint,result,joint->axis1); |
|
2204 else |
|
2205 getAxis2 (joint,result,joint->axis2); |
|
2206 } |
|
2207 |
|
2208 |
|
2209 EXPORT_C void dJointSetUniversalParam (dJointID j, int parameter, dReal value) |
|
2210 { |
|
2211 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2212 |
|
2213 if ((parameter & 0xff00) == 0x100) { |
|
2214 joint->limot2.set (parameter & 0xff,value); |
|
2215 } |
|
2216 else { |
|
2217 joint->limot1.set (parameter,value); |
|
2218 } |
|
2219 } |
|
2220 |
|
2221 |
|
2222 EXPORT_C dReal dJointGetUniversalParam (dJointID j, int parameter) |
|
2223 { |
|
2224 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2225 |
|
2226 if ((parameter & 0xff00) == 0x100) { |
|
2227 return joint->limot2.get (parameter & 0xff); |
|
2228 } |
|
2229 else { |
|
2230 return joint->limot1.get (parameter); |
|
2231 } |
|
2232 } |
|
2233 |
|
2234 EXPORT_C void dJointGetUniversalAngles (dJointID j, dReal *angle1, dReal *angle2) |
|
2235 { |
|
2236 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2237 |
|
2238 if (joint->flags & dJOINT_REVERSE) |
|
2239 return getUniversalAngles (joint, angle2, angle1); |
|
2240 else |
|
2241 return getUniversalAngles (joint, angle1, angle2); |
|
2242 } |
|
2243 |
|
2244 |
|
2245 EXPORT_C dReal dJointGetUniversalAngle1 (dJointID j) |
|
2246 { |
|
2247 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2248 |
|
2249 if (joint->flags & dJOINT_REVERSE) |
|
2250 return getUniversalAngle2 (joint); |
|
2251 else |
|
2252 return getUniversalAngle1 (joint); |
|
2253 } |
|
2254 |
|
2255 |
|
2256 EXPORT_C dReal dJointGetUniversalAngle2 (dJointID j) |
|
2257 { |
|
2258 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2259 |
|
2260 if (joint->flags & dJOINT_REVERSE) |
|
2261 return getUniversalAngle1 (joint); |
|
2262 else |
|
2263 return getUniversalAngle2 (joint); |
|
2264 } |
|
2265 |
|
2266 |
|
2267 EXPORT_C dReal dJointGetUniversalAngle1Rate (dJointID j) |
|
2268 { |
|
2269 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2270 |
|
2271 |
|
2272 if (joint->node[0].body) { |
|
2273 dVector3 axis; |
|
2274 |
|
2275 if (joint->flags & dJOINT_REVERSE) |
|
2276 getAxis2 (joint,axis,joint->axis2); |
|
2277 else |
|
2278 getAxis (joint,axis,joint->axis1); |
|
2279 |
|
2280 dReal rate = dDOT(axis, joint->node[0].body->avel); |
|
2281 if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); |
|
2282 return rate; |
|
2283 } |
|
2284 return 0; |
|
2285 } |
|
2286 |
|
2287 |
|
2288 EXPORT_C dReal dJointGetUniversalAngle2Rate (dJointID j) |
|
2289 { |
|
2290 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2291 |
|
2292 |
|
2293 if (joint->node[0].body) { |
|
2294 dVector3 axis; |
|
2295 |
|
2296 if (joint->flags & dJOINT_REVERSE) |
|
2297 getAxis (joint,axis,joint->axis1); |
|
2298 else |
|
2299 getAxis2 (joint,axis,joint->axis2); |
|
2300 |
|
2301 dReal rate = dDOT(axis, joint->node[0].body->avel); |
|
2302 if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); |
|
2303 return rate; |
|
2304 } |
|
2305 return 0; |
|
2306 } |
|
2307 |
|
2308 |
|
2309 EXPORT_C void dJointAddUniversalTorques (dJointID j, dReal torque1, dReal torque2) |
|
2310 { |
|
2311 dxJointUniversal* joint = (dxJointUniversal*)j; |
|
2312 dVector3 axis1, axis2; |
|
2313 |
|
2314 |
|
2315 if (joint->flags & dJOINT_REVERSE) { |
|
2316 dReal temp = torque1; |
|
2317 torque1 = - torque2; |
|
2318 torque2 = - temp; |
|
2319 } |
|
2320 |
|
2321 getAxis (joint,axis1,joint->axis1); |
|
2322 getAxis2 (joint,axis2,joint->axis2); |
|
2323 axis1[0] = dMUL(axis1[0],torque1) + dMUL(axis2[0],torque2); |
|
2324 axis1[1] = dMUL(axis1[1],torque1) + dMUL(axis2[1],torque2); |
|
2325 axis1[2] = dMUL(axis1[2],torque1) + dMUL(axis2[2],torque2); |
|
2326 |
|
2327 if (joint->node[0].body != 0) |
|
2328 dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); |
|
2329 if (joint->node[1].body != 0) |
|
2330 dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); |
|
2331 } |
|
2332 |
|
2333 |
|
2334 |
|
2335 |
|
2336 |
|
2337 dxJoint::Vtable __duniversal_vtable = { |
|
2338 sizeof(dxJointUniversal), |
|
2339 (dxJoint::init_fn*) universalInit, |
|
2340 (dxJoint::getInfo1_fn*) universalGetInfo1, |
|
2341 (dxJoint::getInfo2_fn*) universalGetInfo2, |
|
2342 dJointTypeUniversal}; |
|
2343 |
|
2344 |
|
2345 |
|
2346 //**************************************************************************** |
|
2347 // Prismatic and Rotoide |
|
2348 |
|
2349 static void PRInit (dxJointPR *j) |
|
2350 { |
|
2351 // Default Position |
|
2352 // Z^ |
|
2353 // | Body 1 P R Body2 |
|
2354 // |+---------+ _ _ +-----------+ |
|
2355 // || |----|----(_)--------+ | |
|
2356 // |+---------+ - +-----------+ |
|
2357 // | |
|
2358 // X.-----------------------------------------> Y |
|
2359 // N.B. X is comming out of the page |
|
2360 dSetZero (j->anchor2,4); |
|
2361 |
|
2362 dSetZero (j->axisR1,4); |
|
2363 j->axisR1[0] = REAL(1.0); |
|
2364 dSetZero (j->axisR2,4); |
|
2365 j->axisR2[0] = REAL(1.0); |
|
2366 |
|
2367 dSetZero (j->axisP1,4); |
|
2368 j->axisP1[1] = REAL(1.0); |
|
2369 dSetZero (j->qrel,4); |
|
2370 dSetZero (j->offset,4); |
|
2371 |
|
2372 j->limotR.init (j->world); |
|
2373 j->limotP.init (j->world); |
|
2374 } |
|
2375 |
|
2376 |
|
2377 EXPORT_C dReal dJointGetPRPosition (dJointID j) |
|
2378 { |
|
2379 dxJointPR* joint = (dxJointPR*)j; |
|
2380 |
|
2381 |
|
2382 dVector3 q; |
|
2383 // get the offset in global coordinates |
|
2384 dMULTIPLY0_331 (q,joint->node[0].body->posr.R,joint->offset); |
|
2385 |
|
2386 if (joint->node[1].body) { |
|
2387 dVector3 anchor2; |
|
2388 |
|
2389 // get the anchor2 in global coordinates |
|
2390 dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2); |
|
2391 |
|
2392 q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) - |
|
2393 (joint->node[1].body->posr.pos[0] + anchor2[0]) ); |
|
2394 q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) - |
|
2395 (joint->node[1].body->posr.pos[1] + anchor2[1]) ); |
|
2396 q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) - |
|
2397 (joint->node[1].body->posr.pos[2] + anchor2[2]) ); |
|
2398 |
|
2399 } |
|
2400 else { |
|
2401 //N.B. When there is no body 2 the joint->anchor2 is already in |
|
2402 // global coordinates |
|
2403 |
|
2404 q[0] = ( (joint->node[0].body->posr.pos[0] + q[0]) - |
|
2405 (joint->anchor2[0]) ); |
|
2406 q[1] = ( (joint->node[0].body->posr.pos[1] + q[1]) - |
|
2407 (joint->anchor2[1]) ); |
|
2408 q[2] = ( (joint->node[0].body->posr.pos[2] + q[2]) - |
|
2409 (joint->anchor2[2]) ); |
|
2410 |
|
2411 } |
|
2412 |
|
2413 dVector3 axP; |
|
2414 // get prismatic axis in global coordinates |
|
2415 dMULTIPLY0_331 (axP,joint->node[0].body->posr.R,joint->axisP1); |
|
2416 |
|
2417 return dDOT(axP, q); |
|
2418 } |
|
2419 |
|
2420 |
|
2421 EXPORT_C dReal dJointGetPRPositionRate (dJointID j) |
|
2422 { |
|
2423 dxJointPR* joint = (dxJointPR*)j; |
|
2424 |
|
2425 |
|
2426 if (joint->node[0].body) { |
|
2427 // We want to find the rate of change of the prismatic part of the joint |
|
2428 // We can find it by looking at the speed difference between body1 and the |
|
2429 // anchor point. |
|
2430 |
|
2431 // r will be used to find the distance between body1 and the anchor point |
|
2432 dVector3 r; |
|
2433 if (joint->node[1].body) { |
|
2434 // Find joint->anchor2 in global coordinates |
|
2435 dVector3 anchor2; |
|
2436 dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R,joint->anchor2); |
|
2437 |
|
2438 r[0] = joint->node[0].body->posr.pos[0] - anchor2[0]; |
|
2439 r[1] = joint->node[0].body->posr.pos[1] - anchor2[1]; |
|
2440 r[2] = joint->node[0].body->posr.pos[2] - anchor2[2]; |
|
2441 } |
|
2442 else { |
|
2443 //N.B. When there is no body 2 the joint->anchor2 is already in |
|
2444 // global coordinates |
|
2445 r[0] = joint->node[0].body->posr.pos[0] - joint->anchor2[0]; |
|
2446 r[1] = joint->node[0].body->posr.pos[1] - joint->anchor2[1]; |
|
2447 r[2] = joint->node[0].body->posr.pos[2] - joint->anchor2[2]; |
|
2448 } |
|
2449 |
|
2450 // The body1 can have velocity coming from the rotation of |
|
2451 // the rotoide axis. We need to remove this. |
|
2452 |
|
2453 // Take only the angular rotation coming from the rotation |
|
2454 // of the rotoide articulation |
|
2455 // N.B. Body1 and Body2 should have the same rotation along axis |
|
2456 // other than the rotoide axis. |
|
2457 dVector3 angular; |
|
2458 dMULTIPLY0_331 (angular,joint->node[0].body->posr.R,joint->axisR1); |
|
2459 dReal omega = dDOT(angular, joint->node[0].body->avel); |
|
2460 angular[0] = dMUL(angular[0],omega); |
|
2461 angular[1] = dMUL(angular[1],omega); |
|
2462 angular[2] = dMUL(angular[2],omega); |
|
2463 |
|
2464 // Find the contribution of the angular rotation to the linear speed |
|
2465 // N.B. We do vel = r X w instead of vel = w x r to have vel negative |
|
2466 // since we want to remove it from the linear velocity of the body |
|
2467 dVector3 lvel1; |
|
2468 dCROSS(lvel1, =, r, angular); |
|
2469 |
|
2470 lvel1[0] += joint->node[0].body->lvel[0]; |
|
2471 lvel1[1] += joint->node[0].body->lvel[1]; |
|
2472 lvel1[2] += joint->node[0].body->lvel[2]; |
|
2473 |
|
2474 // Since we want rate of change along the prismatic axis |
|
2475 // get axisP1 in global coordinates and get the component |
|
2476 // along this axis only |
|
2477 dVector3 axP1; |
|
2478 dMULTIPLY0_331 (axP1,joint->node[0].body->posr.R,joint->axisP1); |
|
2479 return dDOT(axP1, lvel1); |
|
2480 } |
|
2481 |
|
2482 return REAL(0.0); |
|
2483 } |
|
2484 |
|
2485 |
|
2486 |
|
2487 static void PRGetInfo1 (dxJointPR *j, dxJoint::Info1 *info) |
|
2488 { |
|
2489 info->m = 4; |
|
2490 info->nub = 4; |
|
2491 |
|
2492 bool added = false; |
|
2493 |
|
2494 added = false; |
|
2495 // see if the prismatic articulation is powered |
|
2496 if (j->limotP.fmax > 0) |
|
2497 { |
|
2498 added = true; |
|
2499 (info->m)++; // powered needs an extra constraint row |
|
2500 } |
|
2501 |
|
2502 // see if we're at a joint limit. |
|
2503 j->limotP.limit = 0; |
|
2504 if ((j->limotP.lostop > -dInfinity || j->limotP.histop < dInfinity) && |
|
2505 j->limotP.lostop <= j->limotP.histop) { |
|
2506 // measure joint position |
|
2507 dReal pos = dJointGetPRPosition (j); |
|
2508 if (pos <= j->limotP.lostop) { |
|
2509 j->limotP.limit = 1; |
|
2510 j->limotP.limit_err = pos - j->limotP.lostop; |
|
2511 if (!added) |
|
2512 (info->m)++; |
|
2513 } |
|
2514 |
|
2515 if (pos >= j->limotP.histop) { |
|
2516 j->limotP.limit = 2; |
|
2517 j->limotP.limit_err = pos - j->limotP.histop; |
|
2518 if (!added) |
|
2519 (info->m)++; |
|
2520 } |
|
2521 } |
|
2522 |
|
2523 } |
|
2524 |
|
2525 |
|
2526 |
|
2527 static void PRGetInfo2 (dxJointPR *joint, dxJoint::Info2 *info) |
|
2528 { |
|
2529 int s = info->rowskip; |
|
2530 int s2= 2*s; |
|
2531 int s3= 3*s; |
|
2532 int s4= 4*s; |
|
2533 |
|
2534 dReal k = dMUL(info->fps,info->erp); |
|
2535 |
|
2536 |
|
2537 dVector3 q; // plane space of axP and after that axR |
|
2538 |
|
2539 // pull out pos and R for both bodies. also get the `connection' |
|
2540 // vector pos2-pos1. |
|
2541 |
|
2542 dReal *pos1,*pos2 = 0,*R1,*R2 = 0; |
|
2543 pos1 = joint->node[0].body->posr.pos; |
|
2544 R1 = joint->node[0].body->posr.R; |
|
2545 if (joint->node[1].body) { |
|
2546 pos2 = joint->node[1].body->posr.pos; |
|
2547 R2 = joint->node[1].body->posr.R; |
|
2548 } |
|
2549 else { |
|
2550 // pos2 = 0; // N.B. We can do that to be safe but it is no necessary |
|
2551 // R2 = 0; // N.B. We can do that to be safe but it is no necessary |
|
2552 } |
|
2553 |
|
2554 |
|
2555 dVector3 axP; // Axis of the prismatic joint in global frame |
|
2556 dMULTIPLY0_331 (axP, R1, joint->axisP1); |
|
2557 |
|
2558 // distance between the body1 and the anchor2 in global frame |
|
2559 // Calculated in the same way as the offset |
|
2560 dVector3 dist; |
|
2561 |
|
2562 if (joint->node[1].body) |
|
2563 { |
|
2564 dMULTIPLY0_331 (dist, R2, joint->anchor2); |
|
2565 dist[0] += pos2[0] - pos1[0]; |
|
2566 dist[1] += pos2[1] - pos1[1]; |
|
2567 dist[2] += pos2[2] - pos1[2]; |
|
2568 } |
|
2569 else { |
|
2570 dist[0] = joint->anchor2[0] - pos1[0]; |
|
2571 dist[1] = joint->anchor2[1] - pos1[1]; |
|
2572 dist[2] = joint->anchor2[2] - pos1[2]; |
|
2573 } |
|
2574 |
|
2575 |
|
2576 // ====================================================================== |
|
2577 // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered |
|
2578 |
|
2579 // Set the two rotoide rows. The rotoide axis should be the only unconstrained |
|
2580 // rotational axis, the angular velocity of the two bodies perpendicular to |
|
2581 // the rotoide axis should be equal. Thus the constraint equations are |
|
2582 // p*w1 - p*w2 = 0 |
|
2583 // q*w1 - q*w2 = 0 |
|
2584 // where p and q are unit vectors normal to the rotoide axis, and w1 and w2 |
|
2585 // are the angular velocity vectors of the two bodies. |
|
2586 dVector3 ax1; |
|
2587 dMULTIPLY0_331 (ax1, joint->node[0].body->posr.R, joint->axisR1); |
|
2588 dCROSS(q , =, ax1, axP); |
|
2589 |
|
2590 info->J1a[0] = axP[0]; |
|
2591 info->J1a[1] = axP[1]; |
|
2592 info->J1a[2] = axP[2]; |
|
2593 info->J1a[s+0] = q[0]; |
|
2594 info->J1a[s+1] = q[1]; |
|
2595 info->J1a[s+2] = q[2]; |
|
2596 |
|
2597 if (joint->node[1].body) { |
|
2598 info->J2a[0] = -axP[0]; |
|
2599 info->J2a[1] = -axP[1]; |
|
2600 info->J2a[2] = -axP[2]; |
|
2601 info->J2a[s+0] = -q[0]; |
|
2602 info->J2a[s+1] = -q[1]; |
|
2603 info->J2a[s+2] = -q[2]; |
|
2604 } |
|
2605 |
|
2606 |
|
2607 // Compute the right hand side of the constraint equation set. Relative |
|
2608 // body velocities along p and q to bring the rotoide back into alignment. |
|
2609 // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame. |
|
2610 // We need to rotate both bodies along the axis u = (ax1 x ax2). |
|
2611 // if `theta' is the angle between ax1 and ax2, we need an angular velocity |
|
2612 // along u to cover angle erp*theta in one step : |
|
2613 // |angular_velocity| = angle/time = erp*theta / stepsize |
|
2614 // = (erp*fps) * theta |
|
2615 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| |
|
2616 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) |
|
2617 // ...as ax1 and ax2 are unit length. if theta is smallish, |
|
2618 // theta ~= sin(theta), so |
|
2619 // angular_velocity = (erp*fps) * (ax1 x ax2) |
|
2620 // ax1 x ax2 is in the plane space of ax1, so we project the angular |
|
2621 // velocity to p and q to find the right hand side. |
|
2622 |
|
2623 dVector3 ax2; |
|
2624 if (joint->node[1].body) { |
|
2625 dMULTIPLY0_331 (ax2, R2, joint->axisR2); |
|
2626 } |
|
2627 else { |
|
2628 ax2[0] = joint->axisR2[0]; |
|
2629 ax2[1] = joint->axisR2[1]; |
|
2630 ax2[2] = joint->axisR2[2]; |
|
2631 } |
|
2632 |
|
2633 dVector3 b; |
|
2634 dCROSS (b,=,ax1, ax2); |
|
2635 info->c[0] = dMUL(k,dDOT(b, axP)); |
|
2636 info->c[1] = dMUL(k,dDOT(b, q)); |
|
2637 |
|
2638 |
|
2639 |
|
2640 // ========================== |
|
2641 // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered |
|
2642 // or 5 if rotoide and prismatic powered |
|
2643 |
|
2644 // two rows. we want: vel2 = vel1 + w1 x c ... but this would |
|
2645 // result in three equations, so we project along the planespace vectors |
|
2646 // so that sliding along the prismatic axis is disregarded. for symmetry we |
|
2647 // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. |
|
2648 |
|
2649 // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist' |
|
2650 // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p |
|
2651 // v_p is speed of prismatic joint (i.e. elongation rate) |
|
2652 // Since the constraints are perpendicular to v_p we have: |
|
2653 // p dot v_p = 0 and q dot v_p = 0 |
|
2654 // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 ) |
|
2655 // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 ) |
|
2656 // == |
|
2657 // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist |
|
2658 // since a . (b x c) = - b . (a x c) = - (a x c) . b |
|
2659 // and a x b = - b x a |
|
2660 // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0 |
|
2661 // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0 |
|
2662 // Coeff for 1er line of: J1l => ax1, J2l => -ax1 |
|
2663 // Coeff for 2er line of: J1l => q, J2l => -q |
|
2664 // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1 |
|
2665 // Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q |
|
2666 |
|
2667 |
|
2668 dCROSS ((info->J1a)+s2, = , dist, ax1); |
|
2669 |
|
2670 dCROSS ((info->J1a)+s3, = , dist, q); |
|
2671 |
|
2672 |
|
2673 info->J1l[s2+0] = ax1[0]; |
|
2674 info->J1l[s2+1] = ax1[1]; |
|
2675 info->J1l[s2+2] = ax1[2]; |
|
2676 |
|
2677 info->J1l[s3+0] = q[0]; |
|
2678 info->J1l[s3+1] = q[1]; |
|
2679 info->J1l[s3+2] = q[2]; |
|
2680 |
|
2681 if (joint->node[1].body) { |
|
2682 dVector3 anchor2; |
|
2683 |
|
2684 // Calculate anchor2 in world coordinate |
|
2685 dMULTIPLY0_331 (anchor2, R2, joint->anchor2); |
|
2686 |
|
2687 // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value |
|
2688 dCROSS ((info->J2a)+s2, = , ax2, anchor2); // since ax1 == ax2 |
|
2689 |
|
2690 // The cross product is in reverse order since we want the negative value |
|
2691 dCROSS ((info->J2a)+s3, = , q, anchor2); |
|
2692 |
|
2693 info->J2l[s2+0] = -ax1[0]; |
|
2694 info->J2l[s2+1] = -ax1[1]; |
|
2695 info->J2l[s2+2] = -ax1[2]; |
|
2696 |
|
2697 info->J2l[s3+0] = -q[0]; |
|
2698 info->J2l[s3+1] = -q[1]; |
|
2699 info->J2l[s3+2] = -q[2]; |
|
2700 } |
|
2701 |
|
2702 |
|
2703 // We want to make correction for motion not in the line of the axisP |
|
2704 // We calculate the displacement w.r.t. the anchor pt. |
|
2705 // |
|
2706 // compute the elements 2 and 3 of right hand side. |
|
2707 // we want to align the offset point (in body 2's frame) with the center of body 1. |
|
2708 // The position should be the same when we are not along the prismatic axis |
|
2709 dVector3 err; |
|
2710 dMULTIPLY0_331 (err, R1, joint->offset); |
|
2711 err[0] += dist[0]; |
|
2712 err[1] += dist[1]; |
|
2713 err[2] += dist[2]; |
|
2714 info->c[2] = dMUL(k,dDOT(ax1, err)); |
|
2715 info->c[3] = dMUL(k,dDOT(q, err)); |
|
2716 |
|
2717 // Here we can't use addLimot because of some assumption in the function |
|
2718 int powered = joint->limotP.fmax > 0; |
|
2719 if (powered || joint->limotP.limit) { |
|
2720 info->J1l[s4+0] = axP[0]; |
|
2721 info->J1l[s4+1] = axP[1]; |
|
2722 info->J1l[s4+2] = axP[2]; |
|
2723 if (joint->node[1].body) { |
|
2724 info->J2l[s4+0] = -axP[0]; |
|
2725 info->J2l[s4+1] = -axP[1]; |
|
2726 info->J2l[s4+2] = -axP[2]; |
|
2727 } |
|
2728 // linear limot torque decoupling step: |
|
2729 // |
|
2730 // if this is a linear limot (e.g. from a slider), we have to be careful |
|
2731 // that the linear constraint forces (+/- ax1) applied to the two bodies |
|
2732 // do not create a torque couple. in other words, the points that the |
|
2733 // constraint force is applied at must lie along the same ax1 axis. |
|
2734 // a torque couple will result in powered or limited slider-jointed free |
|
2735 // bodies from gaining angular momentum. |
|
2736 // the solution used here is to apply the constraint forces at the point |
|
2737 // halfway between the body centers. there is no penalty (other than an |
|
2738 // extra tiny bit of computation) in doing this adjustment. note that we |
|
2739 // only need to do this if the constraint connects two bodies. |
|
2740 |
|
2741 dVector3 ltd = {}; // Linear Torque Decoupling vector (a torque) |
|
2742 if (joint->node[1].body) { |
|
2743 dVector3 c; |
|
2744 c[0]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0])); |
|
2745 c[1]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1])); |
|
2746 c[2]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2])); |
|
2747 dReal val = dDOT(q, c); |
|
2748 c[0] -= dMUL(val,c[0]); |
|
2749 c[1] -= dMUL(val,c[1]); |
|
2750 c[2] -= dMUL(val,c[2]); |
|
2751 |
|
2752 dCROSS (ltd,=,c,axP); |
|
2753 info->J1a[s4+0] = ltd[0]; |
|
2754 info->J1a[s4+1] = ltd[1]; |
|
2755 info->J1a[s4+2] = ltd[2]; |
|
2756 info->J2a[s4+0] = ltd[0]; |
|
2757 info->J2a[s4+1] = ltd[1]; |
|
2758 info->J2a[s4+2] = ltd[2]; |
|
2759 } |
|
2760 |
|
2761 // if we're limited low and high simultaneously, the joint motor is |
|
2762 // ineffective |
|
2763 if (joint->limotP.limit && (joint->limotP.lostop == joint->limotP.histop)) |
|
2764 powered = 0; |
|
2765 |
|
2766 int row = 4; |
|
2767 if (powered) { |
|
2768 info->cfm[row] = joint->limotP.normal_cfm; |
|
2769 if (!joint->limotP.limit) { |
|
2770 info->c[row] = joint->limotP.vel; |
|
2771 info->lo[row] = -joint->limotP.fmax; |
|
2772 info->hi[row] = joint->limotP.fmax; |
|
2773 } |
|
2774 else { |
|
2775 // the joint is at a limit, AND is being powered. if the joint is |
|
2776 // being powered into the limit then we apply the maximum motor force |
|
2777 // in that direction, because the motor is working against the |
|
2778 // immovable limit. if the joint is being powered away from the limit |
|
2779 // then we have problems because actually we need *two* lcp |
|
2780 // constraints to handle this case. so we fake it and apply some |
|
2781 // fraction of the maximum force. the fraction to use can be set as |
|
2782 // a fudge factor. |
|
2783 |
|
2784 dReal fm = joint->limotP.fmax; |
|
2785 dReal vel = joint->limotP.vel; |
|
2786 int limit = joint->limotP.limit; |
|
2787 if ((vel > 0) || (vel==0 && limit==2)) fm = -fm; |
|
2788 |
|
2789 // if we're powering away from the limit, apply the fudge factor |
|
2790 if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) |
|
2791 fm = dMUL(fm,joint->limotP.fudge_factor); |
|
2792 |
|
2793 |
|
2794 dBodyAddForce (joint->node[0].body,-dMUL(fm,axP[0]),-dMUL(fm,axP[1]),-dMUL(fm,axP[2])); |
|
2795 |
|
2796 if (joint->node[1].body) { |
|
2797 dBodyAddForce (joint->node[1].body,dMUL(fm,axP[0]),dMUL(fm,axP[1]),dMUL(fm,axP[2])); |
|
2798 |
|
2799 // linear limot torque decoupling step: refer to above discussion |
|
2800 dBodyAddTorque (joint->node[0].body,-dMUL(fm,ltd[0]),-dMUL(fm,ltd[1]), |
|
2801 -dMUL(fm,ltd[2])); |
|
2802 dBodyAddTorque (joint->node[1].body,-dMUL(fm,ltd[0]),-dMUL(fm,ltd[1]), |
|
2803 -dMUL(fm,ltd[2])); |
|
2804 } |
|
2805 } |
|
2806 } |
|
2807 |
|
2808 if (joint->limotP.limit) { |
|
2809 dReal k = dMUL(info->fps,joint->limotP.stop_erp); |
|
2810 info->c[row] = -dMUL(k,joint->limotP.limit_err); |
|
2811 info->cfm[row] = joint->limotP.stop_cfm; |
|
2812 |
|
2813 if (joint->limotP.lostop == joint->limotP.histop) { |
|
2814 // limited low and high simultaneously |
|
2815 info->lo[row] = -dInfinity; |
|
2816 info->hi[row] = dInfinity; |
|
2817 } |
|
2818 else { |
|
2819 if (joint->limotP.limit == 1) { |
|
2820 // low limit |
|
2821 info->lo[row] = 0; |
|
2822 info->hi[row] = dInfinity; |
|
2823 } |
|
2824 else { |
|
2825 // high limit |
|
2826 info->lo[row] = -dInfinity; |
|
2827 info->hi[row] = 0; |
|
2828 } |
|
2829 |
|
2830 // deal with bounce |
|
2831 if (joint->limotP.bounce > 0) { |
|
2832 // calculate joint velocity |
|
2833 dReal vel; |
|
2834 vel = dDOT(joint->node[0].body->lvel, axP); |
|
2835 if (joint->node[1].body) |
|
2836 vel -= dDOT(joint->node[1].body->lvel, axP); |
|
2837 |
|
2838 // only apply bounce if the velocity is incoming, and if the |
|
2839 // resulting c[] exceeds what we already have. |
|
2840 if (joint->limotP.limit == 1) { |
|
2841 // low limit |
|
2842 if (vel < 0) { |
|
2843 dReal newc = -dMUL(joint->limotP.bounce,vel); |
|
2844 if (newc > info->c[row]) info->c[row] = newc; |
|
2845 } |
|
2846 } |
|
2847 else { |
|
2848 // high limit - all those computations are reversed |
|
2849 if (vel > 0) { |
|
2850 dReal newc = -dMUL(joint->limotP.bounce,vel); |
|
2851 if (newc < info->c[row]) info->c[row] = newc; |
|
2852 } |
|
2853 } |
|
2854 } |
|
2855 } |
|
2856 } |
|
2857 } |
|
2858 } |
|
2859 |
|
2860 |
|
2861 // compute initial relative rotation body1 -> body2, or env -> body1 |
|
2862 static void PRComputeInitialRelativeRotation (dxJointPR *joint) |
|
2863 { |
|
2864 if (joint->node[0].body) { |
|
2865 if (joint->node[1].body) { |
|
2866 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
|
2867 } |
|
2868 else { |
|
2869 // set joint->qrel to the transpose of the first body q |
|
2870 joint->qrel[0] = joint->node[0].body->q[0]; |
|
2871 for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
|
2872 // WARNING do we need the - in -joint->node[0].body->q[i]; or not |
|
2873 } |
|
2874 } |
|
2875 } |
|
2876 |
|
2877 EXPORT_C void dJointSetPRAnchor (dJointID j, dReal x, dReal y, dReal z) |
|
2878 { |
|
2879 dxJointPR* joint = (dxJointPR*)j; |
|
2880 |
|
2881 |
|
2882 dVector3 dummy; |
|
2883 setAnchors (joint,x,y,z,dummy,joint->anchor2); |
|
2884 } |
|
2885 |
|
2886 |
|
2887 EXPORT_C void dJointSetPRAxis1 (dJointID j, dReal x, dReal y, dReal z) |
|
2888 { |
|
2889 dxJointPR* joint = (dxJointPR*)j; |
|
2890 //int i; |
|
2891 |
|
2892 |
|
2893 setAxes (joint,x,y,z,joint->axisP1, 0); |
|
2894 |
|
2895 PRComputeInitialRelativeRotation (joint); |
|
2896 |
|
2897 // compute initial relative rotation body1 -> body2, or env -> body1 |
|
2898 // also compute distance between anchor of body1 w.r.t center of body 2 |
|
2899 dVector3 c; |
|
2900 if (joint->node[1].body) { |
|
2901 dVector3 anchor2; |
|
2902 dMULTIPLY0_331 (anchor2,joint->node[1].body->posr.R, joint->anchor2); |
|
2903 |
|
2904 c[0] = ( joint->node[1].body->posr.pos[0] + anchor2[0] - |
|
2905 joint->node[0].body->posr.pos[0] ); |
|
2906 c[1] = ( joint->node[1].body->posr.pos[1] + anchor2[1] - |
|
2907 joint->node[0].body->posr.pos[1] ); |
|
2908 c[2] = ( joint->node[1].body->posr.pos[2] + anchor2[2] - |
|
2909 joint->node[0].body->posr.pos[2] ); |
|
2910 } |
|
2911 else if (joint->node[0].body) { |
|
2912 c[0] = joint->anchor2[0] - joint->node[0].body->posr.pos[0]; |
|
2913 c[1] = joint->anchor2[1] - joint->node[0].body->posr.pos[1]; |
|
2914 c[2] = joint->anchor2[2] - joint->node[0].body->posr.pos[2]; |
|
2915 } |
|
2916 else |
|
2917 { |
|
2918 joint->offset[0] = joint->anchor2[0]; |
|
2919 joint->offset[1] = joint->anchor2[1]; |
|
2920 joint->offset[2] = joint->anchor2[2]; |
|
2921 |
|
2922 return; |
|
2923 } |
|
2924 |
|
2925 |
|
2926 dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,c); |
|
2927 } |
|
2928 |
|
2929 |
|
2930 EXPORT_C void dJointSetPRAxis2 (dJointID j, dReal x, dReal y, dReal z) |
|
2931 { |
|
2932 dxJointPR* joint = (dxJointPR*)j; |
|
2933 |
|
2934 setAxes (joint,x,y,z,joint->axisR1,joint->axisR2); |
|
2935 PRComputeInitialRelativeRotation (joint); |
|
2936 } |
|
2937 |
|
2938 |
|
2939 EXPORT_C void dJointSetPRParam (dJointID j, int parameter, dReal value) |
|
2940 { |
|
2941 dxJointPR* joint = (dxJointPR*)j; |
|
2942 |
|
2943 if ((parameter & 0xff00) == 0x100) { |
|
2944 joint->limotR.set (parameter,value); |
|
2945 } |
|
2946 else { |
|
2947 joint->limotP.set (parameter & 0xff,value); |
|
2948 } |
|
2949 } |
|
2950 |
|
2951 EXPORT_C void dJointGetPRAnchor (dJointID j, dVector3 result) |
|
2952 { |
|
2953 dxJointPR* joint = (dxJointPR*)j; |
|
2954 |
|
2955 |
|
2956 if (joint->node[1].body) |
|
2957 getAnchor2 (joint,result,joint->anchor2); |
|
2958 else |
|
2959 { |
|
2960 result[0] = joint->anchor2[0]; |
|
2961 result[1] = joint->anchor2[1]; |
|
2962 result[2] = joint->anchor2[2]; |
|
2963 } |
|
2964 |
|
2965 } |
|
2966 |
|
2967 EXPORT_C void dJointGetPRAxis1 (dJointID j, dVector3 result) |
|
2968 { |
|
2969 dxJointPR* joint = (dxJointPR*)j; |
|
2970 |
|
2971 getAxis(joint, result, joint->axisP1); |
|
2972 } |
|
2973 |
|
2974 EXPORT_C void dJointGetPRAxis2 (dJointID j, dVector3 result) |
|
2975 { |
|
2976 dxJointPR* joint = (dxJointPR*)j; |
|
2977 |
|
2978 getAxis(joint, result, joint->axisR1); |
|
2979 } |
|
2980 |
|
2981 EXPORT_C dReal dJointGetPRParam (dJointID j, int parameter) |
|
2982 { |
|
2983 dxJointPR* joint = (dxJointPR*)j; |
|
2984 |
|
2985 if ((parameter & 0xff00) == 0x100) { |
|
2986 return joint->limotR.get (parameter & 0xff); |
|
2987 } |
|
2988 else { |
|
2989 return joint->limotP.get (parameter); |
|
2990 } |
|
2991 } |
|
2992 |
|
2993 EXPORT_C void dJointAddPRTorque (dJointID j, dReal torque) |
|
2994 { |
|
2995 dxJointPR* joint = (dxJointPR*)j; |
|
2996 dVector3 axis; |
|
2997 |
|
2998 |
|
2999 if (joint->flags & dJOINT_REVERSE) |
|
3000 torque = -torque; |
|
3001 |
|
3002 getAxis (joint,axis,joint->axisR1); |
|
3003 axis[0] = dMUL(axis[0],torque); |
|
3004 axis[1] = dMUL(axis[1],torque); |
|
3005 axis[2] = dMUL(axis[2],torque); |
|
3006 |
|
3007 if (joint->node[0].body != 0) |
|
3008 dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); |
|
3009 if (joint->node[1].body != 0) |
|
3010 dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); |
|
3011 } |
|
3012 |
|
3013 |
|
3014 dxJoint::Vtable __dPR_vtable = { |
|
3015 sizeof(dxJointPR), |
|
3016 (dxJoint::init_fn*) PRInit, |
|
3017 (dxJoint::getInfo1_fn*) PRGetInfo1, |
|
3018 (dxJoint::getInfo2_fn*) PRGetInfo2, |
|
3019 dJointTypePR |
|
3020 }; |
|
3021 |
|
3022 |
|
3023 //**************************************************************************** |
|
3024 // angular motor |
|
3025 |
|
3026 static void amotorInit (dxJointAMotor *j) |
|
3027 { |
|
3028 int i; |
|
3029 j->num = 0; |
|
3030 j->mode = dAMotorUser; |
|
3031 for (i=0; i<3; i++) { |
|
3032 j->rel[i] = 0; |
|
3033 dSetZero (j->axis[i],4); |
|
3034 j->limot[i].init (j->world); |
|
3035 j->angle[i] = 0; |
|
3036 } |
|
3037 dSetZero (j->reference1,4); |
|
3038 dSetZero (j->reference2,4); |
|
3039 } |
|
3040 |
|
3041 |
|
3042 // compute the 3 axes in global coordinates |
|
3043 |
|
3044 static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) |
|
3045 { |
|
3046 if (joint->mode == dAMotorEuler) { |
|
3047 // special handling for euler mode |
|
3048 dMULTIPLY0_331 (ax[0],joint->node[0].body->posr.R,joint->axis[0]); |
|
3049 if (joint->node[1].body) { |
|
3050 dMULTIPLY0_331 (ax[2],joint->node[1].body->posr.R,joint->axis[2]); |
|
3051 } |
|
3052 else { |
|
3053 ax[2][0] = joint->axis[2][0]; |
|
3054 ax[2][1] = joint->axis[2][1]; |
|
3055 ax[2][2] = joint->axis[2][2]; |
|
3056 } |
|
3057 dCROSS (ax[1],=,ax[2],ax[0]); |
|
3058 dNormalize3 (ax[1]); |
|
3059 } |
|
3060 else { |
|
3061 for (int i=0; i < joint->num; i++) { |
|
3062 if (joint->rel[i] == 1) { |
|
3063 // relative to b1 |
|
3064 dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); |
|
3065 } |
|
3066 else if (joint->rel[i] == 2) { |
|
3067 // relative to b2 |
|
3068 if (joint->node[1].body) { // jds: don't assert, just ignore |
|
3069 dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); |
|
3070 } |
|
3071 } |
|
3072 else { |
|
3073 // global - just copy it |
|
3074 ax[i][0] = joint->axis[i][0]; |
|
3075 ax[i][1] = joint->axis[i][1]; |
|
3076 ax[i][2] = joint->axis[i][2]; |
|
3077 } |
|
3078 } |
|
3079 } |
|
3080 } |
|
3081 |
|
3082 |
|
3083 static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) |
|
3084 { |
|
3085 // assumptions: |
|
3086 // global axes already calculated --> ax |
|
3087 // axis[0] is relative to body 1 --> global ax[0] |
|
3088 // axis[2] is relative to body 2 --> global ax[2] |
|
3089 // ax[1] = ax[2] x ax[0] |
|
3090 // original ax[0] and ax[2] are perpendicular |
|
3091 // reference1 is perpendicular to ax[0] (in body 1 frame) |
|
3092 // reference2 is perpendicular to ax[2] (in body 2 frame) |
|
3093 // all ax[] and reference vectors are unit length |
|
3094 |
|
3095 // calculate references in global frame |
|
3096 dVector3 ref1,ref2; |
|
3097 dMULTIPLY0_331 (ref1,joint->node[0].body->posr.R,joint->reference1); |
|
3098 if (joint->node[1].body) { |
|
3099 dMULTIPLY0_331 (ref2,joint->node[1].body->posr.R,joint->reference2); |
|
3100 } |
|
3101 else { |
|
3102 ref2[0] = joint->reference2[0]; |
|
3103 ref2[1] = joint->reference2[1]; |
|
3104 ref2[2] = joint->reference2[2]; |
|
3105 } |
|
3106 |
|
3107 // get q perpendicular to both ax[0] and ref1, get first euler angle |
|
3108 dVector3 q; |
|
3109 dCROSS (q,=,ax[0],ref1); |
|
3110 joint->angle[0] = -dArcTan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); |
|
3111 |
|
3112 // get q perpendicular to both ax[0] and ax[1], get second euler angle |
|
3113 dCROSS (q,=,ax[0],ax[1]); |
|
3114 joint->angle[1] = -dArcTan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); |
|
3115 |
|
3116 // get q perpendicular to both ax[1] and ax[2], get third euler angle |
|
3117 dCROSS (q,=,ax[1],ax[2]); |
|
3118 joint->angle[2] = -dArcTan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); |
|
3119 } |
|
3120 |
|
3121 |
|
3122 // set the reference vectors as follows: |
|
3123 // * reference1 = current axis[2] relative to body 1 |
|
3124 // * reference2 = current axis[0] relative to body 2 |
|
3125 // this assumes that: |
|
3126 // * axis[0] is relative to body 1 |
|
3127 // * axis[2] is relative to body 2 |
|
3128 |
|
3129 static void amotorSetEulerReferenceVectors (dxJointAMotor *j) |
|
3130 { |
|
3131 if (j->node[0].body && j->node[1].body) { |
|
3132 dVector3 r; // axis[2] and axis[0] in global coordinates |
|
3133 dMULTIPLY0_331 (r,j->node[1].body->posr.R,j->axis[2]); |
|
3134 dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); |
|
3135 dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); |
|
3136 dMULTIPLY1_331 (j->reference2,j->node[1].body->posr.R,r); |
|
3137 } |
|
3138 |
|
3139 else { // jds |
|
3140 // else if (j->node[0].body) { |
|
3141 // dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]); |
|
3142 // dMULTIPLY0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]); |
|
3143 |
|
3144 // We want to handle angular motors attached to passive geoms |
|
3145 dVector3 r; // axis[2] and axis[0] in global coordinates |
|
3146 r[0] = j->axis[2][0]; r[1] = j->axis[2][1]; r[2] = j->axis[2][2]; r[3] = j->axis[2][3]; |
|
3147 dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); |
|
3148 dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); |
|
3149 j->reference2[0] += r[0]; j->reference2[1] += r[1]; |
|
3150 j->reference2[2] += r[2]; j->reference2[3] += r[3]; |
|
3151 } |
|
3152 } |
|
3153 |
|
3154 |
|
3155 static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) |
|
3156 { |
|
3157 info->m = 0; |
|
3158 info->nub = 0; |
|
3159 |
|
3160 // compute the axes and angles, if in euler mode |
|
3161 if (j->mode == dAMotorEuler) { |
|
3162 dVector3 ax[3]; |
|
3163 amotorComputeGlobalAxes (j,ax); |
|
3164 amotorComputeEulerAngles (j,ax); |
|
3165 } |
|
3166 |
|
3167 // see if we're powered or at a joint limit for each axis |
|
3168 for (int i=0; i < j->num; i++) { |
|
3169 if (j->limot[i].testRotationalLimit (j->angle[i]) || |
|
3170 j->limot[i].fmax > 0) { |
|
3171 info->m++; |
|
3172 } |
|
3173 } |
|
3174 } |
|
3175 |
|
3176 |
|
3177 static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) |
|
3178 { |
|
3179 int i; |
|
3180 |
|
3181 // compute the axes (if not global) |
|
3182 dVector3 ax[3]; |
|
3183 amotorComputeGlobalAxes (joint,ax); |
|
3184 |
|
3185 // in euler angle mode we do not actually constrain the angular velocity |
|
3186 // along the axes axis[0] and axis[2] (although we do use axis[1]) : |
|
3187 // |
|
3188 // to get constrain w2-w1 along ...not |
|
3189 // ------ --------------------- ------ |
|
3190 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] |
|
3191 // d(angle[1])/dt = 0 ax[1] |
|
3192 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] |
|
3193 // |
|
3194 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. |
|
3195 // to prove the result for angle[0], write the expression for angle[0] from |
|
3196 // GetInfo1 then take the derivative. to prove this for angle[2] it is |
|
3197 // easier to take the euler rate expression for d(angle[2])/dt with respect |
|
3198 // to the components of w and set that to 0. |
|
3199 |
|
3200 dVector3 *axptr[3]; |
|
3201 axptr[0] = &ax[0]; |
|
3202 axptr[1] = &ax[1]; |
|
3203 axptr[2] = &ax[2]; |
|
3204 |
|
3205 dVector3 ax0_cross_ax1; |
|
3206 dVector3 ax1_cross_ax2; |
|
3207 if (joint->mode == dAMotorEuler) { |
|
3208 dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); |
|
3209 axptr[2] = &ax0_cross_ax1; |
|
3210 dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); |
|
3211 axptr[0] = &ax1_cross_ax2; |
|
3212 } |
|
3213 |
|
3214 int row=0; |
|
3215 for (i=0; i < joint->num; i++) { |
|
3216 row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); |
|
3217 } |
|
3218 } |
|
3219 |
|
3220 |
|
3221 EXPORT_C void dJointSetAMotorNumAxes (dJointID j, int num) |
|
3222 { |
|
3223 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3224 |
|
3225 if (joint->mode == dAMotorEuler) { |
|
3226 joint->num = 3; |
|
3227 } |
|
3228 else { |
|
3229 if (num < 0) num = 0; |
|
3230 if (num > 3) num = 3; |
|
3231 joint->num = num; |
|
3232 } |
|
3233 } |
|
3234 |
|
3235 |
|
3236 EXPORT_C void dJointSetAMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) |
|
3237 { |
|
3238 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3239 |
|
3240 if (anum < 0) anum = 0; |
|
3241 if (anum > 2) anum = 2; |
|
3242 |
|
3243 // adjust rel to match the internal body order |
|
3244 if (!joint->node[1].body && rel==2) rel = 1; |
|
3245 |
|
3246 joint->rel[anum] = rel; |
|
3247 |
|
3248 // x,y,z is always in global coordinates regardless of rel, so we may have |
|
3249 // to convert it to be relative to a body |
|
3250 dVector3 r; |
|
3251 r[0] = x; |
|
3252 r[1] = y; |
|
3253 r[2] = z; |
|
3254 r[3] = 0; |
|
3255 if (rel > 0) { |
|
3256 if (rel==1) { |
|
3257 dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); |
|
3258 } |
|
3259 else { |
|
3260 // don't assert; handle the case of attachment to a bodiless geom |
|
3261 if (joint->node[1].body) { // jds |
|
3262 dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); |
|
3263 } |
|
3264 else { |
|
3265 joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1]; |
|
3266 joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3]; |
|
3267 } |
|
3268 } |
|
3269 } |
|
3270 else { |
|
3271 joint->axis[anum][0] = r[0]; |
|
3272 joint->axis[anum][1] = r[1]; |
|
3273 joint->axis[anum][2] = r[2]; |
|
3274 } |
|
3275 dNormalize3 (joint->axis[anum]); |
|
3276 if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); |
|
3277 } |
|
3278 |
|
3279 |
|
3280 EXPORT_C void dJointSetAMotorAngle (dJointID j, int anum, dReal angle) |
|
3281 { |
|
3282 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3283 |
|
3284 if (joint->mode == dAMotorUser) { |
|
3285 if (anum < 0) anum = 0; |
|
3286 if (anum > 3) anum = 3; |
|
3287 joint->angle[anum] = angle; |
|
3288 } |
|
3289 } |
|
3290 |
|
3291 |
|
3292 EXPORT_C void dJointSetAMotorParam (dJointID j, int parameter, dReal value) |
|
3293 { |
|
3294 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3295 |
|
3296 int anum = parameter >> 8; |
|
3297 if (anum < 0) anum = 0; |
|
3298 if (anum > 2) anum = 2; |
|
3299 parameter &= 0xff; |
|
3300 joint->limot[anum].set (parameter, value); |
|
3301 } |
|
3302 |
|
3303 |
|
3304 EXPORT_C void dJointSetAMotorMode (dJointID j, int mode) |
|
3305 { |
|
3306 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3307 |
|
3308 joint->mode = mode; |
|
3309 if (joint->mode == dAMotorEuler) { |
|
3310 joint->num = 3; |
|
3311 amotorSetEulerReferenceVectors (joint); |
|
3312 } |
|
3313 } |
|
3314 |
|
3315 |
|
3316 EXPORT_C int dJointGetAMotorNumAxes (dJointID j) |
|
3317 { |
|
3318 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3319 |
|
3320 return joint->num; |
|
3321 } |
|
3322 |
|
3323 |
|
3324 EXPORT_C void dJointGetAMotorAxis (dJointID j, int anum, dVector3 result) |
|
3325 { |
|
3326 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3327 |
|
3328 if (anum < 0) anum = 0; |
|
3329 if (anum > 2) anum = 2; |
|
3330 if (joint->rel[anum] > 0) { |
|
3331 if (joint->rel[anum]==1) { |
|
3332 dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis[anum]); |
|
3333 } |
|
3334 else { |
|
3335 if (joint->node[1].body) { // jds |
|
3336 dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis[anum]); |
|
3337 } |
|
3338 else { |
|
3339 result[0] = joint->axis[anum][0]; result[1] = joint->axis[anum][1]; |
|
3340 result[2] = joint->axis[anum][2]; result[3] = joint->axis[anum][3]; |
|
3341 } |
|
3342 } |
|
3343 } |
|
3344 else { |
|
3345 result[0] = joint->axis[anum][0]; |
|
3346 result[1] = joint->axis[anum][1]; |
|
3347 result[2] = joint->axis[anum][2]; |
|
3348 } |
|
3349 } |
|
3350 |
|
3351 |
|
3352 EXPORT_C int dJointGetAMotorAxisRel (dJointID j, int anum) |
|
3353 { |
|
3354 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3355 |
|
3356 if (anum < 0) anum = 0; |
|
3357 if (anum > 2) anum = 2; |
|
3358 return joint->rel[anum]; |
|
3359 } |
|
3360 |
|
3361 |
|
3362 EXPORT_C dReal dJointGetAMotorAngle (dJointID j, int anum) |
|
3363 { |
|
3364 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3365 |
|
3366 if (anum < 0) anum = 0; |
|
3367 if (anum > 3) anum = 3; |
|
3368 return joint->angle[anum]; |
|
3369 } |
|
3370 |
|
3371 |
|
3372 EXPORT_C dReal dJointGetAMotorAngleRate (dJointID /*j*/, int /*anum*/) |
|
3373 { |
|
3374 // @@@ |
|
3375 return 0; |
|
3376 } |
|
3377 |
|
3378 |
|
3379 EXPORT_C dReal dJointGetAMotorParam (dJointID j, int parameter) |
|
3380 { |
|
3381 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3382 |
|
3383 int anum = parameter >> 8; |
|
3384 if (anum < 0) anum = 0; |
|
3385 if (anum > 2) anum = 2; |
|
3386 parameter &= 0xff; |
|
3387 return joint->limot[anum].get (parameter); |
|
3388 } |
|
3389 |
|
3390 |
|
3391 EXPORT_C int dJointGetAMotorMode (dJointID j) |
|
3392 { |
|
3393 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3394 |
|
3395 return joint->mode; |
|
3396 } |
|
3397 |
|
3398 |
|
3399 EXPORT_C void dJointAddAMotorTorques (dJointID j, dReal torque1, dReal torque2, dReal torque3) |
|
3400 { |
|
3401 dxJointAMotor* joint = (dxJointAMotor*)j; |
|
3402 dVector3 axes[3]; |
|
3403 |
|
3404 |
|
3405 if (joint->num == 0) |
|
3406 return; |
|
3407 |
|
3408 |
|
3409 amotorComputeGlobalAxes (joint,axes); |
|
3410 axes[0][0] = dMUL(axes[0][0],torque1); |
|
3411 axes[0][1] = dMUL(axes[0][1],torque1); |
|
3412 axes[0][2] = dMUL(axes[0][2],torque1); |
|
3413 if (joint->num >= 2) { |
|
3414 axes[0][0] += dMUL(axes[1][0],torque2); |
|
3415 axes[0][1] += dMUL(axes[1][1],torque2); |
|
3416 axes[0][2] += dMUL(axes[1][2],torque2); |
|
3417 if (joint->num >= 3) { |
|
3418 axes[0][0] += dMUL(axes[2][0],torque3); |
|
3419 axes[0][1] += dMUL(axes[2][1],torque3); |
|
3420 axes[0][2] += dMUL(axes[2][2],torque3); |
|
3421 } |
|
3422 } |
|
3423 |
|
3424 if (joint->node[0].body != 0) |
|
3425 dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); |
|
3426 if (joint->node[1].body != 0) |
|
3427 dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); |
|
3428 } |
|
3429 |
|
3430 |
|
3431 dxJoint::Vtable __damotor_vtable = { |
|
3432 sizeof(dxJointAMotor), |
|
3433 (dxJoint::init_fn*) amotorInit, |
|
3434 (dxJoint::getInfo1_fn*) amotorGetInfo1, |
|
3435 (dxJoint::getInfo2_fn*) amotorGetInfo2, |
|
3436 dJointTypeAMotor}; |
|
3437 |
|
3438 |
|
3439 |
|
3440 //**************************************************************************** |
|
3441 // lmotor joint |
|
3442 static void lmotorInit (dxJointLMotor *j) |
|
3443 { |
|
3444 int i; |
|
3445 j->num = 0; |
|
3446 for (i=0;i<3;i++) { |
|
3447 dSetZero(j->axis[i],4); |
|
3448 j->limot[i].init(j->world); |
|
3449 } |
|
3450 } |
|
3451 |
|
3452 static void lmotorComputeGlobalAxes (dxJointLMotor *joint, dVector3 ax[3]) |
|
3453 { |
|
3454 for (int i=0; i< joint->num; i++) { |
|
3455 if (joint->rel[i] == 1) { |
|
3456 dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); |
|
3457 } |
|
3458 else if (joint->rel[i] == 2) { |
|
3459 if (joint->node[1].body) { // jds: don't assert, just ignore |
|
3460 dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); |
|
3461 } |
|
3462 } else { |
|
3463 ax[i][0] = joint->axis[i][0]; |
|
3464 ax[i][1] = joint->axis[i][1]; |
|
3465 ax[i][2] = joint->axis[i][2]; |
|
3466 } |
|
3467 } |
|
3468 } |
|
3469 |
|
3470 static void lmotorGetInfo1 (dxJointLMotor *j, dxJoint::Info1 *info) |
|
3471 { |
|
3472 info->m = 0; |
|
3473 info->nub = 0; |
|
3474 for (int i=0; i < j->num; i++) { |
|
3475 if (j->limot[i].fmax > 0) { |
|
3476 info->m++; |
|
3477 } |
|
3478 } |
|
3479 } |
|
3480 |
|
3481 static void lmotorGetInfo2 (dxJointLMotor *joint, dxJoint::Info2 *info) |
|
3482 { |
|
3483 int row=0; |
|
3484 dVector3 ax[3]; |
|
3485 lmotorComputeGlobalAxes(joint, ax); |
|
3486 |
|
3487 for (int i=0;i<joint->num;i++) { |
|
3488 row += joint->limot[i].addLimot(joint,info,row,ax[i], 0); |
|
3489 } |
|
3490 } |
|
3491 |
|
3492 EXPORT_C void dJointSetLMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) |
|
3493 { |
|
3494 dxJointLMotor* joint = (dxJointLMotor*)j; |
|
3495 //for now we are ignoring rel! |
|
3496 |
|
3497 if (anum < 0) anum = 0; |
|
3498 if (anum > 2) anum = 2; |
|
3499 |
|
3500 if (!joint->node[1].body && rel==2) rel = 1; //ref 1 |
|
3501 |
|
3502 joint->rel[anum] = rel; |
|
3503 |
|
3504 dVector3 r; |
|
3505 r[0] = x; |
|
3506 r[1] = y; |
|
3507 r[2] = z; |
|
3508 r[3] = 0; |
|
3509 if (rel > 0) { |
|
3510 if (rel==1) { |
|
3511 dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); |
|
3512 } else { |
|
3513 //second body has to exists thanks to ref 1 line |
|
3514 dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); |
|
3515 } |
|
3516 } else { |
|
3517 joint->axis[anum][0] = r[0]; |
|
3518 joint->axis[anum][1] = r[1]; |
|
3519 joint->axis[anum][2] = r[2]; |
|
3520 } |
|
3521 |
|
3522 dNormalize3 (joint->axis[anum]); |
|
3523 } |
|
3524 |
|
3525 EXPORT_C void dJointSetLMotorNumAxes (dJointID j, int num) |
|
3526 { |
|
3527 dxJointLMotor* joint = (dxJointLMotor*)j; |
|
3528 |
|
3529 if (num < 0) num = 0; |
|
3530 if (num > 3) num = 3; |
|
3531 joint->num = num; |
|
3532 } |
|
3533 |
|
3534 EXPORT_C void dJointSetLMotorParam (dJointID j, int parameter, dReal value) |
|
3535 { |
|
3536 dxJointLMotor* joint = (dxJointLMotor*)j; |
|
3537 |
|
3538 int anum = parameter >> 8; |
|
3539 if (anum < 0) anum = 0; |
|
3540 if (anum > 2) anum = 2; |
|
3541 parameter &= 0xff; |
|
3542 joint->limot[anum].set (parameter, value); |
|
3543 } |
|
3544 |
|
3545 EXPORT_C int dJointGetLMotorNumAxes (dJointID j) |
|
3546 { |
|
3547 dxJointLMotor* joint = (dxJointLMotor*)j; |
|
3548 |
|
3549 return joint->num; |
|
3550 } |
|
3551 |
|
3552 |
|
3553 EXPORT_C void dJointGetLMotorAxis (dJointID j, int anum, dVector3 result) |
|
3554 { |
|
3555 dxJointLMotor* joint = (dxJointLMotor*)j; |
|
3556 |
|
3557 if (anum < 0) anum = 0; |
|
3558 if (anum > 2) anum = 2; |
|
3559 result[0] = joint->axis[anum][0]; |
|
3560 result[1] = joint->axis[anum][1]; |
|
3561 result[2] = joint->axis[anum][2]; |
|
3562 } |
|
3563 |
|
3564 EXPORT_C dReal dJointGetLMotorParam (dJointID j, int parameter) |
|
3565 { |
|
3566 dxJointLMotor* joint = (dxJointLMotor*)j; |
|
3567 |
|
3568 int anum = parameter >> 8; |
|
3569 if (anum < 0) anum = 0; |
|
3570 if (anum > 2) anum = 2; |
|
3571 parameter &= 0xff; |
|
3572 return joint->limot[anum].get (parameter); |
|
3573 } |
|
3574 |
|
3575 dxJoint::Vtable __dlmotor_vtable = { |
|
3576 sizeof(dxJointLMotor), |
|
3577 (dxJoint::init_fn*) lmotorInit, |
|
3578 (dxJoint::getInfo1_fn*) lmotorGetInfo1, |
|
3579 (dxJoint::getInfo2_fn*) lmotorGetInfo2, |
|
3580 dJointTypeLMotor |
|
3581 }; |
|
3582 |
|
3583 |
|
3584 //**************************************************************************** |
|
3585 // fixed joint |
|
3586 |
|
3587 static void fixedInit (dxJointFixed *j) |
|
3588 { |
|
3589 dSetZero (j->offset,4); |
|
3590 dSetZero (j->qrel,4); |
|
3591 } |
|
3592 |
|
3593 |
|
3594 static void fixedGetInfo1 (dxJointFixed */*j*/, dxJoint::Info1 *info) |
|
3595 { |
|
3596 info->m = 6; |
|
3597 info->nub = 6; |
|
3598 } |
|
3599 |
|
3600 |
|
3601 static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) |
|
3602 { |
|
3603 int s = info->rowskip; |
|
3604 |
|
3605 // Three rows for orientation |
|
3606 setFixedOrientation(joint, info, joint->qrel, 3); |
|
3607 |
|
3608 // Three rows for position. |
|
3609 // set jacobian |
|
3610 info->J1l[0] = REAL(1.0); |
|
3611 info->J1l[s+1] = REAL(1.0); |
|
3612 info->J1l[2*s+2] = REAL(1.0); |
|
3613 |
|
3614 dVector3 ofs; |
|
3615 dMULTIPLY0_331 (ofs,joint->node[0].body->posr.R,joint->offset); |
|
3616 if (joint->node[1].body) { |
|
3617 dCROSSMAT (info->J1a,ofs,s,+,-); |
|
3618 info->J2l[0] = REAL(-1.0); |
|
3619 info->J2l[s+1] = REAL(-1.0); |
|
3620 info->J2l[2*s+2] = REAL(-1.0); |
|
3621 } |
|
3622 |
|
3623 // set right hand side for the first three rows (linear) |
|
3624 dReal k = dMUL(info->fps,info->erp); |
|
3625 if (joint->node[1].body) { |
|
3626 for (int j=0; j<3; j++) |
|
3627 info->c[j] = dMUL(k,(joint->node[1].body->posr.pos[j] - |
|
3628 joint->node[0].body->posr.pos[j] + ofs[j])); |
|
3629 } |
|
3630 else { |
|
3631 for (int j=0; j<3; j++) |
|
3632 info->c[j] = dMUL(k,(joint->offset[j] - joint->node[0].body->posr.pos[j])); |
|
3633 } |
|
3634 } |
|
3635 |
|
3636 |
|
3637 EXPORT_C void dJointSetFixed (dJointID j) |
|
3638 { |
|
3639 dxJointFixed* joint = (dxJointFixed*)j; |
|
3640 |
|
3641 int i; |
|
3642 |
|
3643 // This code is taken from sJointSetSliderAxis(), we should really put the |
|
3644 // common code in its own function. |
|
3645 // compute the offset between the bodies |
|
3646 if (joint->node[0].body) { |
|
3647 if (joint->node[1].body) { |
|
3648 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
|
3649 dReal ofs[4]; |
|
3650 for (i=0; i<4; i++) ofs[i] = joint->node[0].body->posr.pos[i]; |
|
3651 for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->posr.pos[i]; |
|
3652 dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,ofs); |
|
3653 } |
|
3654 else { |
|
3655 // set joint->qrel to the transpose of the first body's q |
|
3656 joint->qrel[0] = joint->node[0].body->q[0]; |
|
3657 for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
|
3658 for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; |
|
3659 } |
|
3660 } |
|
3661 } |
|
3662 |
|
3663 |
|
3664 dxJoint::Vtable __dfixed_vtable = { |
|
3665 sizeof(dxJointFixed), |
|
3666 (dxJoint::init_fn*) fixedInit, |
|
3667 (dxJoint::getInfo1_fn*) fixedGetInfo1, |
|
3668 (dxJoint::getInfo2_fn*) fixedGetInfo2, |
|
3669 dJointTypeFixed}; |
|
3670 |
|
3671 //**************************************************************************** |
|
3672 // null joint |
|
3673 |
|
3674 static void nullGetInfo1 (dxJointNull */*j*/, dxJoint::Info1 *info) |
|
3675 { |
|
3676 info->m = 0; |
|
3677 info->nub = 0; |
|
3678 } |
|
3679 |
|
3680 |
|
3681 static void nullGetInfo2 (dxJointNull */*joint*/, dxJoint::Info2 */*info*/) |
|
3682 { |
|
3683 |
|
3684 } |
|
3685 |
|
3686 |
|
3687 dxJoint::Vtable __dnull_vtable = { |
|
3688 sizeof(dxJointNull), |
|
3689 (dxJoint::init_fn*) 0, |
|
3690 (dxJoint::getInfo1_fn*) nullGetInfo1, |
|
3691 (dxJoint::getInfo2_fn*) nullGetInfo2, |
|
3692 dJointTypeNull}; |
|
3693 |
|
3694 |
|
3695 |
|
3696 |
|
3697 /* |
|
3698 This code is part of the Plane2D ODE joint |
|
3699 by psero@gmx.de |
|
3700 Wed Apr 23 18:53:43 CEST 2003 |
|
3701 |
|
3702 Add this code to the file: ode/src/joint.cpp |
|
3703 */ |
|
3704 |
|
3705 |
|
3706 # define VoXYZ(v1, o1, x, y, z) \ |
|
3707 ( \ |
|
3708 (v1)[0] o1 (x), \ |
|
3709 (v1)[1] o1 (y), \ |
|
3710 (v1)[2] o1 (z) \ |
|
3711 ) |
|
3712 |
|
3713 static const dReal Midentity[3][3] = |
|
3714 { |
|
3715 { REAL(1.0), 0, 0 }, |
|
3716 { 0, REAL(1.0), 0 }, |
|
3717 { 0, 0, REAL(1.0), } |
|
3718 }; |
|
3719 |
|
3720 |
|
3721 |
|
3722 static void plane2dInit (dxJointPlane2D *j) |
|
3723 /*********************************************/ |
|
3724 { |
|
3725 /* MINFO ("plane2dInit ()"); */ |
|
3726 j->motor_x.init (j->world); |
|
3727 j->motor_y.init (j->world); |
|
3728 j->motor_angle.init (j->world); |
|
3729 } |
|
3730 |
|
3731 |
|
3732 |
|
3733 static void plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info) |
|
3734 /***********************************************************************/ |
|
3735 { |
|
3736 /* MINFO ("plane2dGetInfo1 ()"); */ |
|
3737 |
|
3738 info->nub = 3; |
|
3739 info->m = 3; |
|
3740 |
|
3741 if (j->motor_x.fmax > 0) |
|
3742 j->row_motor_x = info->m ++; |
|
3743 if (j->motor_y.fmax > 0) |
|
3744 j->row_motor_y = info->m ++; |
|
3745 if (j->motor_angle.fmax > 0) |
|
3746 j->row_motor_angle = info->m ++; |
|
3747 } |
|
3748 |
|
3749 |
|
3750 |
|
3751 static void plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info) |
|
3752 /***************************************************************************/ |
|
3753 { |
|
3754 int r0 = 0, |
|
3755 r1 = info->rowskip, |
|
3756 r2 = 2 * r1; |
|
3757 dReal eps = dMUL(info->fps,info->erp); |
|
3758 |
|
3759 /* MINFO ("plane2dGetInfo2 ()"); */ |
|
3760 |
|
3761 /* |
|
3762 v = v1, w = omega1 |
|
3763 (v2, omega2 not important (== static environment)) |
|
3764 |
|
3765 constraint equations: |
|
3766 xz = 0 |
|
3767 wx = 0 |
|
3768 wy = 0 |
|
3769 |
|
3770 <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 ) |
|
3771 ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 ) |
|
3772 ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 ) |
|
3773 J1/J1l Omega1/J1a |
|
3774 */ |
|
3775 |
|
3776 // fill in linear and angular coeff. for left hand side: |
|
3777 |
|
3778 VoXYZ (&info->J1l[r0], =, 0, 0, REAL(1.0)); |
|
3779 VoXYZ (&info->J1l[r1], =, 0, 0, 0); |
|
3780 VoXYZ (&info->J1l[r2], =, 0, 0, 0); |
|
3781 |
|
3782 VoXYZ (&info->J1a[r0], =, 0, 0, 0); |
|
3783 VoXYZ (&info->J1a[r1], =, REAL(1.0), 0, 0); |
|
3784 VoXYZ (&info->J1a[r2], =, 0, REAL(1.0), 0); |
|
3785 |
|
3786 // error correction (against drift): |
|
3787 |
|
3788 // a) linear vz, so that z (== pos[2]) == 0 |
|
3789 info->c[0] = dMUL(eps,-joint->node[0].body->posr.pos[2]); |
|
3790 |
|
3791 |
|
3792 // if the slider is powered, or has joint limits, add in the extra row: |
|
3793 |
|
3794 if (joint->row_motor_x > 0) |
|
3795 joint->motor_x.addLimot ( |
|
3796 joint, info, joint->row_motor_x, Midentity[0], 0); |
|
3797 |
|
3798 if (joint->row_motor_y > 0) |
|
3799 joint->motor_y.addLimot ( |
|
3800 joint, info, joint->row_motor_y, Midentity[1], 0); |
|
3801 |
|
3802 if (joint->row_motor_angle > 0) |
|
3803 joint->motor_angle.addLimot ( |
|
3804 joint, info, joint->row_motor_angle, Midentity[2], 1); |
|
3805 } |
|
3806 |
|
3807 |
|
3808 |
|
3809 dxJoint::Vtable __dplane2d_vtable = |
|
3810 { |
|
3811 sizeof (dxJointPlane2D), |
|
3812 (dxJoint::init_fn*) plane2dInit, |
|
3813 (dxJoint::getInfo1_fn*) plane2dGetInfo1, |
|
3814 (dxJoint::getInfo2_fn*) plane2dGetInfo2, |
|
3815 dJointTypePlane2D |
|
3816 }; |
|
3817 |
|
3818 |
|
3819 EXPORT_C void dJointSetPlane2DXParam (dxJoint *joint, |
|
3820 int parameter, dReal value) |
|
3821 { |
|
3822 |
|
3823 dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); |
|
3824 joint2d->motor_x.set (parameter, value); |
|
3825 } |
|
3826 |
|
3827 |
|
3828 EXPORT_C void dJointSetPlane2DYParam (dxJoint *joint, |
|
3829 int parameter, dReal value) |
|
3830 { |
|
3831 |
|
3832 dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); |
|
3833 joint2d->motor_y.set (parameter, value); |
|
3834 } |
|
3835 |
|
3836 |
|
3837 |
|
3838 EXPORT_C void dJointSetPlane2DAngleParam (dxJoint *joint, |
|
3839 int parameter, dReal value) |
|
3840 { |
|
3841 |
|
3842 dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); |
|
3843 joint2d->motor_angle.set (parameter, value); |
|
3844 } |
|
3845 |
|
3846 |
|
3847 |