void dJointGetScrewAnchor2( dJointID j, dVector3 result )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, Screw );
if ( joint->flags & dJOINT_REVERSE )
getAnchor( joint, result, joint->anchor1 );
else
getAnchor2( joint, result, joint->anchor2 );
}
void dJointGetPUAnchor( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
if ( joint->node[1].body )
getAnchor2( joint, result, joint->anchor2 );
else
{
// result[i] = joint->anchor2[i];
dCopyVector3( result, joint->anchor2 );
}
}
void dJointGetPRAnchor( dJointID j, dVector3 result )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PR );
if ( joint->node[1].body )
getAnchor2( joint, result, joint->anchor2 );
else
{
result[0] = joint->anchor2[0];
result[1] = joint->anchor2[1];
result[2] = joint->anchor2[2];
}
}
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 );
}
}
