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 #include "pr.h"
25 #include "joint_internal.h"
26
27
28
29 //****************************************************************************
30 // Prismatic and Rotoide
31
dxJointPR(dxWorld * w)32 dxJointPR::dxJointPR( dxWorld *w ) :
33 dxJoint( w )
34 {
35 // Default Position
36 // Z^
37 // | Body 1 P R Body2
38 // |+---------+ _ _ +-----------+
39 // || |----|----(_)--------+ |
40 // |+---------+ - +-----------+
41 // |
42 // X.-----------------------------------------> Y
43 // N.B. X is comming out of the page
44 dSetZero( anchor2, 4 );
45
46 dSetZero( axisR1, 4 );
47 axisR1[0] = 1;
48 dSetZero( axisR2, 4 );
49 axisR2[0] = 1;
50
51 dSetZero( axisP1, 4 );
52 axisP1[1] = 1;
53 dSetZero( qrel, 4 );
54 dSetZero( offset, 4 );
55
56 limotR.init( world );
57 limotP.init( world );
58 }
59
60
dJointGetPRPosition(dJointID j)61 dReal dJointGetPRPosition( dJointID j )
62 {
63 dxJointPR* joint = ( dxJointPR* ) j;
64 dUASSERT( joint, "bad joint argument" );
65 checktype( joint, PR );
66
67 dVector3 q;
68 // get the offset in global coordinates
69 dMULTIPLY0_331( q, joint->node[0].body->posr.R, joint->offset );
70
71 if ( joint->node[1].body )
72 {
73 dVector3 anchor2;
74
75 // get the anchor2 in global coordinates
76 dMULTIPLY0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
77
78 q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
79 ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
80 q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
81 ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
82 q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
83 ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
84
85 }
86 else
87 {
88 //N.B. When there is no body 2 the joint->anchor2 is already in
89 // global coordinates
90
91 q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
92 ( joint->anchor2[0] ) );
93 q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
94 ( joint->anchor2[1] ) );
95 q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
96 ( joint->anchor2[2] ) );
97
98 if ( joint->flags & dJOINT_REVERSE )
99 {
100 q[0] = -q[0];
101 q[1] = -q[1];
102 q[2] = -q[2];
103 }
104 }
105
106 dVector3 axP;
107 // get prismatic axis in global coordinates
108 dMULTIPLY0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
109
110 return dDOT( axP, q );
111 }
112
dJointGetPRPositionRate(dJointID j)113 dReal dJointGetPRPositionRate( dJointID j )
114 {
115 dxJointPR* joint = ( dxJointPR* ) j;
116 dUASSERT( joint, "bad joint argument" );
117 checktype( joint, PR );
118 // get axis1 in global coordinates
119 dVector3 ax1;
120 dMULTIPLY0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );
121
122 if ( joint->node[1].body )
123 {
124 dVector3 lv2;
125 dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
126 return dDOT( ax1, joint->node[0].body->lvel ) - dDOT( ax1, lv2 );
127 }
128 else
129 {
130 dReal rate = dDOT( ax1, joint->node[0].body->lvel );
131 return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
132 }
133 }
134
135
136
dJointGetPRAngle(dJointID j)137 dReal dJointGetPRAngle( dJointID j )
138 {
139 dxJointPR* joint = ( dxJointPR* )j;
140 dAASSERT( joint );
141 checktype( joint, PR );
142 if ( joint->node[0].body )
143 {
144 dReal ang = getHingeAngle( joint->node[0].body,
145 joint->node[1].body,
146 joint->axisR1,
147 joint->qrel );
148 if ( joint->flags & dJOINT_REVERSE )
149 return -ang;
150 else
151 return ang;
152 }
153 else return 0;
154 }
155
156
157
dJointGetPRAngleRate(dJointID j)158 dReal dJointGetPRAngleRate( dJointID j )
159 {
160 dxJointPR* joint = ( dxJointPR* )j;
161 dAASSERT( joint );
162 checktype( joint, PR );
163 if ( joint->node[0].body )
164 {
165 dVector3 axis;
166 dMULTIPLY0_331( axis, joint->node[0].body->posr.R, joint->axisR1 );
167 dReal rate = dDOT( axis, joint->node[0].body->avel );
168 if ( joint->node[1].body ) rate -= dDOT( axis, joint->node[1].body->avel );
169 if ( joint->flags & dJOINT_REVERSE ) rate = -rate;
170 return rate;
171 }
172 else return 0;
173 }
174
175
176
177
178 void
getInfo1(dxJoint::Info1 * info)179 dxJointPR::getInfo1( dxJoint::Info1 *info )
180 {
181 info->nub = 4;
182 info->m = 4;
183
184
185 // see if we're at a joint limit.
186 limotP.limit = 0;
187 if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
188 limotP.lostop <= limotP.histop )
189 {
190 // measure joint position
191 dReal pos = dJointGetPRPosition( this );
192 limotP.testRotationalLimit( pos ); // N.B. The function is ill named
193 }
194
195 // powered needs an extra constraint row
196 if ( limotP.limit || limotP.fmax > 0 ) info->m++;
197
198
199 // see if we're at a joint limit.
200 limotR.limit = 0;
201 if (( limotR.lostop >= -M_PI || limotR.histop <= M_PI ) &&
202 limotR.lostop <= limotR.histop )
203 {
204 dReal angle = getHingeAngle( node[0].body,
205 node[1].body,
206 axisR1, qrel );
207 limotR.testRotationalLimit( angle );
208 }
209
210 // powered morit or at limits needs an extra constraint row
211 if ( limotR.limit || limotR.fmax > 0 ) info->m++;
212
213 }
214
215
216
217 void
getInfo2(dxJoint::Info2 * info)218 dxJointPR::getInfo2( dxJoint::Info2 *info )
219 {
220 int s = info->rowskip;
221 int s2 = 2 * s;
222 int s3 = 3 * s;
223 //int s4= 4*s;
224
225 dReal k = info->fps * info->erp;
226
227
228 dVector3 q; // plane space of axP and after that axR
229
230 // pull out pos and R for both bodies. also get the `connection'
231 // vector pos2-pos1.
232
233 dReal *pos1, *pos2 = 0, *R1, *R2 = 0;
234 pos1 = node[0].body->posr.pos;
235 R1 = node[0].body->posr.R;
236 if ( node[1].body )
237 {
238 pos2 = node[1].body->posr.pos;
239 R2 = node[1].body->posr.R;
240 }
241 else
242 {
243 // pos2 = 0; // N.B. We can do that to be safe but it is no necessary
244 // R2 = 0; // N.B. We can do that to be safe but it is no necessary
245 }
246
247
248 dVector3 axP; // Axis of the prismatic joint in global frame
249 dMULTIPLY0_331( axP, R1, axisP1 );
250
251 // distance between the body1 and the anchor2 in global frame
252 // Calculated in the same way as the offset
253 dVector3 wanchor2 = {0,0,0}, dist;
254
255 if ( node[1].body )
256 {
257 // Calculate anchor2 in world coordinate
258 dMULTIPLY0_331( wanchor2, R2, anchor2 );
259 dist[0] = wanchor2[0] + pos2[0] - pos1[0];
260 dist[1] = wanchor2[1] + pos2[1] - pos1[1];
261 dist[2] = wanchor2[2] + pos2[2] - pos1[2];
262 }
263 else
264 {
265 if (flags & dJOINT_REVERSE )
266 {
267 dist[0] = pos1[0] - anchor2[0]; // Invert the value
268 dist[1] = pos1[1] - anchor2[1];
269 dist[2] = pos1[2] - anchor2[2];
270 }
271 else
272 {
273 dist[0] = anchor2[0] - pos1[0];
274 dist[1] = anchor2[1] - pos1[1];
275 dist[2] = anchor2[2] - pos1[2];
276 }
277 }
278
279
280 // ======================================================================
281 // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered
282
283 // Set the two rotoide rows. The rotoide axis should be the only unconstrained
284 // rotational axis, the angular velocity of the two bodies perpendicular to
285 // the rotoide axis should be equal. Thus the constraint equations are
286 // p*w1 - p*w2 = 0
287 // q*w1 - q*w2 = 0
288 // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
289 // are the angular velocity vectors of the two bodies.
290 dVector3 ax1;
291 dMULTIPLY0_331( ax1, node[0].body->posr.R, axisR1 );
292 dCROSS( q , = , ax1, axP );
293
294 info->J1a[0] = axP[0];
295 info->J1a[1] = axP[1];
296 info->J1a[2] = axP[2];
297 info->J1a[s+0] = q[0];
298 info->J1a[s+1] = q[1];
299 info->J1a[s+2] = q[2];
300
301 if ( node[1].body )
302 {
303 info->J2a[0] = -axP[0];
304 info->J2a[1] = -axP[1];
305 info->J2a[2] = -axP[2];
306 info->J2a[s+0] = -q[0];
307 info->J2a[s+1] = -q[1];
308 info->J2a[s+2] = -q[2];
309 }
310
311
312 // Compute the right hand side of the constraint equation set. Relative
313 // body velocities along p and q to bring the rotoide back into alignment.
314 // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
315 // We need to rotate both bodies along the axis u = (ax1 x ax2).
316 // if `theta' is the angle between ax1 and ax2, we need an angular velocity
317 // along u to cover angle erp*theta in one step :
318 // |angular_velocity| = angle/time = erp*theta / stepsize
319 // = (erp*fps) * theta
320 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
321 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
322 // ...as ax1 and ax2 are unit length. if theta is smallish,
323 // theta ~= sin(theta), so
324 // angular_velocity = (erp*fps) * (ax1 x ax2)
325 // ax1 x ax2 is in the plane space of ax1, so we project the angular
326 // velocity to p and q to find the right hand side.
327
328 dVector3 ax2;
329 if ( node[1].body )
330 {
331 dMULTIPLY0_331( ax2, R2, axisR2 );
332 }
333 else
334 {
335 ax2[0] = axisR2[0];
336 ax2[1] = axisR2[1];
337 ax2[2] = axisR2[2];
338 }
339
340 dVector3 b;
341 dCROSS( b, = , ax1, ax2 );
342 info->c[0] = k * dDOT( b, axP );
343 info->c[1] = k * dDOT( b, q );
344
345
346
347 // ==========================
348 // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
349 // or 5 if rotoide and prismatic powered
350
351 // two rows. we want: vel2 = vel1 + w1 x c ... but this would
352 // result in three equations, so we project along the planespace vectors
353 // so that sliding along the prismatic axis is disregarded. for symmetry we
354 // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
355
356 // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
357 // 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
358 // v_p is speed of prismatic joint (i.e. elongation rate)
359 // Since the constraints are perpendicular to v_p we have:
360 // p dot v_p = 0 and q dot v_p = 0
361 // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
362 // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
363 // ==
364 // 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
365 // since a . (b x c) = - b . (a x c) = - (a x c) . b
366 // and a x b = - b x a
367 // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
368 // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
369 // Coeff for 1er line of: J1l => ax1, J2l => -ax1
370 // Coeff for 2er line of: J1l => q, J2l => -q
371 // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
372 // Coeff for 2er line of: J1a => dist x q, J2a => - anchor2 x q
373
374
375 dCROSS(( info->J1a ) + s2, = , dist, ax1 );
376
377 dCROSS(( info->J1a ) + s3, = , dist, q );
378
379
380 info->J1l[s2+0] = ax1[0];
381 info->J1l[s2+1] = ax1[1];
382 info->J1l[s2+2] = ax1[2];
383
384 info->J1l[s3+0] = q[0];
385 info->J1l[s3+1] = q[1];
386 info->J1l[s3+2] = q[2];
387
388 if ( node[1].body )
389 {
390 // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
391 dCROSS(( info->J2a ) + s2, = , ax2, wanchor2 ); // since ax1 == ax2
392
393 // The cross product is in reverse order since we want the negative value
394 dCROSS(( info->J2a ) + s3, = , q, wanchor2 );
395
396 info->J2l[s2+0] = -ax1[0];
397 info->J2l[s2+1] = -ax1[1];
398 info->J2l[s2+2] = -ax1[2];
399
400 info->J2l[s3+0] = -q[0];
401 info->J2l[s3+1] = -q[1];
402 info->J2l[s3+2] = -q[2];
403 }
404
405
406 // We want to make correction for motion not in the line of the axisP
407 // We calculate the displacement w.r.t. the anchor pt.
408 //
409 // compute the elements 2 and 3 of right hand side.
410 // we want to align the offset point (in body 2's frame) with the center of body 1.
411 // The position should be the same when we are not along the prismatic axis
412 dVector3 err;
413 dMULTIPLY0_331( err, R1, offset );
414 err[0] = dist[0] - err[0];
415 err[1] = dist[1] - err[1];
416 err[2] = dist[2] - err[2];
417 info->c[2] = k * dDOT( ax1, err );
418 info->c[3] = k * dDOT( q, err );
419
420 int row = 4;
421 if ( node[1].body || !(flags & dJOINT_REVERSE) )
422 {
423 row += limotP.addLimot ( this, info, 4, axP, 0 );
424 }
425 else
426 {
427 dVector3 rAxP;
428 rAxP[0] = -axP[0];
429 rAxP[1] = -axP[1];
430 rAxP[2] = -axP[2];
431 row += limotP.addLimot ( this, info, 4, rAxP, 0 );
432 }
433
434 limotR.addLimot ( this, info, row, ax1, 1 );
435 }
436
437
438 // compute initial relative rotation body1 -> body2, or env -> body1
439 void
computeInitialRelativeRotation()440 dxJointPR::computeInitialRelativeRotation()
441 {
442 if ( node[0].body )
443 {
444 if ( node[1].body )
445 {
446 dQMultiply1( qrel, node[0].body->q, node[1].body->q );
447 }
448 else
449 {
450 // set joint->qrel to the transpose of the first body q
451 qrel[0] = node[0].body->q[0];
452 for ( int i = 1; i < 4; i++ )
453 qrel[i] = -node[0].body->q[i];
454 // WARNING do we need the - in -joint->node[0].body->q[i]; or not
455 }
456 }
457 }
458
dJointSetPRAnchor(dJointID j,dReal x,dReal y,dReal z)459 void dJointSetPRAnchor( dJointID j, dReal x, dReal y, dReal z )
460 {
461 dxJointPR* joint = ( dxJointPR* ) j;
462 dUASSERT( joint, "bad joint argument" );
463 checktype( joint, PR );
464 setAnchors( joint, x, y, z, joint->offset, joint->anchor2 );
465 }
466
467
dJointSetPRAxis1(dJointID j,dReal x,dReal y,dReal z)468 void dJointSetPRAxis1( dJointID j, dReal x, dReal y, dReal z )
469 {
470 dxJointPR* joint = ( dxJointPR* ) j;
471 dUASSERT( joint, "bad joint argument" );
472 checktype( joint, PR );
473
474 setAxes( joint, x, y, z, joint->axisP1, 0 );
475
476 joint->computeInitialRelativeRotation();
477 }
478
479
dJointSetPRAxis2(dJointID j,dReal x,dReal y,dReal z)480 void dJointSetPRAxis2( dJointID j, dReal x, dReal y, dReal z )
481 {
482 dxJointPR* joint = ( dxJointPR* ) j;
483 dUASSERT( joint, "bad joint argument" );
484 checktype( joint, PR );
485 setAxes( joint, x, y, z, joint->axisR1, joint->axisR2 );
486 joint->computeInitialRelativeRotation();
487 }
488
489
dJointSetPRParam(dJointID j,int parameter,dReal value)490 void dJointSetPRParam( dJointID j, int parameter, dReal value )
491 {
492 dxJointPR* joint = ( dxJointPR* ) j;
493 dUASSERT( joint, "bad joint argument" );
494 checktype( joint, PR );
495 if (( parameter & 0xff00 ) == 0x100 )
496 {
497 joint->limotR.set( parameter & 0xff, value ); // Take only lower part of the
498 } // parameter alue
499 else
500 {
501 joint->limotP.set( parameter, value );
502 }
503 }
504
dJointGetPRAnchor(dJointID j,dVector3 result)505 void dJointGetPRAnchor( dJointID j, dVector3 result )
506 {
507 dxJointPR* joint = ( dxJointPR* ) j;
508 dUASSERT( joint, "bad joint argument" );
509 dUASSERT( result, "bad result argument" );
510 checktype( joint, PR );
511
512 if ( joint->node[1].body )
513 getAnchor2( joint, result, joint->anchor2 );
514 else
515 {
516 result[0] = joint->anchor2[0];
517 result[1] = joint->anchor2[1];
518 result[2] = joint->anchor2[2];
519 }
520 }
521
dJointGetPRAxis1(dJointID j,dVector3 result)522 void dJointGetPRAxis1( dJointID j, dVector3 result )
523 {
524 dxJointPR* joint = ( dxJointPR* ) j;
525 dUASSERT( joint, "bad joint argument" );
526 dUASSERT( result, "bad result argument" );
527 checktype( joint, PR );
528 getAxis( joint, result, joint->axisP1 );
529 }
530
dJointGetPRAxis2(dJointID j,dVector3 result)531 void dJointGetPRAxis2( dJointID j, dVector3 result )
532 {
533 dxJointPR* joint = ( dxJointPR* ) j;
534 dUASSERT( joint, "bad joint argument" );
535 dUASSERT( result, "bad result argument" );
536 checktype( joint, PR );
537 getAxis( joint, result, joint->axisR1 );
538 }
539
dJointGetPRParam(dJointID j,int parameter)540 dReal dJointGetPRParam( dJointID j, int parameter )
541 {
542 dxJointPR* joint = ( dxJointPR* ) j;
543 dUASSERT( joint, "bad joint argument" );
544 checktype( joint, PR );
545 if (( parameter & 0xff00 ) == 0x100 )
546 {
547 return joint->limotR.get( parameter & 0xff );
548 }
549 else
550 {
551 return joint->limotP.get( parameter );
552 }
553 }
554
dJointAddPRTorque(dJointID j,dReal torque)555 void dJointAddPRTorque( dJointID j, dReal torque )
556 {
557 dxJointPR* joint = ( dxJointPR* ) j;
558 dVector3 axis;
559 dAASSERT( joint );
560 checktype( joint, PR );
561
562 if ( joint->flags & dJOINT_REVERSE )
563 torque = -torque;
564
565 getAxis( joint, axis, joint->axisR1 );
566 axis[0] *= torque;
567 axis[1] *= torque;
568 axis[2] *= torque;
569
570 if ( joint->node[0].body != 0 )
571 dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] );
572 if ( joint->node[1].body != 0 )
573 dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
574 }
575
576
577 dJointType
type() const578 dxJointPR::type() const
579 {
580 return dJointTypePR;
581 }
582
583 size_t
size() const584 dxJointPR::size() const
585 {
586 return sizeof( *this );
587 }
588
589
590 void
setRelativeValues()591 dxJointPR::setRelativeValues()
592 {
593 dVector3 anchor;
594 dJointGetPRAnchor(this, anchor);
595 setAnchors( this, anchor[0], anchor[1], anchor[2], offset, anchor2 );
596
597 dVector3 axis;
598 dJointGetPRAxis1(this, axis);
599 setAxes( this, axis[0], axis[1], axis[2], axisP1, 0 );
600
601 dJointGetPRAxis2(this, axis);
602 setAxes( this, axis[0], axis[1], axis[2], axisR1, axisR2 );
603
604 computeInitialRelativeRotation();
605 }
606
607
608
609
610
611