void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
int s = info->rowskip;
// Three rows for orientation
setFixedOrientation ( this, info, qrel, 3 );
// Three rows for position.
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
info->erp = erp;
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
dVector3 ofs;
dMULTIPLY0_331 ( ofs, node[0].body->posr.R, offset );
if ( node[1].body )
{
dCROSSMAT ( info->J1a, ofs, s, + , - );
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
}
void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
info->cfm[5] = cfm;
}
int s = info->rowskip;
// Three rows for orientation
setFixedOrientation ( this, info, qrel, 3 );
// Three rows for position.
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
dVector3 ofs;
dMultiply0_331 ( ofs, node[0].body->posr.R, offset );
if ( node[1].body )
{
dSetCrossMatrixPlus( info->J1a, ofs, s );
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
}
// set right hand side for the first three rows (linear)
dReal k = info->fps * info->erp;
if ( node[1].body )
{
for ( int j = 0; j < 3; j++ )
info->c[j] = k * ( node[1].body->posr.pos[j]
- node[0].body->posr.pos[j]
+ ofs[j] );
}
else
{
for ( int j = 0; j < 3; j++ )
info->c[j] = k * ( offset[j] - node[0].body->posr.pos[j] );
}
}
void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
int s = info->rowskip;
// Three rows for orientation
setFixedOrientation ( this, info, qrel, 3 );
// Three rows for position.
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
info->erp = erp;
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
dVector3 ofs;
dMultiply0_331 ( ofs, node[0].body->posr.R, offset );
if ( node[1].body )
{
dSetCrossMatrixPlus( info->J1a, ofs, s );
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
}
// set right hand side for the first three rows (linear)
dReal k = info->fps * info->erp;
if ( node[1].body )
{
for ( int j = 0; j < 3; j++ )
info->c[j] = k * ( node[1].body->posr.pos[j]
- node[0].body->posr.pos[j]
+ ofs[j] );
}
else
{
for ( int j = 0; j < 3; j++ )
info->c[j] = k * ( offset[j] - node[0].body->posr.pos[j] );
}
}
void
dxJointSlider::getInfo2 ( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
int i, s = info->rowskip;
int s3 = 3 * s, s4 = 4 * s;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2, *R1, *R2;
dVector3 c;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
for ( i = 0; i < 3; i++ )
c[i] = pos2[i] - pos1[i];
}
else
{
pos2 = 0;
R2 = 0;
}
// 3 rows to make body rotations equal
setFixedOrientation ( this, worldFPS, worldERP, info, qrel, 0 );
// remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
dVector3 ax1; // joint axis in global coordinates (unit length)
dVector3 p, q; // plane space of ax1
dMultiply0_331 ( ax1, R1, axis1 );
dPlaneSpace ( ax1, p, q );
if ( node[1].body )
{
dVector3 tmp;
dCalcVectorCross3( tmp, c, p );
dScaleVector3r4( tmp, REAL( 0.5 ));
for ( i = 0; i < 3; i++ ) info->J1a[s3+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2a[s3+i] = tmp[i];
dCalcVectorCross3( tmp, c, q );
dScaleVector3r4( tmp, REAL( 0.5 ));
for ( i = 0; i < 3; i++ ) info->J1a[s4+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2a[s4+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2l[s3+i] = -p[i];
for ( i = 0; i < 3; i++ ) info->J2l[s4+i] = -q[i];
}
for ( i = 0; i < 3; i++ ) info->J1l[s3+i] = p[i];
for ( i = 0; i < 3; i++ ) info->J1l[s4+i] = q[i];
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
dReal k = worldFPS * worldERP;
if ( node[1].body )
{
dVector3 ofs; // offset point in global coordinates
dMultiply0_331 ( ofs, R2, offset );
for ( i = 0; i < 3; i++ ) c[i] += ofs[i];
info->c[3] = k * dCalcVectorDot3 ( p, c );
info->c[4] = k * dCalcVectorDot3 ( q, c );
}
else
{
dVector3 ofs; // offset point in global coordinates
for ( i = 0; i < 3; i++ ) ofs[i] = offset[i] - pos1[i];
info->c[3] = k * dCalcVectorDot3 ( p, ofs );
info->c[4] = k * dCalcVectorDot3 ( q, ofs );
if ( flags & dJOINT_REVERSE )
for ( i = 0; i < 3; ++i ) ax1[i] = -ax1[i];
}
// if the slider is powered, or has joint limits, add in the extra row
limot.addLimot ( this, worldFPS, info, 5, ax1, 0 );
}