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;
}
dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis,
dQuaternion q_initial )
{
// get qrel = relative rotation between the two bodies
dQuaternion qrel;
if ( body2 )
{
dQuaternion qq;
dQMultiply1( qq, body1->q, body2->q );
dQMultiply2( qrel, qq, q_initial );
}
else
{
// pretend body2->q is the identity
dQMultiply3( qrel, body1->q, q_initial );
}
return getHingeAngleFromRelativeQuat( qrel, axis );
}
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;
}
}
