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