void setFixedOrientation( dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row )
{
int s = info->rowskip;
int start_index = start_row * s;
// 3 rows to make body rotations equal
info->J1a[start_index] = 1;
info->J1a[start_index + s + 1] = 1;
info->J1a[start_index + s*2+2] = 1;
if ( joint->node[1].body )
{
info->J2a[start_index] = -1;
info->J2a[start_index + s+1] = -1;
info->J2a[start_index + s*2+2] = -1;
}
// compute the right hand side. the first three elements will result in
// relative angular velocity of the two bodies - this is set to bring them
// back into alignment. the correcting angular velocity is
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * u
// = (erp*fps) * theta * u
// where rotation along unit length axis u by theta brings body 2's frame
// to qrel with respect to body 1's frame. using a small angle approximation
// for sin(), this gives
// angular_velocity = (erp*fps) * 2 * v
// where the quaternion of the relative rotation between the two bodies is
// q = [cos(theta/2) sin(theta/2)*u] = [s v]
// get qerr = relative rotation (rotation error) between two bodies
dQuaternion qerr, e;
if ( joint->node[1].body )
{
dQuaternion qq;
dQMultiply1( qq, joint->node[0].body->q, joint->node[1].body->q );
dQMultiply2( qerr, qq, qrel );
}
else
{
dQMultiply3( qerr, joint->node[0].body->q, qrel );
}
if ( qerr[0] < 0 )
{
qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
qerr[2] = -qerr[2];
qerr[3] = -qerr[3];
}
dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
dReal k = info->fps * info->erp;
info->c[start_row] = 2 * k * e[0];
info->c[start_row+1] = 2 * k * e[1];
info->c[start_row+2] = 2 * k * e[2];
}
dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis,
dQuaternion q_initial )
{
// get qrel = relative rotation between the two bodies
dQuaternion qrel;
if ( body1 && body2 )
{
dQuaternion qq;
dQMultiply1( qq, body1->q, body2->q );
dQMultiply2( qrel, qq, q_initial );
}
else if (body1)
{
// pretend body2->q is the identity
dQMultiply3( qrel, body1->q, q_initial );
}
else if (body2)
{
// pretend body1->q is the identity
dQMultiply3( qrel, body2->q, q_initial );
}
return getHingeAngleFromRelativeQuat( qrel, axis );
}
void dJointSetScrewAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
setAxes( joint, x, y, z, joint->axis1, joint->axis2 );
joint->computeInitialRelativeRotation();
if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle;
dQuaternion qAngle, qOffset;
dQFromAxisAndAngle(qAngle, x, y, z, dangle);
dQMultiply3(qOffset, qAngle, joint->qrel);
joint->qrel[0] = qOffset[0];
joint->qrel[1] = qOffset[1];
joint->qrel[2] = qOffset[2];
joint->qrel[3] = qOffset[3];
}
void testQuaternionMultiply()
{
HEADER;
dMatrix3 RA,RB,RC,Rtest;
dQuaternion qa,qb,qc;
dReal diff,maxdiff=0;
for (int i=0; i<100; i++) {
makeRandomRotation (RB);
makeRandomRotation (RC);
dRtoQ (RB,qb);
dRtoQ (RC,qc);
dMultiply0 (RA,RB,RC,3,3,3);
dQMultiply0 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply1 (RA,RB,RC,3,3,3);
dQMultiply1 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply2 (RA,RB,RC,3,3,3);
dQMultiply2 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply0 (RA,RC,RB,3,3,3);
transpose3x3 (RA);
dQMultiply3 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
