void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
{
// if the body is translated by `a' relative to its point of reference,
// the new inertia about the point of reference is:
//
// I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
//
// where c is the existing center of mass and I is the old inertia.
int i,j;
dMatrix3 ahat,chat,t1,t2;
dReal a[3];
dAASSERT (m);
// adjust inertia matrix
dSetZero (chat,12);
dSetCrossMatrixPlus (chat,m->c,4);
a[0] = x + m->c[0];
a[1] = y + m->c[1];
a[2] = z + m->c[2];
dSetZero (ahat,12);
dSetCrossMatrixPlus (ahat,a,4);
dMultiply0_333 (t1,ahat,ahat);
dMultiply0_333 (t2,chat,chat);
for (i=0; i<3; i++) for (j=0; j<3; j++)
m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
// ensure perfect symmetry
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
// adjust center of mass
m->c[0] += x;
m->c[1] += y;
m->c[2] += z;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
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 testCrossProduct()
{
HEADER;
dVector3 a1,a2,b,c;
dMatrix3 B;
dMakeRandomVector (b,3,1.0);
dMakeRandomVector (c,3,1.0);
dCalcVectorCross3(a1,b,c);
dSetZero (B,12);
dSetCrossMatrixPlus(B,b,4);
dMultiply0 (a2,B,c,3,3,1);
dReal diff = dMaxDifference(a1,a2,3,1);
printf ("\t%s\n", diff > tol ? "FAILED" : "passed");
}
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 setBall( dxJoint *joint, dxJoint::Info2 *info,
dVector3 anchor1, dVector3 anchor2 )
{
// anchor points in global coordinates with respect to body PORs.
dVector3 a1, a2;
int s = info->rowskip;
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
dSetCrossMatrixMinus( info->J1a, a1, s );
if ( joint->node[1].body )
{
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
dSetCrossMatrixPlus( info->J2a, a2, s );
}
// set right hand side
dReal k = info->fps * info->erp;
if ( joint->node[1].body )
{
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -
a1[j] - joint->node[0].body->posr.pos[j] );
}
}
else
{
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( anchor2[j] - a1[j] -
joint->node[0].body->posr.pos[j] );
}
}
}
int dMassCheck (const dMass *m)
{
int i;
if (m->mass <= 0) {
// dDEBUGMSG ("mass must be > 0");
return 0;
}
if (!dIsPositiveDefinite (m->I,3,NULL)) {
dDEBUGMSG ("inertia must be positive definite");
return 0;
}
// verify that the center of mass position is consistent with the mass
// and inertia matrix. this is done by checking that the inertia around
// the center of mass is also positive definite. from the comment in
// dMassTranslate(), if the body is translated so that its center of mass
// is at the point of reference, then the new inertia is:
// I + mass*crossmat(c)^2
// note that requiring this to be positive definite is exactly equivalent
// to requiring that the spatial inertia matrix
// [ mass*eye(3,3) M*crossmat(c)^T ]
// [ M*crossmat(c) I ]
// is positive definite, given that I is PD and mass>0. see the theorem
// about partitioned PD matrices for proof.
dMatrix3 I2,chat;
dSetZero (chat,12);
dSetCrossMatrixPlus (chat,m->c,4);
dMultiply0_333 (I2,chat,chat);
for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i];
for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i];
for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i];
if (!dIsPositiveDefinite (I2,3,NULL)) {
dDEBUGMSG ("center of mass inconsistent with mass parameters");
return 0;
}
return 1;
}
void
dxJointDBall::getInfo2( dxJoint::Info2 *info )
{
info->erp = erp;
info->cfm[0] = cfm;
dVector3 globalA1, globalA2;
dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
if (node[1].body)
dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
else
dCopyVector3(globalA2, anchor2);
dVector3 q;
dSubtractVectors3(q, globalA1, globalA2);
#ifdef dSINGLE
const dReal MIN_LENGTH = REAL(1e-7);
#else
const dReal MIN_LENGTH = REAL(1e-12);
#endif
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// too small, let's choose an arbitrary direction
// heuristic: difference in velocities at anchors
dVector3 v1, v2;
dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
if (node[1].body)
dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
else
dSetZero(v2, 3);
dSubtractVectors3(q, v1, v2);
if (dCalcVectorLength3(q) < MIN_LENGTH) {
// this direction is as good as any
q[0] = 1;
q[1] = 0;
q[2] = 0;
}
}
dNormalize3(q);
info->J1l[0] = q[0];
info->J1l[1] = q[1];
info->J1l[2] = q[2];
dVector3 relA1;
dBodyVectorToWorld(node[0].body,
anchor1[0], anchor1[1], anchor1[2],
relA1);
dMatrix3 a1m;
dSetZero(a1m, 12);
dSetCrossMatrixMinus(a1m, relA1, 4);
dMultiply1_331(info->J1a, a1m, q);
if (node[1].body) {
info->J2l[0] = -q[0];
info->J2l[1] = -q[1];
info->J2l[2] = -q[2];
dVector3 relA2;
dBodyVectorToWorld(node[1].body,
anchor2[0], anchor2[1], anchor2[2],
relA2);
dMatrix3 a2m;
dSetZero(a2m, 12);
dSetCrossMatrixPlus(a2m, relA2, 4);
dMultiply1_331(info->J2a, a2m, q);
}
const dReal k = info->fps * info->erp;
info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
}
