diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/joints/joint.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/joints/joint.cpp | 931 |
1 files changed, 931 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/joints/joint.cpp b/libs/ode-0.16.1/ode/src/joints/joint.cpp new file mode 100644 index 0000000..1b7de7a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/joint.cpp @@ -0,0 +1,931 @@ +/*************************************************************************
+ * *
+ * 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. *
+ * *
+ *************************************************************************/
+
+/*
+
+design note: the general principle for giving a joint the option of connecting
+to the static environment (i.e. the absolute frame) is to check the second
+body (joint->node[1].body), and if it is zero then behave as if its body
+transform is the identity.
+
+*/
+
+#include <ode/ode.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "joint.h"
+#include "joint_internal.h"
+#include "util.h"
+
+extern void addObjectToList( dObject *obj, dObject **first );
+
+dxJoint::dxJoint( dxWorld *w ) :
+ dObject( w )
+{
+ //printf("constructing %p\n", this);
+ dIASSERT( w );
+ flags = 0;
+ node[0].joint = this;
+ node[0].body = 0;
+ node[0].next = 0;
+ node[1].joint = this;
+ node[1].body = 0;
+ node[1].next = 0;
+ dSetZero( lambda, 6 );
+
+ addObjectToList( this, ( dObject ** ) &w->firstjoint );
+
+ w->nj++;
+ feedback = 0;
+}
+
+dxJoint::~dxJoint()
+{ }
+
+
+/*virtual */
+void dxJoint::setRelativeValues()
+{
+ // Do nothing
+}
+
+bool dxJoint::isEnabled() const
+{
+ return ( (flags & dJOINT_DISABLED) == 0 &&
+ (node[0].body->invMass > 0 ||
+ (node[1].body && node[1].body->invMass > 0)) );
+}
+
+
+sizeint dxJointGroup::exportJoints(dxJoint **jlist)
+{
+ sizeint i=0;
+ dxJoint *j = (dxJoint*) m_stack.rewind();
+ while (j != NULL) {
+ jlist[i++] = j;
+ j = (dxJoint*) (m_stack.next (j->size()));
+ }
+ return i;
+}
+
+void dxJointGroup::freeAll()
+{
+ m_num = 0;
+ m_stack.freeAll();
+}
+
+
+//****************************************************************************
+// externs
+
+// extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
+// extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
+
+//****************************************************************************
+// utility
+
+// set three "ball-and-socket" rows in the constraint equation, and the
+// corresponding right hand side.
+
+void setBall( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dVector3 anchor1, dVector3 anchor2 )
+{
+ // anchor points in global coordinates with respect to body PORs.
+ dVector3 a1, a2;
+
+ // set Jacobian
+ J1[dxJoint::GI2_JLX] = 1;
+ J1[rowskip + dxJoint::GI2_JLY] = 1;
+ J1[2 * rowskip + dxJoint::GI2_JLZ] = 1;
+ dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
+ dSetCrossMatrixMinus( J1 + dxJoint::GI2__JA_MIN, a1, rowskip );
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ J2[dxJoint::GI2_JLX] = -1;
+ J2[rowskip + dxJoint::GI2_JLY] = -1;
+ J2[2 * rowskip + dxJoint::GI2_JLZ] = -1;
+ dMultiply0_331( a2, b1->posr.R, anchor2 );
+ dSetCrossMatrixPlus( J2 + dxJoint::GI2__JA_MIN, a2, rowskip );
+ }
+
+ // set right hand side
+ dReal k = fps * erp;
+ dxBody *b0 = joint->node[0].body;
+ if ( b1 )
+ {
+ dReal *currRhsCfm = pairRhsCfm;
+ for ( int j = dSA__MIN; j != dSA__MAX; j++ )
+ {
+ currRhsCfm[dxJoint::GI2_RHS] = k * ( a2[j] + b1->posr.pos[j] - a1[j] - b0->posr.pos[j] );
+ currRhsCfm += pairskip;
+ }
+ }
+ else
+ {
+ dReal *currRhsCfm = pairRhsCfm;
+ for ( int j = dSA__MIN; j != dSA__MAX; j++ )
+ {
+ currRhsCfm[dxJoint::GI2_RHS] = k * ( anchor2[j] - a1[j] - b0->posr.pos[j] );
+ currRhsCfm += pairskip;
+ }
+ }
+}
+
+
+// this is like setBall(), except that `axis' is a unit length vector
+// (in global coordinates) that should be used for the first jacobian
+// position row (the other two row vectors will be derived from this).
+// `erp1' is the erp value to use along the axis.
+
+void setBall2( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dVector3 anchor1, dVector3 anchor2,
+ dVector3 axis, dReal erp1 )
+{
+ // anchor points in global coordinates with respect to body PORs.
+ dVector3 a1, a2;
+
+ // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
+ // [0 1 0] and [0 0 1], which makes everything much easier.
+ dVector3 q1, q2;
+ dPlaneSpace( axis, q1, q2 );
+
+ // set Jacobian
+ dCopyVector3(J1 + dxJoint::GI2__JL_MIN, axis);
+ dCopyVector3(J1 + rowskip + dxJoint::GI2__JL_MIN, q1);
+ dCopyVector3(J1 + 2 * rowskip + dxJoint::GI2__JL_MIN, q2);
+ dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
+ dCalcVectorCross3( J1 + dxJoint::GI2__JA_MIN, a1, axis );
+ dCalcVectorCross3( J1 + rowskip + dxJoint::GI2__JA_MIN, a1, q1 );
+ dCalcVectorCross3( J1 + 2 * rowskip + dxJoint::GI2__JA_MIN, a1, q2 );
+
+ dxBody *b0 = joint->node[0].body;
+ dAddVectors3(a1, a1, b0->posr.pos);
+
+ // set right hand side - measure error along (axis,q1,q2)
+ dReal k1 = fps * erp1;
+ dReal k = fps * erp;
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ dCopyNegatedVector3(J2 + dxJoint::GI2__JL_MIN, axis);
+ dCopyNegatedVector3(J2 + rowskip + dxJoint::GI2__JL_MIN, q1);
+ dCopyNegatedVector3(J2 + 2 * rowskip + dxJoint::GI2__JL_MIN, q2);
+ dMultiply0_331( a2, b1->posr.R, anchor2 );
+ dCalcVectorCross3( J2 + dxJoint::GI2__JA_MIN, axis, a2 ); //== dCalcVectorCross3( J2 + dxJoint::GI2__J2A_MIN, a2, axis ); dNegateVector3( J2 + dxJoint::GI2__J2A_MIN );
+ dCalcVectorCross3( J2 + rowskip + dxJoint::GI2__JA_MIN, q1, a2 ); //== dCalcVectorCross3( J2 + rowskip + dxJoint::GI2__J2A_MIN, a2, q1 ); dNegateVector3( J2 + rowskip + dxJoint::GI2__J2A_MIN );
+ dCalcVectorCross3( J2 + 2 * rowskip + dxJoint::GI2__JA_MIN, q2, a2 ); //== dCalcVectorCross3( J2 + 2 * rowskip + dxJoint::GI2__J2A_MIN, a2, q2 ); dNegateVector3( J2 + 2 * rowskip + dxJoint::GI2__J2A_MIN );
+
+ dAddVectors3(a2, a2, b1->posr.pos);
+
+ dVector3 a2_minus_a1;
+ dSubtractVectors3(a2_minus_a1, a2, a1);
+ pairRhsCfm[dxJoint::GI2_RHS] = k1 * dCalcVectorDot3( axis, a2_minus_a1 );
+ pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q1, a2_minus_a1 );
+ pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q2, a2_minus_a1 );
+ }
+ else
+ {
+ dVector3 anchor2_minus_a1;
+ dSubtractVectors3(anchor2_minus_a1, anchor2, a1);
+ pairRhsCfm[dxJoint::GI2_RHS] = k1 * dCalcVectorDot3( axis, anchor2_minus_a1 );
+ pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q1, anchor2_minus_a1 );
+ pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q2, anchor2_minus_a1 );
+ }
+}
+
+
+// set three orientation rows in the constraint equation, and the
+// corresponding right hand side.
+
+void setFixedOrientation( dxJoint *joint, dReal fps, dReal erp,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm,
+ dQuaternion qrel )
+{
+ // 3 rows to make body rotations equal
+ J1[dxJoint::GI2_JAX] = 1;
+ J1[rowskip + dxJoint::GI2_JAY] = 1;
+ J1[2 * rowskip + dxJoint::GI2_JAZ] = 1;
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ J2[dxJoint::GI2_JAX] = -1;
+ J2[rowskip + dxJoint::GI2_JAY] = -1;
+ J2[2 * rowskip + dxJoint::GI2_JAZ] = -1;
+ }
+
+ // compute the right hand side. the first three elements will result in
+ // relative angular velocity of the two bodies - this is set to bring them
+ // back into alignment. the correcting angular velocity is
+ // |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
+ // to qrel with respect to body 1's frame. using a small angle approximation
+ // for sin(), this gives
+ // angular_velocity = (erp*fps) * 2 * v
+ // where the quaternion of the relative rotation between the two bodies is
+ // q = [cos(theta/2) sin(theta/2)*u] = [s v]
+
+ // get qerr = relative rotation (rotation error) between two bodies
+ dQuaternion qerr, e;
+ dxBody *b0 = joint->node[0].body;
+ if ( b1 )
+ {
+ dQuaternion qq;
+ dQMultiply1( qq, b0->q, b1->q );
+ dQMultiply2( qerr, qq, qrel );
+ }
+ else
+ {
+ dQMultiply3( qerr, b0->q, qrel );
+ }
+ if ( qerr[0] < 0 )
+ {
+ qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
+ qerr[2] = -qerr[2];
+ qerr[3] = -qerr[3];
+ }
+ dMultiply0_331( e, b0->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
+ dReal k_mul_2 = fps * erp * REAL(2.0);
+ pairRhsCfm[dxJoint::GI2_RHS] = k_mul_2 * e[dSA_X];
+ pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k_mul_2 * e[dSA_Y];
+ pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k_mul_2 * e[dSA_Z];
+}
+
+
+// compute anchor points relative to bodies
+
+void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
+ dVector3 anchor1, dVector3 anchor2 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dReal q[4];
+ q[0] = x - b0->posr.pos[0];
+ q[1] = y - b0->posr.pos[1];
+ q[2] = z - b0->posr.pos[2];
+ q[3] = 0;
+ dMultiply1_331( anchor1, b0->posr.R, q );
+
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ q[0] = x - b1->posr.pos[0];
+ q[1] = y - b1->posr.pos[1];
+ q[2] = z - b1->posr.pos[2];
+ q[3] = 0;
+ dMultiply1_331( anchor2, b1->posr.R, q );
+ }
+ else
+ {
+ anchor2[0] = x;
+ anchor2[1] = y;
+ anchor2[2] = z;
+ }
+ }
+ anchor1[3] = 0;
+ anchor2[3] = 0;
+}
+
+
+// compute axes relative to bodies. either axis1 or axis2 can be 0.
+
+void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
+ dVector3 axis1, dVector3 axis2 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dReal q[4];
+ q[0] = x;
+ q[1] = y;
+ q[2] = z;
+ q[3] = 0;
+ dNormalize3( q );
+
+ if ( axis1 )
+ {
+ dMultiply1_331( axis1, b0->posr.R, q );
+ axis1[3] = 0;
+ }
+
+ if ( axis2 )
+ {
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ dMultiply1_331( axis2, b1->posr.R, q );
+ }
+ else
+ {
+ axis2[0] = x;
+ axis2[1] = y;
+ axis2[2] = z;
+ }
+ axis2[3] = 0;
+ }
+ }
+}
+
+
+void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dMultiply0_331( result, b0->posr.R, anchor1 );
+ result[0] += b0->posr.pos[0];
+ result[1] += b0->posr.pos[1];
+ result[2] += b0->posr.pos[2];
+ }
+}
+
+
+void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 )
+{
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ dMultiply0_331( result, b1->posr.R, anchor2 );
+ result[0] += b1->posr.pos[0];
+ result[1] += b1->posr.pos[1];
+ result[2] += b1->posr.pos[2];
+ }
+ else
+ {
+ result[0] = anchor2[0];
+ result[1] = anchor2[1];
+ result[2] = anchor2[2];
+ }
+}
+
+
+void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
+{
+ dxBody *b0 = j->node[0].body;
+ if ( b0 )
+ {
+ dMultiply0_331( result, b0->posr.R, axis1 );
+ }
+}
+
+
+void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )
+{
+ dxBody *b1 = j->node[1].body;
+ if ( b1 )
+ {
+ dMultiply0_331( result, b1->posr.R, axis2 );
+ }
+ else
+ {
+ result[0] = axis2[0];
+ result[1] = axis2[1];
+ result[2] = axis2[2];
+ }
+}
+
+
+dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
+{
+ // the angle between the two bodies is extracted from the quaternion that
+ // represents the relative rotation between them. recall that a quaternion
+ // q is:
+ // [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
+ // where s is a scalar and v is a 3-vector. u is a unit length axis and
+ // theta is a rotation along that axis. we can get theta/2 by:
+ // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
+ // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
+ // |v| = |sin(theta/2)| * |u|
+ // = |sin(theta/2)|
+ // using this value will have a strange effect. recall that there are two
+ // quaternion representations of a given rotation, q and -q. typically as
+ // a body rotates along the axis it will go through a complete cycle using
+ // one representation and then the next cycle will use the other
+ // representation. this corresponds to u pointing in the direction of the
+ // hinge axis and then in the opposite direction. the result is that theta
+ // will appear to go "backwards" every other cycle. here is a fix: if u
+ // points "away" from the direction of the hinge (motor) axis (i.e. more
+ // than 90 degrees) then use -q instead of q. this represents the same
+ // rotation, but results in the cos(theta/2) value being sign inverted.
+
+ // extract the angle from the quaternion. cost2 = cos(theta/2),
+ // sint2 = |sin(theta/2)|
+ dReal cost2 = qrel[0];
+ dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
+ dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
+ ( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direction of axis
+ ( 2 * dAtan2( sint2, -cost2 ) ); // if u points in opposite direction
+
+ // the angle we get will be between 0..2*pi, but we want to return angles
+ // between -pi..pi
+ if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );
+
+ // the angle we've just extracted has the wrong sign
+ theta = -theta;
+
+ return theta;
+}
+
+
+// given two bodies (body1,body2), the hinge axis that they are connected by
+// w.r.t. body1 (axis), and the initial relative orientation between them
+// (q_initial), return the relative rotation angle. the initial relative
+// orientation corresponds to an angle of zero. if body2 is 0 then measure the
+// angle between body1 and the static frame.
+//
+// this will not return the correct angle if the bodies rotate along any axis
+// other than the given hinge axis.
+
+dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis,
+ dQuaternion q_initial )
+{
+ // get qrel = relative rotation between the two bodies
+ dQuaternion qrel;
+ if ( body2 )
+ {
+ dQuaternion qq;
+ dQMultiply1( qq, body1->q, body2->q );
+ dQMultiply2( qrel, qq, q_initial );
+ }
+ else
+ {
+ // pretend body2->q is the identity
+ dQMultiply3( qrel, body1->q, q_initial );
+ }
+
+ return getHingeAngleFromRelativeQuat( qrel, axis );
+}
+
+//****************************************************************************
+// dxJointLimitMotor
+
+void dxJointLimitMotor::init( dxWorld *world )
+{
+ vel = 0;
+ fmax = 0;
+ lostop = -dInfinity;
+ histop = dInfinity;
+ fudge_factor = 1;
+ normal_cfm = world->global_cfm;
+ stop_erp = world->global_erp;
+ stop_cfm = world->global_cfm;
+ bounce = 0;
+ limit = 0;
+ limit_err = 0;
+}
+
+
+void dxJointLimitMotor::set( int num, dReal value )
+{
+ switch ( num )
+ {
+ case dParamLoStop:
+ lostop = value;
+ break;
+ case dParamHiStop:
+ histop = value;
+ break;
+ case dParamVel:
+ vel = value;
+ break;
+ case dParamFMax:
+ if ( value >= 0 ) fmax = value;
+ break;
+ case dParamFudgeFactor:
+ if ( value >= 0 && value <= 1 ) fudge_factor = value;
+ break;
+ case dParamBounce:
+ bounce = value;
+ break;
+ case dParamCFM:
+ normal_cfm = value;
+ break;
+ case dParamStopERP:
+ stop_erp = value;
+ break;
+ case dParamStopCFM:
+ stop_cfm = value;
+ break;
+ }
+}
+
+
+dReal dxJointLimitMotor::get( int num ) const
+{
+ switch ( num )
+ {
+ case dParamLoStop:
+ return lostop;
+ case dParamHiStop:
+ return histop;
+ case dParamVel:
+ return vel;
+ case dParamFMax:
+ return fmax;
+ case dParamFudgeFactor:
+ return fudge_factor;
+ case dParamBounce:
+ return bounce;
+ case dParamCFM:
+ return normal_cfm;
+ case dParamStopERP:
+ return stop_erp;
+ case dParamStopCFM:
+ return stop_cfm;
+ default:
+ return 0;
+ }
+}
+
+
+bool dxJointLimitMotor::testRotationalLimit( dReal angle )
+{
+ if ( angle <= lostop )
+ {
+ limit = 1;
+ limit_err = angle - lostop;
+ return true;
+ }
+ else if ( angle >= histop )
+ {
+ limit = 2;
+ limit_err = angle - histop;
+ return true;
+ }
+ else
+ {
+ limit = 0;
+ return false;
+ }
+}
+
+
+bool dxJointLimitMotor::addLimot( dxJoint *joint,
+ dReal fps, dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+ const dVector3 ax1, int rotational )
+{
+ // if the joint is powered, or has joint limits, add in the extra row
+ int powered = fmax > 0;
+ if ( powered || limit )
+ {
+ dReal *J1Used = rotational ? J1 + GI2__JA_MIN : J1 + GI2__JL_MIN;
+ dReal *J2Used = rotational ? J2 + GI2__JA_MIN : J2 + GI2__JL_MIN;
+
+ dCopyVector3(J1Used, ax1);
+
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 )
+ {
+ dCopyNegatedVector3(J2Used, ax1);
+ }
+
+ // linear limot torque decoupling step:
+ //
+ // if this is a linear limot (e.g. from a slider), we have to be careful
+ // that the linear constraint forces (+/- ax1) applied to the two bodies
+ // do not create a torque couple. in other words, the points that the
+ // constraint force is applied at must lie along the same ax1 axis.
+ // a torque couple will result in powered or limited slider-jointed free
+ // bodies from gaining angular momentum.
+ // the solution used here is to apply the constraint forces at the point
+ // halfway between the body centers. there is no penalty (other than an
+ // extra tiny bit of computation) in doing this adjustment. note that we
+ // only need to do this if the constraint connects two bodies.
+
+ dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
+ if ( !rotational && b1 )
+ {
+ dxBody *b0 = joint->node[0].body;
+ dVector3 c;
+ c[0] = REAL( 0.5 ) * ( b1->posr.pos[0] - b0->posr.pos[0] );
+ c[1] = REAL( 0.5 ) * ( b1->posr.pos[1] - b0->posr.pos[1] );
+ c[2] = REAL( 0.5 ) * ( b1->posr.pos[2] - b0->posr.pos[2] );
+ dCalcVectorCross3( ltd, c, ax1 );
+ dCopyVector3(J1 + dxJoint::GI2__JA_MIN, ltd);
+ dCopyVector3(J2 + dxJoint::GI2__JA_MIN, ltd);
+ }
+
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if ( limit && ( lostop == histop ) ) powered = 0;
+
+ if ( powered )
+ {
+ pairRhsCfm[GI2_CFM] = normal_cfm;
+ if ( ! limit )
+ {
+ pairRhsCfm[GI2_RHS] = vel;
+ pairLoHi[GI2_LO] = -fmax;
+ pairLoHi[GI2_HI] = fmax;
+ }
+ else
+ {
+ // the joint is at a limit, AND is being powered. if the joint is
+ // being powered into the limit then we apply the maximum motor force
+ // in that direction, because the motor is working against the
+ // immovable limit. if the joint is being powered away from the limit
+ // then we have problems because actually we need *two* lcp
+ // constraints to handle this case. so we fake it and apply some
+ // fraction of the maximum force. the fraction to use can be set as
+ // a fudge factor.
+
+ dReal fm = fmax;
+ if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+
+ // if we're powering away from the limit, apply the fudge factor
+ if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+
+
+ dReal fm_ax1_0 = fm*ax1[0], fm_ax1_1 = fm*ax1[1], fm_ax1_2 = fm*ax1[2];
+
+ dxBody *b0 = joint->node[0].body;
+ dxWorldProcessContext *world_process_context = b0->world->unsafeGetWorldProcessingContext();
+
+ world_process_context->LockForAddLimotSerialization();
+
+ if ( rotational )
+ {
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 != NULL )
+ {
+ dBodyAddTorque( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
+ }
+
+ dBodyAddTorque( b0, -fm_ax1_0, -fm_ax1_1, -fm_ax1_2 );
+ }
+ else
+ {
+ dxBody *b1 = joint->node[1].body;
+ if ( b1 != NULL )
+ {
+ // linear limot torque decoupling step: refer to above discussion
+ dReal neg_fm_ltd_0 = -fm*ltd[0], neg_fm_ltd_1 = -fm*ltd[1], neg_fm_ltd_2 = -fm*ltd[2];
+ dBodyAddTorque( b0, neg_fm_ltd_0, neg_fm_ltd_1, neg_fm_ltd_2 );
+ dBodyAddTorque( b1, neg_fm_ltd_0, neg_fm_ltd_1, neg_fm_ltd_2 );
+
+ dBodyAddForce( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
+ }
+
+ dBodyAddForce( b0, -fm_ax1_0, -fm_ax1_1, -fm_ax1_2 );
+ }
+
+ world_process_context->UnlockForAddLimotSerialization();
+ }
+ }
+
+ if ( limit )
+ {
+ dReal k = fps * stop_erp;
+ pairRhsCfm[GI2_RHS] = -k * limit_err;
+ pairRhsCfm[GI2_CFM] = stop_cfm;
+
+ if ( lostop == histop )
+ {
+ // limited low and high simultaneously
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ if ( limit == 1 )
+ {
+ // low limit
+ pairLoHi[GI2_LO] = 0;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ // high limit
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = 0;
+ }
+
+ // deal with bounce
+ if ( bounce > 0 )
+ {
+ // calculate joint velocity
+ dReal vel;
+ if ( rotational )
+ {
+ vel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
+ if ( joint->node[1].body )
+ vel -= dCalcVectorDot3( joint->node[1].body->avel, ax1 );
+ }
+ else
+ {
+ vel = dCalcVectorDot3( joint->node[0].body->lvel, ax1 );
+ if ( joint->node[1].body )
+ vel -= dCalcVectorDot3( joint->node[1].body->lvel, ax1 );
+ }
+
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if ( limit == 1 )
+ {
+ // low limit
+ if ( vel < 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc > pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ else
+ {
+ // high limit - all those computations are reversed
+ if ( vel > 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc < pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ }
+ }
+ }
+ return true;
+ }
+ return false;
+}
+
+/**
+ This function generalizes the "linear limot torque decoupling"
+ in addLimot to use anchor points provided by the caller.
+
+ This makes it so that the appropriate torques are applied to
+ a body when it's being linearly motored or limited using anchor points
+ that aren't at the center of mass.
+
+ pt1 and pt2 are centered in body coordinates but use global directions.
+ I.e., they are conveniently found within joint code with:
+ getAxis(joint,pt1,anchor1);
+ getAxis2(joint,pt2,anchor2);
+*/
+bool dxJointLimitMotor::addTwoPointLimot( dxJoint *joint, dReal fps,
+ dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+ const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 )
+{
+ // if the joint is powered, or has joint limits, add in the extra row
+ int powered = fmax > 0;
+ if ( powered || limit )
+ {
+ // Set the linear portion
+ dCopyVector3(J1 + GI2__JL_MIN, ax1);
+ // Set the angular portion (to move the linear constraint
+ // away from the center of mass).
+ dCalcVectorCross3(J1 + GI2__JA_MIN, pt1, ax1);
+ // Set the constraints for the second body
+ if ( joint->node[1].body ) {
+ dCopyNegatedVector3(J2 + GI2__JL_MIN, ax1);
+ dCalcVectorCross3(J2 + GI2__JA_MIN, pt2, J2 + GI2__JL_MIN);
+ }
+
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if ( limit && ( lostop == histop ) ) powered = 0;
+
+ if ( powered )
+ {
+ pairRhsCfm[GI2_CFM] = normal_cfm;
+ if ( ! limit )
+ {
+ pairRhsCfm[GI2_RHS] = vel;
+ pairLoHi[GI2_LO] = -fmax;
+ pairLoHi[GI2_HI] = fmax;
+ }
+ else
+ {
+ // the joint is at a limit, AND is being powered. if the joint is
+ // being powered into the limit then we apply the maximum motor force
+ // in that direction, because the motor is working against the
+ // immovable limit. if the joint is being powered away from the limit
+ // then we have problems because actually we need *two* lcp
+ // constraints to handle this case. so we fake it and apply some
+ // fraction of the maximum force. the fraction to use can be set as
+ // a fudge factor.
+
+ dReal fm = fmax;
+ if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+
+ // if we're powering away from the limit, apply the fudge factor
+ if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+
+
+ const dReal* tAx1 = J1 + GI2__JA_MIN;
+ dBodyAddForce( joint->node[0].body, -fm*ax1[dSA_X], -fm*ax1[dSA_Y], -fm*ax1[dSA_Z] );
+ dBodyAddTorque( joint->node[0].body, -fm*tAx1[dSA_X], -fm*tAx1[dSA_Y], -fm*tAx1[dSA_Z] );
+
+ if ( joint->node[1].body )
+ {
+ const dReal* tAx2 = J2 + GI2__JA_MIN;
+ dBodyAddForce( joint->node[1].body, fm*ax1[dSA_X], fm*ax1[dSA_Y], fm*ax1[dSA_Z] );
+ dBodyAddTorque( joint->node[1].body, -fm*tAx2[dSA_X], -fm*tAx2[dSA_Y], -fm*tAx2[dSA_Z] );
+ }
+
+ }
+ }
+
+ if ( limit )
+ {
+ dReal k = fps * stop_erp;
+ pairRhsCfm[GI2_RHS] = -k * limit_err;
+ pairRhsCfm[GI2_CFM] = stop_cfm;
+
+ if ( lostop == histop )
+ {
+ // limited low and high simultaneously
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ if ( limit == 1 )
+ {
+ // low limit
+ pairLoHi[GI2_LO] = 0;
+ pairLoHi[GI2_HI] = dInfinity;
+ }
+ else
+ {
+ // high limit
+ pairLoHi[GI2_LO] = -dInfinity;
+ pairLoHi[GI2_HI] = 0;
+ }
+
+ // deal with bounce
+ if ( bounce > 0 )
+ {
+ // calculate relative velocity of the two anchor points
+ dReal vel =
+ dCalcVectorDot3( joint->node[0].body->lvel, J1 + GI2__JL_MIN ) +
+ dCalcVectorDot3( joint->node[0].body->avel, J1 + GI2__JA_MIN );
+ if (joint->node[1].body) {
+ vel +=
+ dCalcVectorDot3( joint->node[1].body->lvel, J2 + GI2__JL_MIN ) +
+ dCalcVectorDot3( joint->node[1].body->avel, J2 + GI2__JA_MIN );
+ }
+
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if ( limit == 1 )
+ {
+ // low limit
+ if ( vel < 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc > pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ else
+ {
+ // high limit - all those computations are reversed
+ if ( vel > 0 )
+ {
+ dReal newc = -bounce * vel;
+ if ( newc < pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+ }
+ }
+ }
+ }
+ }
+ return true;
+ }
+ return false;
+}
+
+
+// Local Variables:
+// mode:c++
+// c-basic-offset:4
+// End:
|