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