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)); }