void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal bx, dReal by, dReal bz)
{
dReal l,k;
dAASSERT (R);
l = dSqrt (ax*ax + ay*ay + az*az);
if (l <= REAL(0.0)) {
dDEBUGMSG ("zero length vector");
memset(R, 0, sizeof(dReal)*12);
return;
}
l = dRecip(l);
ax *= l;
ay *= l;
az *= l;
k = ax*bx + ay*by + az*bz;
bx -= k*ax;
by -= k*ay;
bz -= k*az;
l = dSqrt (bx*bx + by*by + bz*bz);
if (l <= REAL(0.0)) {
memset(R, 0, sizeof(dReal)*12);
dDEBUGMSG ("zero length vector");
return;
}
l = dRecip(l);
bx *= l;
by *= l;
bz *= l;
_R(0,0) = ax;
_R(1,0) = ay;
_R(2,0) = az;
_R(0,1) = bx;
_R(1,1) = by;
_R(2,1) = bz;
_R(0,2) = - by*az + ay*bz;
_R(1,2) = - bz*ax + az*bx;
_R(2,2) = - bx*ay + ax*by;
_R(0,3) = REAL(0.0);
_R(1,3) = REAL(0.0);
_R(2,3) = REAL(0.0);
}
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 dNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dDOT(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
}
else {
dDEBUGMSG ("vector has zero size");
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
}
}
