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 <ode/odeconfig.h>
25 #include "config.h"
26 #include "piston.h"
27 #include "joint_internal.h"
28
29
30
31 //****************************************************************************
32 // Piston
33 //
34
dxJointPiston(dxWorld * w)35 dxJointPiston::dxJointPiston ( dxWorld *w ) :
36 dxJoint ( w )
37 {
38 dSetZero ( axis1, 4 );
39 dSetZero ( axis2, 4 );
40
41 axis1[0] = 1;
42 axis2[0] = 1;
43
44 dSetZero ( qrel, 4 );
45
46 dSetZero ( anchor1, 4 );
47 dSetZero ( anchor2, 4 );
48
49 limotP.init ( world );
50
51 limotR.init ( world );
52 }
53
54
dJointGetPistonPosition(dJointID j)55 dReal dJointGetPistonPosition ( dJointID j )
56 {
57 dxJointPiston* joint = ( dxJointPiston* ) j;
58 dUASSERT ( joint, "bad joint argument" );
59 checktype ( joint, Piston );
60
61 if ( joint->node[0].body )
62 {
63 dVector3 q;
64 // get the anchor (or offset) in global coordinates
65 dMultiply0_331 ( q, joint->node[0].body->posr.R, joint->anchor1 );
66
67 if ( joint->node[1].body )
68 {
69 dVector3 anchor2;
70 // get the anchor2 in global coordinates
71 dMultiply0_331 ( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
72
73 q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
74 ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
75 q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
76 ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
77 q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
78 ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
79 }
80 else
81 {
82 // N.B. When there is no body 2 the joint->anchor2 is already in
83 // global coordinates
84 q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
85 ( joint->anchor2[0] ) );
86 q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
87 ( joint->anchor2[1] ) );
88 q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
89 ( joint->anchor2[2] ) );
90
91 if ( joint->flags & dJOINT_REVERSE )
92 {
93 q[0] = -q[0];
94 q[1] = -q[1];
95 q[2] = -q[2];
96 }
97 }
98
99 // get axis in global coordinates
100 dVector3 ax;
101 dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
102
103 return dCalcVectorDot3 ( ax, q );
104 }
105
106 dDEBUGMSG ( "The function always return 0 since no body are attached" );
107 return 0;
108 }
109
110
dJointGetPistonPositionRate(dJointID j)111 dReal dJointGetPistonPositionRate ( dJointID j )
112 {
113 dxJointPiston* joint = ( dxJointPiston* ) j;
114 dUASSERT ( joint, "bad joint argument" );
115 checktype ( joint, Piston );
116
117 // get axis in global coordinates
118 dVector3 ax;
119 dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
120
121 // The linear velocity created by the rotation can be discarded since
122 // the rotation is along the prismatic axis and this rotation don't create
123 // linear velocity in the direction of the prismatic axis.
124 if ( joint->node[1].body )
125 {
126 return ( dCalcVectorDot3 ( ax, joint->node[0].body->lvel ) -
127 dCalcVectorDot3 ( ax, joint->node[1].body->lvel ) );
128 }
129 else
130 {
131 dReal rate = dCalcVectorDot3 ( ax, joint->node[0].body->lvel );
132 return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
133 }
134 }
135
136
dJointGetPistonAngle(dJointID j)137 dReal dJointGetPistonAngle ( dJointID j )
138 {
139 dxJointPiston* joint = ( dxJointPiston * ) j;
140 dAASSERT ( joint );
141 checktype ( joint, Piston );
142
143 if ( joint->node[0].body )
144 {
145 dReal ang = getHingeAngle ( joint->node[0].body, joint->node[1].body, joint->axis1,
146 joint->qrel );
147 if ( joint->flags & dJOINT_REVERSE )
148 return -ang;
149 else
150 return ang;
151 }
152 else return 0;
153 }
154
155
dJointGetPistonAngleRate(dJointID j)156 dReal dJointGetPistonAngleRate ( dJointID j )
157 {
158 dxJointPiston* joint = ( dxJointPiston* ) j;
159 dAASSERT ( joint );
160 checktype ( joint, Piston );
161
162 if ( joint->node[0].body )
163 {
164 dVector3 axis;
165 dMultiply0_331 ( axis, joint->node[0].body->posr.R, joint->axis1 );
166 dReal rate = dCalcVectorDot3 ( axis, joint->node[0].body->avel );
167 if ( joint->node[1].body ) rate -= dCalcVectorDot3 ( axis, joint->node[1].body->avel );
168 if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
169 return rate;
170 }
171 else return 0;
172 }
173
174
175 void
getSureMaxInfo(SureMaxInfo * info)176 dxJointPiston::getSureMaxInfo( SureMaxInfo* info )
177 {
178 info->max_m = 6;
179 }
180
181
182 void
getInfo1(dxJoint::Info1 * info)183 dxJointPiston::getInfo1 ( dxJoint::Info1 *info )
184 {
185 info->nub = 4; // Number of unbound variables
186 // The only bound variable is one linear displacement
187
188 info->m = 4; // Default number of constraint row
189
190 // see if we're at a joint limit.
191 limotP.limit = 0;
192 if ( ( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
193 limotP.lostop <= limotP.histop )
194 {
195 // measure joint position
196 dReal pos = dJointGetPistonPosition ( this );
197 limotP.testRotationalLimit ( pos ); // N.B. The fucntion is ill named
198 }
199
200 // powered Piston or at limits needs an extra constraint row
201 if ( limotP.limit || limotP.fmax > 0 ) info->m++;
202
203
204 // see if we're at a joint limit.
205 limotR.limit = 0;
206 if ( ( limotR.lostop > -dInfinity || limotR.histop < dInfinity ) &&
207 limotR.lostop <= limotR.histop )
208 {
209 // measure joint position
210 dReal angle = getHingeAngle ( node[0].body, node[1].body, axis1,
211 qrel );
212 limotR.testRotationalLimit ( angle );
213 }
214
215 // powered Piston or at limits needs an extra constraint row
216 if ( limotR.limit || limotR.fmax > 0 ) info->m++;
217
218 }
219
220
221 void
getInfo2(dReal worldFPS,dReal worldERP,const Info2Descr * info)222 dxJointPiston::getInfo2 ( dReal worldFPS, dReal worldERP, const Info2Descr *info )
223 {
224 const int s0 = 0;
225 const int s1 = info->rowskip;
226 const int s2 = 2 * s1, s3 = 3 * s1 /*, s4=4*s1*/;
227
228 const dReal k = worldFPS * worldERP;
229
230
231 // Pull out pos and R for both bodies. also get the `connection'
232 // vector pos2-pos1.
233
234 dReal *pos1, *pos2, *R1, *R2=0;
235 dVector3 dist; // Current position of body_1 w.r.t "anchor"
236 // 2 bodies anchor is center of body 2
237 // 1 bodies anchor is origin
238 dVector3 lanchor2=
239 {
240 0,0,0
241 };
242
243 pos1 = node[0].body->posr.pos;
244 R1 = node[0].body->posr.R;
245
246 if ( node[1].body )
247 {
248 pos2 = node[1].body->posr.pos;
249 R2 = node[1].body->posr.R;
250
251 dMultiply0_331 ( lanchor2, R2, anchor2 );
252 dist[0] = lanchor2[0] + pos2[0] - pos1[0];
253 dist[1] = lanchor2[1] + pos2[1] - pos1[1];
254 dist[2] = lanchor2[2] + pos2[2] - pos1[2];
255 }
256 else
257 {
258 // pos2 = 0; // N.B. We can do that to be safe but it is no necessary
259 // R2 = 0; // N.B. We can do that to be safe but it is no necessary
260 if (flags & dJOINT_REVERSE )
261 {
262 dist[0] = pos1[0] - anchor2[0]; // Invert the value
263 dist[1] = pos1[1] - anchor2[1];
264 dist[2] = pos1[2] - anchor2[2];
265 }
266 else
267 {
268 dist[0] = anchor2[0] - pos1[0];
269 dist[1] = anchor2[1] - pos1[1];
270 dist[2] = anchor2[2] - pos1[2];
271 }
272 }
273
274 // ======================================================================
275 // Work on the angular part (i.e. row 0, 1)
276 // Set the two orientation rows. The rotoide axis should be the only
277 // unconstrained rotational axis, the angular velocity of the two bodies
278 // perpendicular to the rotoide axis should be equal.
279 // Thus the constraint equations are:
280 // p*w1 - p*w2 = 0
281 // q*w1 - q*w2 = 0
282 // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
283 // are the angular velocity vectors of the two bodies.
284 // Since the rotoide axis is the same as the prismatic axis.
285 //
286 //
287 // Also, compute the right hand side (RHS) of the rotation constraint equation set.
288 // The first 2 element will result in the relative angular velocity of the two
289 // bodies along axis p and q. This is set to bring the rotoide back into alignment.
290 // if `theta' is the angle between ax1 and ax2, we need an angular velocity
291 // along u to cover angle erp*theta in one step :
292 // |angular_velocity| = angle/time = erp*theta / stepsize
293 // = (erp*fps) * theta
294 // angular_velocity = |angular_velocity| * u
295 // = (erp*fps) * theta * u
296 // where rotation along unit length axis u by theta brings body 2's frame
297 //
298 // if theta is smallish, sin(theta) ~= theta and cos(theta) ~= 1
299 // where the quaternion of the relative rotation between the two bodies is
300 // quat = [cos(theta/2) sin(theta/2)*u]
301 // quat = [1 theta/2*u]
302 // => q[0] ~= 1
303 // 2 * q[1+i] = theta * u[i]
304 //
305 // Since there is no constraint along the rotoide axis
306 // only along p and q that we want the same angular velocity and need to reduce
307 // the error
308 dVector3 ax1, p, q;
309 dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 );
310
311 // Find the 2 axis perpendicular to the rotoide axis.
312 dPlaneSpace ( ax1, p, q );
313
314 // LHS
315 dCopyVector3 ( ( info->J1a ) + s0, p );
316 dCopyVector3 ( ( info->J1a ) + s1, q );
317
318 dVector3 b;
319 if ( node[1].body )
320 {
321 // LHS
322 // info->J2a[s0+i] = -p[i]
323 dCopyNegatedVector3 ( ( info->J2a ) + s0, p );
324 dCopyNegatedVector3 ( ( info->J2a ) + s1, q );
325
326
327 // Some math for the RHS
328 dVector3 ax2;
329 dMultiply0_331 ( ax2, R2, axis2 );
330 dCalcVectorCross3( b, ax1, ax2 );
331 }
332 else
333 {
334 // Some math for the RHS
335 dCalcVectorCross3( b, ax1, axis2 );
336 }
337
338 // RHS
339 info->c[0] = k * dCalcVectorDot3 ( p, b );
340 info->c[1] = k * dCalcVectorDot3 ( q, b );
341
342
343 // ======================================================================
344 // Work on the linear part (i.e row 2,3)
345 // p2 + R2 anchor2' = p1 + R1 dist'
346 // v2 + w2 R2 anchor2' + R2 d(anchor2')/dt = v1 + w1 R1 dist' + R1 d(dist')/dt
347 // v2 + w2 x anchor2 = v1 + w1 x dist + v_p
348 // v_p is speed of prismatic joint (i.e. elongation rate)
349 // Since the constraints are perpendicular to v_p we have:
350 // p . v_p = 0 and q . v_p = 0
351 // Along p and q we have (since sliding along the prismatic axis is disregarded):
352 // u . ( v2 + w2 x anchor2 = v1 + w1 x dist + v_p) ( where u is p or q )
353 // Simplify
354 // u . v2 + u. w2 x anchor2 = u . v1 + u . w1 x dist
355 // or
356 // u . v1 - u . v2 + u . w1 x dist - u2 . w2 x anchor2 = 0
357 // using the fact that (a x b = - b x a)
358 // u . v1 - u . v2 - u . dist x w1 + u . anchor2 x w2 = 0
359 // With the help of the triple product:
360 // i.e. a . b x c = b . c x a = c . a x b or a . b x c = a x b . c
361 // Ref: http://mathworld.wolfram.com/ScalarTripleProduct.html
362 // u . v1 - u . v2 - u x dist . w1 + u x anchor2 . w2 = 0
363 // u . v1 - u . v2 + dist x u . w1 - u x anchor2 . w2 = 0
364 //
365 // Coeff for 1er line of: J1l => p, J2l => -p
366 // Coeff for 2er line of: J1l => q, J2l => -q
367 // Coeff for 1er line of: J1a => dist x p, J2a => p x anchor2
368 // Coeff for 2er line of: J1a => dist x q, J2a => q x anchor2
369
370 dCalcVectorCross3( ( info->J1a ) + s2, dist, p );
371
372 dCalcVectorCross3( ( info->J1a ) + s3, dist, q );
373
374 dCopyVector3 ( ( info->J1l ) + s2, p );
375 dCopyVector3 ( ( info->J1l ) + s3, q );
376
377 if ( node[1].body )
378 {
379 // q x anchor2 instead of anchor2 x q since we want the negative value
380 dCalcVectorCross3( ( info->J2a ) + s2, p, lanchor2 );
381
382 // The cross product is in reverse order since we want the negative value
383 dCalcVectorCross3( ( info->J2a ) + s3, q, lanchor2 );
384
385 // info->J2l[s2+i] = -p[i];
386 dCopyNegatedVector3 ( ( info->J2l ) + s2, p );
387 dCopyNegatedVector3 ( ( info->J2l ) + s3, q );
388 }
389
390
391 // We want to make correction for motion not in the line of the axis
392 // We calculate the displacement w.r.t. the "anchor" pt.
393 // i.e. Find the difference between the current position and the initial
394 // position along the constrained axies (i.e. axis p and q).
395 // The bodies can move w.r.t each other only along the prismatic axis
396 //
397 // Compute the RHS of rows 2 and 3
398 dVector3 err;
399 dMultiply0_331 ( err, R1, anchor1 );
400 dSubtractVectors3( err, dist, err );
401
402 info->c[2] = k * dCalcVectorDot3 ( p, err );
403 info->c[3] = k * dCalcVectorDot3 ( q, err );
404
405
406 int row = 4;
407 if ( node[1].body )
408 {
409 row += limotP.addLimot ( this, worldFPS, info, 4, ax1, 0 );
410 }
411 else if (flags & dJOINT_REVERSE )
412 {
413 dVector3 rAx1;
414 rAx1[0] = -ax1[0];
415 rAx1[1] = -ax1[1];
416 rAx1[2] = -ax1[2];
417 row += limotP.addLimot ( this, worldFPS, info, 4, rAx1, 0 );
418 }
419 else
420 row += limotP.addLimot ( this, worldFPS, info, 4, ax1, 0 );
421
422 limotR.addLimot ( this, worldFPS, info, row, ax1, 1 );
423 }
424
dJointSetPistonAnchor(dJointID j,dReal x,dReal y,dReal z)425 void dJointSetPistonAnchor ( dJointID j, dReal x, dReal y, dReal z )
426 {
427 dxJointPiston* joint = ( dxJointPiston* ) j;
428 dUASSERT ( joint, "bad joint argument" );
429 checktype ( joint, Piston );
430 setAnchors ( joint, x, y, z, joint->anchor1, joint->anchor2 );
431 joint->computeInitialRelativeRotation();
432
433 }
434
dJointSetPistonAnchorOffset(dJointID j,dReal x,dReal y,dReal z,dReal dx,dReal dy,dReal dz)435 void dJointSetPistonAnchorOffset (dJointID j, dReal x, dReal y, dReal z,
436 dReal dx, dReal dy, dReal dz)
437 {
438 dxJointPiston* joint = (dxJointPiston*) j;
439 dUASSERT (joint,"bad joint argument");
440 checktype ( joint, Piston );
441
442 if (joint->flags & dJOINT_REVERSE)
443 {
444 dx = -dx;
445 dy = -dy;
446 dz = -dz;
447 }
448
449 if (joint->node[0].body)
450 {
451 joint->node[0].body->posr.pos[0] -= dx;
452 joint->node[0].body->posr.pos[1] -= dy;
453 joint->node[0].body->posr.pos[2] -= dz;
454 }
455
456 setAnchors (joint,x ,y, z, joint->anchor1, joint->anchor2);
457
458 if (joint->node[0].body)
459 {
460 joint->node[0].body->posr.pos[0] += dx;
461 joint->node[0].body->posr.pos[1] += dy;
462 joint->node[0].body->posr.pos[2] += dz;
463 }
464
465 joint->computeInitialRelativeRotation();
466 }
467
468
469
dJointGetPistonAnchor(dJointID j,dVector3 result)470 void dJointGetPistonAnchor ( dJointID j, dVector3 result )
471 {
472 dxJointPiston* joint = ( dxJointPiston* ) j;
473 dUASSERT ( joint, "bad joint argument" );
474 dUASSERT ( result, "bad result argument" );
475 checktype ( joint, Piston );
476 if ( joint->flags & dJOINT_REVERSE )
477 getAnchor2 ( joint, result, joint->anchor2 );
478 else
479 getAnchor ( joint, result, joint->anchor1 );
480 }
481
482
dJointGetPistonAnchor2(dJointID j,dVector3 result)483 void dJointGetPistonAnchor2 ( dJointID j, dVector3 result )
484 {
485 dxJointPiston* joint = ( dxJointPiston* ) j;
486 dUASSERT ( joint, "bad joint argument" );
487 dUASSERT ( result, "bad result argument" );
488 checktype ( joint, Piston );
489 if ( joint->flags & dJOINT_REVERSE )
490 getAnchor ( joint, result, joint->anchor1 );
491 else
492 getAnchor2 ( joint, result, joint->anchor2 );
493 }
494
495
496
dJointSetPistonAxis(dJointID j,dReal x,dReal y,dReal z)497 void dJointSetPistonAxis ( dJointID j, dReal x, dReal y, dReal z )
498 {
499 dxJointPiston* joint = ( dxJointPiston* ) j;
500 dUASSERT ( joint, "bad joint argument" );
501 checktype ( joint, Piston );
502
503 setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
504
505 joint->computeInitialRelativeRotation();
506 }
507
508
dJointSetPistonAxisDelta(dJointID j,dReal x,dReal y,dReal z,dReal dx,dReal dy,dReal dz)509 void dJointSetPistonAxisDelta ( dJointID j, dReal x, dReal y, dReal z,
510 dReal dx, dReal dy, dReal dz )
511 {
512 dxJointPiston* joint = ( dxJointPiston* ) j;
513 dUASSERT ( joint, "bad joint argument" );
514 checktype ( joint, Piston );
515
516 setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
517
518 joint->computeInitialRelativeRotation();
519
520 dVector3 c = {0,0,0};
521 if ( joint->node[1].body )
522 {
523 c[0] = ( joint->node[0].body->posr.pos[0] -
524 joint->node[1].body->posr.pos[0] - dx );
525 c[1] = ( joint->node[0].body->posr.pos[1] -
526 joint->node[1].body->posr.pos[1] - dy );
527 c[2] = ( joint->node[0].body->posr.pos[2] -
528 joint->node[1].body->posr.pos[2] - dz );
529 }
530 else if ( joint->node[0].body )
531 {
532 c[0] = joint->node[0].body->posr.pos[0] - dx;
533 c[1] = joint->node[0].body->posr.pos[1] - dy;
534 c[2] = joint->node[0].body->posr.pos[2] - dz;
535 }
536
537 // Convert into frame of body 1
538 dMultiply1_331 ( joint->anchor1, joint->node[0].body->posr.R, c );
539 }
540
541
542
dJointGetPistonAxis(dJointID j,dVector3 result)543 void dJointGetPistonAxis ( dJointID j, dVector3 result )
544 {
545 dxJointPiston* joint = ( dxJointPiston* ) j;
546 dUASSERT ( joint, "bad joint argument" );
547 dUASSERT ( result, "bad result argument" );
548 checktype ( joint, Piston );
549
550 getAxis ( joint, result, joint->axis1 );
551 }
552
dJointSetPistonParam(dJointID j,int parameter,dReal value)553 void dJointSetPistonParam ( dJointID j, int parameter, dReal value )
554 {
555 dxJointPiston* joint = ( dxJointPiston* ) j;
556 dUASSERT ( joint, "bad joint argument" );
557 checktype ( joint, Piston );
558
559 if ( ( parameter & 0xff00 ) == 0x100 )
560 {
561 joint->limotR.set ( parameter & 0xff, value );
562 }
563 else
564 {
565 joint->limotP.set ( parameter, value );
566 }
567 }
568
569
dJointGetPistonParam(dJointID j,int parameter)570 dReal dJointGetPistonParam ( dJointID j, int parameter )
571 {
572 dxJointPiston* joint = ( dxJointPiston* ) j;
573 dUASSERT ( joint, "bad joint argument" );
574 checktype ( joint, Piston );
575
576 if ( ( parameter & 0xff00 ) == 0x100 )
577 {
578 return joint->limotR.get ( parameter & 0xff );
579 }
580 else
581 {
582 return joint->limotP.get ( parameter );
583 }
584 }
585
586
dJointAddPistonForce(dJointID j,dReal force)587 void dJointAddPistonForce ( dJointID j, dReal force )
588 {
589 dxJointPiston* joint = ( dxJointPiston* ) j;
590 dUASSERT ( joint, "bad joint argument" );
591 checktype ( joint, Piston );
592
593 if ( joint->flags & dJOINT_REVERSE )
594 force -= force;
595
596 dVector3 axis;
597 getAxis ( joint, axis, joint->axis1 );
598 // axis[i] *= force
599 dScaleVector3( axis, force );
600
601
602 if ( joint->node[0].body != 0 )
603 dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
604 if ( joint->node[1].body != 0 )
605 dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
606
607 if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
608 {
609 // Case where we don't need ltd since center of mass of both bodies
610 // pass by the anchor point '*' when travelling along the prismatic axis.
611 // Body_2
612 // Body_1 -----
613 // --- |-- | |
614 // | |---------------*-------------| | ---> prismatic axis
615 // --- |-- | |
616 // -----
617 // Body_2
618 // Case where we need ltd
619 // Body_1
620 // ---
621 // | |---------
622 // --- |
623 // | |--
624 // -----*----- ---> prismatic axis
625 // |-- |
626 // |
627 // |
628 // | -----
629 // | | |
630 // -------| |
631 // | |
632 // -----
633 // Body_2
634 //
635 // In real life force apply at the '*' point
636 // But in ODE the force are applied on the center of mass of Body_1 and Body_2
637 // So we have to add torques on both bodies to compensate for that when there
638 // is an offset between the anchor point and the center of mass of both bodies.
639 //
640 // We need to add to each body T = r x F
641 // Where r is the distance between the cm and '*'
642
643 dVector3 ltd; // Linear Torque Decoupling vector (a torque)
644 dVector3 c; // Distance of the body w.r.t the anchor
645 // N.B. The distance along the prismatic axis might not
646 // not be included in this variable since it won't add
647 // anything to the ltd.
648
649 // Calculate the distance of the body w.r.t the anchor
650
651 // The anchor1 of body1 can be used since:
652 // Real anchor = Position of body 1 + anchor + d* axis1 = anchor in world frame
653 // d is the position of the prismatic joint (i.e. elongation)
654 // Since axis1 x axis1 == 0
655 // We can do the following.
656 dMultiply0_331 ( c, joint->node[0].body->posr.R, joint->anchor1 );
657 dCalcVectorCross3( ltd, c, axis );
658 dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
659
660
661 dMultiply0_331 ( c, joint->node[1].body->posr.R, joint->anchor2 );
662 dCalcVectorCross3( ltd, c, axis );
663 dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
664 }
665 }
666
667
668 dJointType
type() const669 dxJointPiston::type() const
670 {
671 return dJointTypePiston;
672 }
673
674
675 size_t
size() const676 dxJointPiston::size() const
677 {
678 return sizeof ( *this );
679 }
680
681
682
683 void
setRelativeValues()684 dxJointPiston::setRelativeValues()
685 {
686 dVector3 vec;
687 dJointGetPistonAnchor(this, vec);
688 setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );
689
690 dJointGetPistonAxis(this, vec);
691 setAxes( this, vec[0], vec[1], vec[2], axis1, axis2 );
692
693 computeInitialRelativeRotation();
694 }
695
696
697
698
699 void
computeInitialRelativeRotation()700 dxJointPiston::computeInitialRelativeRotation()
701 {
702 if ( node[0].body )
703 {
704 if ( node[1].body )
705 {
706 dQMultiply1 ( qrel, node[0].body->q, node[1].body->q );
707 }
708 else
709 {
710 // set joint->qrel to the transpose of the first body q
711 qrel[0] = node[0].body->q[0];
712 for ( int i = 1; i < 4; i++ )
713 qrel[i] = -node[0].body->q[i];
714 // WARNING do we need the - in -joint->node[0].body->q[i]; or not
715 }
716 }
717 }
718