From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/src/joints/joint.cpp | 931 +++++++++++++++++++++++++++++++ 1 file changed, 931 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/joints/joint.cpp (limited to 'libs/ode-0.16.1/ode/src/joints/joint.cpp') 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 +#include +#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: -- cgit v1.2.1