void dJointAddUniversalTorques( dJointID j, dReal torque1, dReal torque2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dVector3 axis1, axis2;
dAASSERT( joint );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
dReal temp = torque1;
torque1 = - torque2;
torque2 = - temp;
}
getAxis( joint, axis1, joint->axis1 );
getAxis2( joint, axis2, joint->axis2 );
axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
if ( joint->node[0].body != 0 )
dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
if ( joint->node[1].body != 0 )
dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
}
void dJointGetPUAxis2( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
getAxis( joint, result, joint->axis1 );
else
getAxis2( joint, result, joint->axis2 );
}
dReal dJointGetPUAngle2Rate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->node[0].body )
{
dVector3 axis;
if ( joint->flags & dJOINT_REVERSE )
getAxis( joint, axis, joint->axis1 );
else
getAxis2( joint, axis, joint->axis2 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
return rate;
}
return 0;
}
void
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
const dReal k = worldFPS * worldERP;
// ======================================================================
// The angular constraint
//
dVector3 ax1, ax2; // Global axes of rotation
getAxis(this, ax1, axis1);
getAxis2(this,ax2, axis2);
dVector3 uniPerp; // Axis perpendicular to axes of rotation
dCalcVectorCross3(uniPerp,ax1,ax2);
dNormalize3( uniPerp );
dCopyVector3( J1 + GI2__JA_MIN, uniPerp );
dxBody *body1 = node[1].body;
if ( body1 ) {
dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp );
}
// Corrective velocity attempting to keep uni axes perpendicular
dReal val = dCalcVectorDot3( ax1, ax2 );
// Small angle approximation :
// theta = asin(val)
// theta is approximately val when val is near zero.
pairRhsCfm[GI2_RHS] = -k * val;
// ==========================================================================
// Handle axes orthogonal to the prismatic
dVector3 an1, an2; // Global anchor positions
dVector3 axP, sep; // Prismatic axis and separation vector
getAnchor(this, an1, anchor1);
getAnchor2(this, an2, anchor2);
if (flags & dJOINT_REVERSE) {
getAxis2(this, axP, axisP1);
} else {
getAxis(this, axP, axisP1);
}
dSubtractVectors3(sep, an2, an1);
dVector3 p, q;
dPlaneSpace(axP, p, q);
dCopyVector3( J1 + rowskip + GI2__JL_MIN, p );
dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q );
// Make the anchors be body local
// Aliasing isn't a problem here.
dSubtractVectors3(an1, an1, node[0].body->posr.pos);
dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p );
dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q );
if (body1) {
dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p );
dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q );
dSubtractVectors3(an2, an2, body1->posr.pos);
dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 );
dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 );
}
pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep );
pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep );
// ==========================================================================
// Handle the limits/motors
int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip;
if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) {
currRowSkip += rowskip; currPairSkip += pairskip;
}
if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) {
currRowSkip += rowskip; currPairSkip += pairskip;
}
if ( body1 || (flags & dJOINT_REVERSE) == 0 ) {
limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
} else {
dNegateVector3(axP);
limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
}
}
void
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
const int s1 = info->rowskip;
const int s2 = 2 * s1;
const dReal k = worldFPS * worldERP;
// ======================================================================
// The angular constraint
//
dVector3 ax1, ax2; // Global axes of rotation
getAxis(this, ax1, axis1);
getAxis2(this,ax2, axis2);
dVector3 uniPerp; // Axis perpendicular to axes of rotation
dCalcVectorCross3(uniPerp,ax1,ax2);
dNormalize3( uniPerp );
dCopyVector3( info->J1a , uniPerp );
if ( node[1].body )
{
dCopyNegatedVector3( info->J2a , uniPerp );
}
// Corrective velocity attempting to keep uni axes perpendicular
dReal val = dCalcVectorDot3( ax1, ax2 );
// Small angle approximation :
// theta = asin(val)
// theta is approximately val when val is near zero.
info->c[0] = -k * val;
// ==========================================================================
// Handle axes orthogonal to the prismatic
dVector3 an1, an2; // Global anchor positions
dVector3 axP, sep; // Prismatic axis and separation vector
getAnchor(this,an1,anchor1);
getAnchor2(this,an2,anchor2);
if (flags & dJOINT_REVERSE) {
getAxis2(this, axP, axisP1);
} else {
getAxis(this, axP, axisP1);
}
dSubtractVectors3(sep,an2,an1);
dVector3 p,q;
dPlaneSpace(axP,p,q);
dCopyVector3(( info->J1l ) + s1, p );
dCopyVector3(( info->J1l ) + s2, q );
// Make the anchors be body local
// Aliasing isn't a problem here.
dSubtractVectors3(an1,an1,node[0].body->posr.pos);
dCalcVectorCross3(( info->J1a ) + s1, an1, p );
dCalcVectorCross3(( info->J1a ) + s2, an1, q );
if (node[1].body) {
dCopyNegatedVector3(( info->J2l ) + s1, p );
dCopyNegatedVector3(( info->J2l ) + s2, q );
dSubtractVectors3(an2,an2,node[1].body->posr.pos);
dCalcVectorCross3(( info->J2a ) + s1, p, an2 );
dCalcVectorCross3(( info->J2a ) + s2, q, an2 );
}
info->c[1] = k * dCalcVectorDot3( p, sep );
info->c[2] = k * dCalcVectorDot3( q, sep );
// ==========================================================================
// Handle the limits/motors
int row = 3 + limot1.addLimot( this, worldFPS, info, 3, ax1, 1 );
row += limot2.addLimot( this, worldFPS, info, row, ax2, 1 );
if ( node[1].body || !(flags & dJOINT_REVERSE) )
limotP.addTwoPointLimot( this, worldFPS, info, row, axP, an1, an2 );
else
{
axP[0] = -axP[0];
axP[1] = -axP[1];
axP[2] = -axP[2];
limotP.addTwoPointLimot ( this, worldFPS, info, row, axP, an1, an2 );
}
}
void dJointSetUniversalAxis1Offset( dJointID j, dReal x, dReal y, dReal z,
dReal offset1, dReal offset2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
setAxes( joint, x, y, z, NULL, joint->axis2 );
offset1 = -offset1;
offset2 = -offset2;
}
else
setAxes( joint, x, y, z, joint->axis1, NULL );
joint->computeInitialRelativeRotations();
dVector3 ax2;
getAxis2( joint, ax2, joint->axis2 );
{
dVector3 ax1;
joint->getAxes(ax1, ax2);
}
dQuaternion qAngle;
dQFromAxisAndAngle(qAngle, x, y, z, offset1);
dMatrix3 R;
dRFrom2Axes( R, x, y, z, ax2[0], ax2[1], ax2[2] );
dQuaternion qcross;
dRtoQ( R, qcross );
dQuaternion qOffset;
dQMultiply0(qOffset, qAngle, qcross);
dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
// Calculating the second offset
dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], x, y, z );
dRtoQ( R, qcross );
dQMultiply1(qOffset, qAngle, qcross);
if ( joint->node[1].body )
{
dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
}
else
{
joint->qrel2[0] = qcross[0];
joint->qrel2[1] = qcross[1];
joint->qrel2[2] = qcross[2];
joint->qrel2[3] = qcross[3];
}
}
void PhysicsAMotorJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
if(!(whichField & VelFieldMask))
{
setVel(dJointGetAMotorParam(_JointID,dParamVel));
}
if(!(whichField & FMaxFieldMask))
{
setFMax(dJointGetAMotorParam(_JointID,dParamFMax));
}
if(!(whichField & FudgeFactorFieldMask))
{
setFudgeFactor(dJointGetAMotorParam(_JointID,dParamFudgeFactor));
}
if(!(whichField & Vel2FieldMask))
{
setVel2(dJointGetAMotorParam(_JointID,dParamVel2));
}
if(!(whichField & FMax2FieldMask))
{
setFMax2(dJointGetAMotorParam(_JointID,dParamFMax2));
}
if(!(whichField & FudgeFactor2FieldMask))
{
setFudgeFactor2(dJointGetAMotorParam(_JointID,dParamFudgeFactor2));
}
if(!(whichField & Vel3FieldMask))
{
setVel3(dJointGetAMotorParam(_JointID,dParamVel3));
}
if(!(whichField & FMax3FieldMask))
{
setFMax3(dJointGetAMotorParam(_JointID,dParamFMax3));
}
if(!(whichField & FudgeFactor3FieldMask))
{
setFudgeFactor3(dJointGetAMotorParam(_JointID,dParamFudgeFactor3));
}
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetAMotorParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetAMotorParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetAMotorParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetAMotorParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetAMotorParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetAMotorParam(_JointID,dParamStopERP));
}
if(!(whichField & HiStop2FieldMask))
{
setHiStop2(dJointGetAMotorParam(_JointID,dParamHiStop2));
}
if(!(whichField & LoStop2FieldMask))
{
setLoStop2(dJointGetAMotorParam(_JointID,dParamLoStop2));
}
if(!(whichField & Bounce2FieldMask))
{
setBounce2(dJointGetAMotorParam(_JointID,dParamBounce2));
}
if(!(whichField & CFM2FieldMask))
{
setCFM2(dJointGetAMotorParam(_JointID,dParamCFM2));
}
if(!(whichField & StopCFM2FieldMask))
{
setStopCFM2(dJointGetAMotorParam(_JointID,dParamStopCFM2));
}
if(!(whichField & StopERP2FieldMask))
{
setStopERP2(dJointGetAMotorParam(_JointID,dParamStopERP2));
}
if(!(whichField & HiStop3FieldMask))
{
setHiStop3(dJointGetAMotorParam(_JointID,dParamHiStop3));
}
if(!(whichField & LoStop3FieldMask))
{
setLoStop3(dJointGetAMotorParam(_JointID,dParamLoStop3));
}
if(!(whichField & Bounce3FieldMask))
{
setBounce3(dJointGetAMotorParam(_JointID,dParamBounce3));
}
if(!(whichField & CFM3FieldMask))
{
setCFM3(dJointGetAMotorParam(_JointID,dParamCFM3));
}
if(!(whichField & StopCFM3FieldMask))
{
setStopCFM3(dJointGetAMotorParam(_JointID,dParamStopCFM3));
}
if(!(whichField & StopERP3FieldMask))
{
setStopERP3(dJointGetAMotorParam(_JointID,dParamStopERP3));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & NumAxesFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorNumAxes(_JointID,getNumAxes());
}
if((whichField & Axis1FieldMask) ||
(whichField & Axis1ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,0, getAxis1ReferenceFrame(), getAxis1().x(), getAxis1().y(), getAxis1().z());
}
if((whichField & Axis2FieldMask) ||
(whichField & Axis2ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,1, getAxis2ReferenceFrame(), getAxis2().x(), getAxis2().y(), getAxis2().z());
}
if((whichField & Axis3FieldMask) ||
(whichField & Axis3ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,2, getAxis3ReferenceFrame(), getAxis3().x(), getAxis3().y(), getAxis3().z());
}
if((whichField & VelFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel, getVel());
}
if((whichField & FMaxFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax, getFMax());
}
if((whichField & FudgeFactorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor, getFudgeFactor());
}
if((whichField & Vel2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel2, getVel2());
}
if((whichField & FMax2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax2, getFMax2());
}
if((whichField & FudgeFactor2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor2, getFudgeFactor2());
}
if((whichField & Vel3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel3, getVel3());
}
if((whichField & FMax3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax3, getFMax3());
}
if((whichField & FudgeFactor3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor3, getFudgeFactor3());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM, getStopCFM());
}
if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop2, getHiStop2());
}
if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop2, getLoStop2());
}
if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce2, getBounce2());
}
if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM2, getCFM2());
}
if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP2, getStopERP2());
}
if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM2, getStopCFM2());
}
if((whichField & HiStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop3, getHiStop3());
}
if((whichField & LoStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop3, getLoStop3());
}
if((whichField & Bounce3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce3, getBounce3());
}
if((whichField & CFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM3, getCFM3());
}
if((whichField & StopERP3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP3, getStopERP3());
}
if((whichField & StopCFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM3, getStopCFM3());
}
}
