diff options
| author | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
|---|---|---|
| committer | sanine <sanine.not@pm.me> | 2022-10-01 20:59:36 -0500 |
| commit | c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch) | |
| tree | 277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/ode/src/joints/piston.cpp | |
| parent | 1cf9cc3408af7008451f9133fb95af66a9697d15 (diff) | |
add ode
Diffstat (limited to 'libs/ode-0.16.1/ode/src/joints/piston.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/joints/piston.cpp | 729 |
1 files changed, 729 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/joints/piston.cpp b/libs/ode-0.16.1/ode/src/joints/piston.cpp new file mode 100644 index 0000000..3bd7fd0 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/piston.cpp @@ -0,0 +1,729 @@ +/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "piston.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Piston
+//
+
+dxJointPiston::dxJointPiston ( dxWorld *w ) :
+ dxJoint ( w )
+{
+ dSetZero ( axis1, 4 );
+ dSetZero ( axis2, 4 );
+
+ axis1[0] = 1;
+ axis2[0] = 1;
+
+ dSetZero ( qrel, 4 );
+
+ dSetZero ( anchor1, 4 );
+ dSetZero ( anchor2, 4 );
+
+ limotP.init ( world );
+
+ limotR.init ( world );
+}
+
+
+dReal dJointGetPistonPosition ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( joint->node[0].body )
+ {
+ dVector3 q;
+ // get the anchor (or offset) in global coordinates
+ dMultiply0_331 ( q, joint->node[0].body->posr.R, joint->anchor1 );
+
+ if ( joint->node[1].body )
+ {
+ dVector3 anchor2;
+ // get the anchor2 in global coordinates
+ dMultiply0_331 ( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
+
+ q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
+ ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
+ q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
+ ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
+ q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
+ ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
+ }
+ else
+ {
+ // N.B. When there is no body 2 the joint->anchor2 is already in
+ // global coordinates
+ q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
+ ( joint->anchor2[0] ) );
+ q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
+ ( joint->anchor2[1] ) );
+ q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
+ ( joint->anchor2[2] ) );
+
+ if ( joint->flags & dJOINT_REVERSE )
+ {
+ q[0] = -q[0];
+ q[1] = -q[1];
+ q[2] = -q[2];
+ }
+ }
+
+ // get axis in global coordinates
+ dVector3 ax;
+ dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
+
+ return dCalcVectorDot3 ( ax, q );
+ }
+
+ dDEBUGMSG ( "The function always return 0 since no body are attached" );
+ return 0;
+}
+
+
+dReal dJointGetPistonPositionRate ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ // get axis in global coordinates
+ dVector3 ax;
+ dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
+
+ // The linear velocity created by the rotation can be discarded since
+ // the rotation is along the prismatic axis and this rotation don't create
+ // linear velocity in the direction of the prismatic axis.
+ if ( joint->node[1].body )
+ {
+ return ( dCalcVectorDot3 ( ax, joint->node[0].body->lvel ) -
+ dCalcVectorDot3 ( ax, joint->node[1].body->lvel ) );
+ }
+ else
+ {
+ dReal rate = dCalcVectorDot3 ( ax, joint->node[0].body->lvel );
+ return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
+ }
+}
+
+
+dReal dJointGetPistonAngle ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston * ) j;
+ dAASSERT ( joint );
+ checktype ( joint, Piston );
+
+ if ( joint->node[0].body )
+ {
+ dReal ang = getHingeAngle ( joint->node[0].body, joint->node[1].body, joint->axis1,
+ joint->qrel );
+ if ( joint->flags & dJOINT_REVERSE )
+ return -ang;
+ else
+ return ang;
+ }
+ else return 0;
+}
+
+
+dReal dJointGetPistonAngleRate ( dJointID j )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dAASSERT ( joint );
+ checktype ( joint, Piston );
+
+ if ( joint->node[0].body )
+ {
+ dVector3 axis;
+ dMultiply0_331 ( axis, joint->node[0].body->posr.R, joint->axis1 );
+ dReal rate = dCalcVectorDot3 ( axis, joint->node[0].body->avel );
+ if ( joint->node[1].body ) rate -= dCalcVectorDot3 ( axis, joint->node[1].body->avel );
+ if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
+ return rate;
+ }
+ else return 0;
+}
+
+
+void
+dxJointPiston::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = 6;
+}
+
+
+void
+dxJointPiston::getInfo1 ( dxJoint::Info1 *info )
+{
+ info->nub = 4; // Number of unbound variables
+ // The only bound variable is one linear displacement
+
+ info->m = 4; // Default number of constraint row
+
+ // see if we're at a joint limit.
+ limotP.limit = 0;
+ if ( ( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
+ limotP.lostop <= limotP.histop )
+ {
+ // measure joint position
+ dReal pos = dJointGetPistonPosition ( this );
+ limotP.testRotationalLimit ( pos ); // N.B. The fucntion is ill named
+ }
+
+ // powered Piston or at limits needs an extra constraint row
+ if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+
+
+ // see if we're at a joint limit.
+ limotR.limit = 0;
+ if ( ( limotR.lostop > -dInfinity || limotR.histop < dInfinity ) &&
+ limotR.lostop <= limotR.histop )
+ {
+ // measure joint position
+ dReal angle = getHingeAngle ( node[0].body, node[1].body, axis1,
+ qrel );
+ limotR.testRotationalLimit ( angle );
+ }
+
+ // powered Piston or at limits needs an extra constraint row
+ if ( limotR.limit || limotR.fmax > 0 ) info->m++;
+
+}
+
+
+void
+dxJointPiston::getInfo2 ( dReal worldFPS, dReal worldERP,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ const dReal k = worldFPS * worldERP;
+
+
+ // Pull out pos and R for both bodies. also get the `connection'
+ // vector pos2-pos1.
+
+ dVector3 dist; // Current position of body_1 w.r.t "anchor"
+ // 2 bodies anchor is center of body 2
+ // 1 bodies anchor is origin
+ dVector3 lanchor2 = { 0,0,0 };
+
+ dReal *pos1 = node[0].body->posr.pos;
+ dReal *R1 = node[0].body->posr.R;
+ dReal *R2 = NULL;
+
+ dxBody *body1 = node[1].body;
+
+ if ( body1 )
+ {
+ dReal *pos2 = body1->posr.pos;
+ R2 = body1->posr.R;
+
+ dMultiply0_331 ( lanchor2, R2, anchor2 );
+ dist[0] = lanchor2[0] + pos2[0] - pos1[0];
+ dist[1] = lanchor2[1] + pos2[1] - pos1[1];
+ dist[2] = lanchor2[2] + pos2[2] - pos1[2];
+ }
+ else
+ {
+ // pos2 = 0; // N.B. We can do that to be safe but it is no necessary
+ // R2 = 0; // N.B. We can do that to be safe but it is no necessary
+ if ( (flags & dJOINT_REVERSE) != 0 )
+ {
+ dSubtractVectors3(dist, pos1, anchor2); // Invert the value
+ }
+ else
+ {
+ dSubtractVectors3(dist, anchor2, pos1);
+ }
+ }
+
+ // ======================================================================
+ // Work on the angular part (i.e. row 0, 1)
+ // Set the two orientation rows. The rotoide axis should be the only
+ // unconstrained rotational axis, the angular velocity of the two bodies
+ // perpendicular to the rotoide axis should be equal.
+ // Thus the constraint equations are:
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ // Since the rotoide axis is the same as the prismatic axis.
+ //
+ //
+ // Also, compute the right hand side (RHS) of the rotation constraint equation set.
+ // The first 2 element will result in the relative angular velocity of the two
+ // bodies along axis p and q. This is set to bring the rotoide back into alignment.
+ // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * u
+ // = (erp*fps) * theta * u
+ // where rotation along unit length axis u by theta brings body 2's frame
+ //
+ // if theta is smallish, sin(theta) ~= theta and cos(theta) ~= 1
+ // where the quaternion of the relative rotation between the two bodies is
+ // quat = [cos(theta/2) sin(theta/2)*u]
+ // quat = [1 theta/2*u]
+ // => q[0] ~= 1
+ // 2 * q[1+i] = theta * u[i]
+ //
+ // Since there is no constraint along the rotoide axis
+ // only along p and q that we want the same angular velocity and need to reduce
+ // the error
+ dVector3 b, ax1, p, q;
+ dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 );
+
+ // Find the 2 axis perpendicular to the rotoide axis.
+ dPlaneSpace ( ax1, p, q );
+
+ // LHS
+ dCopyVector3 ( J1 + GI2__JA_MIN, p );
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3 ( J2 + GI2__JA_MIN, p );
+ }
+
+ dCopyVector3 ( J1 + rowskip + GI2__JA_MIN, q );
+
+ if ( body1 )
+ {
+ dCopyNegatedVector3 ( J2 + rowskip + GI2__JA_MIN, q );
+
+ // Some math for the RHS
+ dVector3 ax2;
+ dMultiply0_331 ( ax2, R2, axis2 );
+ dCalcVectorCross3( b, ax1, ax2 );
+ }
+ else
+ {
+ // Some math for the RHS
+ dCalcVectorCross3( b, ax1, axis2 );
+ }
+
+ // RHS
+ pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3 ( p, b );
+ pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3 ( q, b );
+
+
+ // ======================================================================
+ // Work on the linear part (i.e row 2,3)
+ // p2 + R2 anchor2' = p1 + R1 dist'
+ // v2 + w2 R2 anchor2' + R2 d(anchor2')/dt = v1 + w1 R1 dist' + R1 d(dist')/dt
+ // v2 + w2 x anchor2 = v1 + w1 x dist + v_p
+ // v_p is speed of prismatic joint (i.e. elongation rate)
+ // Since the constraints are perpendicular to v_p we have:
+ // p . v_p = 0 and q . v_p = 0
+ // Along p and q we have (since sliding along the prismatic axis is disregarded):
+ // u . ( v2 + w2 x anchor2 = v1 + w1 x dist + v_p) ( where u is p or q )
+ // Simplify
+ // u . v2 + u. w2 x anchor2 = u . v1 + u . w1 x dist
+ // or
+ // u . v1 - u . v2 + u . w1 x dist - u2 . w2 x anchor2 = 0
+ // using the fact that (a x b = - b x a)
+ // u . v1 - u . v2 - u . dist x w1 + u . anchor2 x w2 = 0
+ // With the help of the triple product:
+ // i.e. a . b x c = b . c x a = c . a x b or a . b x c = a x b . c
+ // Ref: http://mathworld.wolfram.com/ScalarTripleProduct.html
+ // u . v1 - u . v2 - u x dist . w1 + u x anchor2 . w2 = 0
+ // u . v1 - u . v2 + dist x u . w1 - u x anchor2 . w2 = 0
+ //
+ // Coeff for 1er line of: J1l => p, J2l => -p
+ // Coeff for 2er line of: J1l => q, J2l => -q
+ // Coeff for 1er line of: J1a => dist x p, J2a => p x anchor2
+ // Coeff for 2er line of: J1a => dist x q, J2a => q x anchor2
+
+ int currRowSkip = 2 * rowskip;
+ {
+ dCopyVector3 ( J1 + currRowSkip + GI2__JL_MIN, p );
+ dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, p );
+
+ if ( body1 )
+ {
+ // info->J2l[s2+i] = -p[i];
+ dCopyNegatedVector3 ( J2 + currRowSkip + GI2__JL_MIN, p );
+ // q x anchor2 instead of anchor2 x q since we want the negative value
+ dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, p, lanchor2 );
+ }
+ }
+
+ currRowSkip += rowskip;
+ {
+ dCopyVector3 ( J1 + currRowSkip + GI2__JL_MIN, q );
+ dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, q );
+
+ if ( body1 )
+ {
+ // info->J2l[s3+i] = -q[i];
+ dCopyNegatedVector3 ( J2 + currRowSkip + GI2__JL_MIN, q );
+ // The cross product is in reverse order since we want the negative value
+ dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, lanchor2 );
+ }
+ }
+
+ // We want to make correction for motion not in the line of the axis
+ // We calculate the displacement w.r.t. the "anchor" pt.
+ // i.e. Find the difference between the current position and the initial
+ // position along the constrained axies (i.e. axis p and q).
+ // The bodies can move w.r.t each other only along the prismatic axis
+ //
+ // Compute the RHS of rows 2 and 3
+ dVector3 err;
+ dMultiply0_331 ( err, R1, anchor1 );
+ dSubtractVectors3( err, dist, err );
+
+ int currPairSkip = 2 * pairskip;
+ {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, err );
+ }
+
+ currPairSkip += pairskip;
+ {
+ pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, err );
+ }
+
+ currRowSkip += rowskip; currPairSkip += pairskip;
+
+ if ( body1 || (flags & dJOINT_REVERSE) == 0 )
+ {
+ if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 0 ))
+ {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+ else
+ {
+ dVector3 rAx1;
+ dCopyNegatedVector3(rAx1, ax1);
+
+ if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAx1, 0 ))
+ {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+
+ limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
+}
+
+void dJointSetPistonAnchor ( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+ setAnchors ( joint, x, y, z, joint->anchor1, joint->anchor2 );
+ joint->computeInitialRelativeRotation();
+
+}
+
+void dJointSetPistonAnchorOffset (dJointID j, dReal x, dReal y, dReal z,
+ dReal dx, dReal dy, dReal dz)
+{
+ dxJointPiston* joint = (dxJointPiston*) j;
+ dUASSERT (joint,"bad joint argument");
+ checktype ( joint, Piston );
+
+ if (joint->flags & dJOINT_REVERSE)
+ {
+ dx = -dx;
+ dy = -dy;
+ dz = -dz;
+ }
+
+ if (joint->node[0].body)
+ {
+ joint->node[0].body->posr.pos[0] -= dx;
+ joint->node[0].body->posr.pos[1] -= dy;
+ joint->node[0].body->posr.pos[2] -= dz;
+ }
+
+ setAnchors (joint,x ,y, z, joint->anchor1, joint->anchor2);
+
+ if (joint->node[0].body)
+ {
+ joint->node[0].body->posr.pos[0] += dx;
+ joint->node[0].body->posr.pos[1] += dy;
+ joint->node[0].body->posr.pos[2] += dz;
+ }
+
+ joint->computeInitialRelativeRotation();
+}
+
+
+
+void dJointGetPistonAnchor ( dJointID j, dVector3 result )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ dUASSERT ( result, "bad result argument" );
+ checktype ( joint, Piston );
+ if ( joint->flags & dJOINT_REVERSE )
+ getAnchor2 ( joint, result, joint->anchor2 );
+ else
+ getAnchor ( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetPistonAnchor2 ( dJointID j, dVector3 result )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ dUASSERT ( result, "bad result argument" );
+ checktype ( joint, Piston );
+ if ( joint->flags & dJOINT_REVERSE )
+ getAnchor ( joint, result, joint->anchor1 );
+ else
+ getAnchor2 ( joint, result, joint->anchor2 );
+}
+
+
+
+void dJointSetPistonAxis ( dJointID j, dReal x, dReal y, dReal z )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
+
+ joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPistonAxisDelta ( dJointID j, dReal x, dReal y, dReal z,
+ dReal dx, dReal dy, dReal dz )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
+
+ joint->computeInitialRelativeRotation();
+
+ dVector3 c = {0,0,0};
+ if ( joint->node[1].body )
+ {
+ c[0] = ( joint->node[0].body->posr.pos[0] -
+ joint->node[1].body->posr.pos[0] - dx );
+ c[1] = ( joint->node[0].body->posr.pos[1] -
+ joint->node[1].body->posr.pos[1] - dy );
+ c[2] = ( joint->node[0].body->posr.pos[2] -
+ joint->node[1].body->posr.pos[2] - dz );
+ }
+ else /*if ( joint->node[0].body )*/ // -- body[0] should always be present -- there is a matrix multiplication below
+ {
+ c[0] = joint->node[0].body->posr.pos[0] - dx;
+ c[1] = joint->node[0].body->posr.pos[1] - dy;
+ c[2] = joint->node[0].body->posr.pos[2] - dz;
+ }
+
+ // Convert into frame of body 1
+ dMultiply1_331 ( joint->anchor1, joint->node[0].body->posr.R, c );
+}
+
+
+
+void dJointGetPistonAxis ( dJointID j, dVector3 result )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ dUASSERT ( result, "bad result argument" );
+ checktype ( joint, Piston );
+
+ getAxis ( joint, result, joint->axis1 );
+}
+
+void dJointSetPistonParam ( dJointID j, int parameter, dReal value )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( ( parameter & 0xff00 ) == 0x100 )
+ {
+ joint->limotR.set ( parameter & 0xff, value );
+ }
+ else
+ {
+ joint->limotP.set ( parameter, value );
+ }
+}
+
+
+dReal dJointGetPistonParam ( dJointID j, int parameter )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( ( parameter & 0xff00 ) == 0x100 )
+ {
+ return joint->limotR.get ( parameter & 0xff );
+ }
+ else
+ {
+ return joint->limotP.get ( parameter );
+ }
+}
+
+
+void dJointAddPistonForce ( dJointID j, dReal force )
+{
+ dxJointPiston* joint = ( dxJointPiston* ) j;
+ dUASSERT ( joint, "bad joint argument" );
+ checktype ( joint, Piston );
+
+ if ( joint->flags & dJOINT_REVERSE )
+ force -= force;
+
+ dVector3 axis;
+ getAxis ( joint, axis, joint->axis1 );
+ // axis[i] *= force
+ dScaleVector3( axis, force );
+
+
+ if ( joint->node[0].body != 0 )
+ dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
+ if ( joint->node[1].body != 0 )
+ dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+
+ if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
+ {
+ // Case where we don't need ltd since center of mass of both bodies
+ // pass by the anchor point '*' when travelling along the prismatic axis.
+ // Body_2
+ // Body_1 -----
+ // --- |-- | |
+ // | |---------------*-------------| | ---> prismatic axis
+ // --- |-- | |
+ // -----
+ // Body_2
+ // Case where we need ltd
+ // Body_1
+ // ---
+ // | |---------
+ // --- |
+ // | |--
+ // -----*----- ---> prismatic axis
+ // |-- |
+ // |
+ // |
+ // | -----
+ // | | |
+ // -------| |
+ // | |
+ // -----
+ // Body_2
+ //
+ // In real life force apply at the '*' point
+ // But in ODE the force are applied on the center of mass of Body_1 and Body_2
+ // So we have to add torques on both bodies to compensate for that when there
+ // is an offset between the anchor point and the center of mass of both bodies.
+ //
+ // We need to add to each body T = r x F
+ // Where r is the distance between the cm and '*'
+
+ dVector3 ltd; // Linear Torque Decoupling vector (a torque)
+ dVector3 c; // Distance of the body w.r.t the anchor
+ // N.B. The distance along the prismatic axis might not
+ // not be included in this variable since it won't add
+ // anything to the ltd.
+
+ // Calculate the distance of the body w.r.t the anchor
+
+ // The anchor1 of body1 can be used since:
+ // Real anchor = Position of body 1 + anchor + d* axis1 = anchor in world frame
+ // d is the position of the prismatic joint (i.e. elongation)
+ // Since axis1 x axis1 == 0
+ // We can do the following.
+ dMultiply0_331 ( c, joint->node[0].body->posr.R, joint->anchor1 );
+ dCalcVectorCross3( ltd, c, axis );
+ dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
+
+
+ dMultiply0_331 ( c, joint->node[1].body->posr.R, joint->anchor2 );
+ dCalcVectorCross3( ltd, c, axis );
+ dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
+ }
+}
+
+
+dJointType
+dxJointPiston::type() const
+{
+ return dJointTypePiston;
+}
+
+
+sizeint
+dxJointPiston::size() const
+{
+ return sizeof ( *this );
+}
+
+
+
+void
+dxJointPiston::setRelativeValues()
+{
+ dVector3 vec;
+ dJointGetPistonAnchor(this, vec);
+ setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );
+
+ dJointGetPistonAxis(this, vec);
+ setAxes( this, vec[0], vec[1], vec[2], axis1, axis2 );
+
+ computeInitialRelativeRotation();
+}
+
+
+
+
+void
+dxJointPiston::computeInitialRelativeRotation()
+{
+ if ( node[0].body )
+ {
+ if ( node[1].body )
+ {
+ dQMultiply1 ( qrel, node[0].body->q, node[1].body->q );
+ }
+ else
+ {
+ // set joint->qrel to the transpose of the first body q
+ qrel[0] = node[0].body->q[0];
+ for ( int i = 1; i < 4; i++ )
+ qrel[i] = -node[0].body->q[i];
+ // WARNING do we need the - in -joint->node[0].body->q[i]; or not
+ }
+ }
+}
|