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