void
dxJointUniversal::computeInitialRelativeRotations()
{
if ( node[0].body )
{
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross;
getAxes( ax1, ax2 );
// Axis 1.
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
dRtoQ( R, qcross );
dQMultiply1( qrel1, node[0].body->q, qcross );
// Axis 2.
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
dRtoQ( R, qcross );
if ( node[1].body )
{
dQMultiply1( qrel2, node[1].body->q, qcross );
}
else
{
// set joint->qrel to qcross
for ( int i = 0; i < 4; i++ ) qrel2[i] = qcross[i];
}
}
}
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");
}
void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result)
{
dAASSERT (g);
if (g->offset_posr)
{
dRtoQ (g->offset_posr->R, result);
}
else
{
dSetZero (result,4);
result[0] = 1;
}
}
void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->body) {
const dReal * body_quat = dBodyGetQuaternion (g->body);
quat[0] = body_quat[0];
quat[1] = body_quat[1];
quat[2] = body_quat[2];
quat[3] = body_quat[3];
}
else {
dRtoQ (g->R, quat);
}
}
void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->body && !g->offset_posr) {
const dReal * body_quat = dBodyGetQuaternion (g->body);
quat[0] = body_quat[0];
quat[1] = body_quat[1];
quat[2] = body_quat[2];
quat[3] = body_quat[3];
}
else {
g->recomputePosr();
dRtoQ (g->final_posr->R, quat);
}
}
dReal
dxJointUniversal::getAngle2()
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
dRtoQ( R, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross, qrel2 );
}
return - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
return 0;
}
void testRtoQandQtoR()
{
HEADER;
dMatrix3 R,I,R2;
dQuaternion q;
int i;
// test makeRandomRotation()
makeRandomRotation (R);
dMultiply2 (I,R,R,3,3,3);
printf ("\tmakeRandomRotation() - %s (1)\n",
cmpIdentityMat3(I) ? "passed" : "FAILED");
// test QtoR() on random normalized quaternions
int ok = 1;
for (i=0; i<100; i++) {
dMakeRandomVector (q,4,1.0);
dNormalize4 (q);
dQtoR (q,R);
dMultiply2 (I,R,R,3,3,3);
if (cmpIdentityMat3(I)==0) ok = 0;
}
printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");
// test R -> Q -> R works
dReal maxdiff=0;
for (i=0; i<100; i++) {
makeRandomRotation (R);
dRtoQ (R,q);
dQtoR (q,R2);
dReal diff = dMaxDifference (R,R2,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
void
dxJointUniversal::getAngles( dReal *angle1, dReal *angle2 )
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
dRtoQ( R, qcross );
// This code is essentialy the same as getHingeAngle(), see the comments
// there for details.
// get qrel = relative rotation between node[0] and the cross
dQMultiply1( qq, node[0].body->q, qcross );
dQMultiply2( qrel, qq, qrel1 );
*angle1 = getHingeAngleFromRelativeQuat( qrel, axis1 );
// This is equivalent to
// dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
// You see that the R is constructed from the same 2 axis as for angle1
// but the first and second axis are swapped.
// So we can take the first R and rapply a rotation to it.
// The rotation is around the axis between the 2 axes (ax1 and ax2).
// We do a rotation of 180deg.
dQuaternion qcross2;
// Find the vector between ax1 and ax2 (i.e. in the middle)
// We need to turn around this vector by 180deg
// The 2 axes should be normalize so to find the vector between the 2.
// Add and devide by 2 then normalize or simply normalize
// ax2
// ^
// |
// |
/// *------------> ax1
// We want the vector a 45deg
//
// N.B. We don't need to normalize the ax1 and ax2 since there are
// normalized when we set them.
// We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z]
qrel[0] = 0; // equivalent to cos(Pi/2)
qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1
qrel[2] = ax1[1] + ax2[1];
qrel[3] = ax1[2] + ax2[2];
dReal l = dRecip( sqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] ) );
qrel[1] *= l;
qrel[2] *= l;
qrel[3] *= l;
dQMultiply0( qcross2, qrel, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross2 );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross2, qrel2 );
}
*angle2 = - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
else
{
*angle1 = 0;
*angle2 = 0;
}
}
void dJointSetUniversalAxis2Offset( dJointID j, dReal x, dReal y, dReal z,
dReal offset1, dReal offset2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
setAxes( joint, x, y, z, joint->axis1, NULL );
offset1 = -offset2;
offset2 = -offset1;
}
else
setAxes( joint, x, y, z, NULL, joint->axis2 );
joint->computeInitialRelativeRotations();
// It is easier to retreive the 2 axes here since
// when there is only one body B2 (the axes switch position)
// Doing this way eliminate the need to write the code differently
// for both case.
dVector3 ax1, ax2;
joint->getAxes(ax1, ax2 );
dQuaternion qAngle;
dQFromAxisAndAngle(qAngle, ax1[0], ax1[1], ax1[2], offset1);
dMatrix3 R;
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
dQuaternion qcross;
dRtoQ( R, qcross );
dQuaternion qOffset;
dQMultiply0(qOffset, qAngle, qcross);
dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
// Calculating the second offset
dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
dRtoQ( R, qcross );
dQMultiply1(qOffset, qAngle, qcross);
if ( joint->node[1].body )
{
dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
}
else
{
joint->qrel2[0] = qcross[0];
joint->qrel2[1] = qcross[1];
joint->qrel2[2] = qcross[2];
joint->qrel2[3] = qcross[3];
}
}
void dJointSetUniversalAxis1Offset( dJointID j, dReal x, dReal y, dReal z,
dReal offset1, dReal offset2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
setAxes( joint, x, y, z, NULL, joint->axis2 );
offset1 = -offset1;
offset2 = -offset2;
}
else
setAxes( joint, x, y, z, joint->axis1, NULL );
joint->computeInitialRelativeRotations();
dVector3 ax2;
getAxis2( joint, ax2, joint->axis2 );
{
dVector3 ax1;
joint->getAxes(ax1, ax2);
}
dQuaternion qAngle;
dQFromAxisAndAngle(qAngle, x, y, z, offset1);
dMatrix3 R;
dRFrom2Axes( R, x, y, z, ax2[0], ax2[1], ax2[2] );
dQuaternion qcross;
dRtoQ( R, qcross );
dQuaternion qOffset;
dQMultiply0(qOffset, qAngle, qcross);
dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
// Calculating the second offset
dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], x, y, z );
dRtoQ( R, qcross );
dQMultiply1(qOffset, qAngle, qcross);
if ( joint->node[1].body )
{
dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
}
else
{
joint->qrel2[0] = qcross[0];
joint->qrel2[1] = qcross[1];
joint->qrel2[2] = qcross[2];
joint->qrel2[3] = qcross[3];
}
}
