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 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
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;
}
/// Compute initial relative rotation body1 -> body2, or env -> body1
void
dxJointScrew::computeInitialRelativeRotation()
{
if ( node[0].body )
{
if ( node[1].body )
{
dQMultiply1( qrel, node[0].body->q, node[1].body->q );
}
else
{
// set qrel to the transpose of the first body q
qrel[0] = node[0].body->q[0];
qrel[1] = -node[0].body->q[1];
qrel[2] = -node[0].body->q[2];
qrel[3] = -node[0].body->q[3];
}
}
}
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 );
}
// compute initial relative rotation body1 -> body2, or env -> body1
void
dxJointPR::computeInitialRelativeRotation()
{
if ( node[0].body )
{
if ( node[1].body )
{
dQMultiply1( qrel, node[0].body->q, node[1].body->q );
}
else
{
// set joint->qrel to the transpose of the first body q
qrel[0] = node[0].body->q[0];
for ( int i = 1; i < 4; i++ )
qrel[i] = -node[0].body->q[i];
// WARNING do we need the - in -joint->node[0].body->q[i]; or not
}
}
}
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");
}
/// Compute initial relative rotation body1 -> body2, or env -> body1
void
dxJointSlider::computeInitialRelativeRotation()
{
if ( node[0].body )
{
// compute initial relative rotation body1 -> body2, or env -> body1
// also compute center of body1 w.r.t body 2
if ( node[1].body )
{
dQMultiply1 ( qrel, node[0].body->q, node[1].body->q );
}
else
{
// set qrel to the transpose of the first body's q
qrel[0] = node[0].body->q[0];
qrel[1] = -node[0].body->q[1];
qrel[2] = -node[0].body->q[2];
qrel[3] = -node[0].body->q[3];
}
}
}
ManNiiPosture OculusDK2DeviceManager::convertQuaternion2ManNiiPosture(const SigCmn::Vector4 &quaternion)
{
ManNiiPosture posture;
dQuaternion tmpQ1;
tmpQ1[0] = +quaternion.w;
tmpQ1[1] = -quaternion.x;
tmpQ1[2] = +quaternion.y;
tmpQ1[3] = -quaternion.z;
dQuaternion tmpQ2;
//dQuaternion defaultQ = this->defaultHeadJointQuaternion;
dQMultiply1(tmpQ2, defaultHeadJoint0Quaternion, tmpQ1);
Quaternion tmpQ3(tmpQ2[0], tmpQ2[1], tmpQ2[2], tmpQ2[3]);
posture.joint[ManNiiPosture::HEAD_JOINT0].jointType = ManNiiPosture::HEAD_JOINT0;
posture.joint[ManNiiPosture::HEAD_JOINT0].quaternion = tmpQ3;
return posture;
}
void dJointSetGearboxReferenceBody2( dJointID j, dBodyID b )
{
dxJointGearbox* joint = dynamic_cast<dxJointGearbox*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT( b, "bad body argument" );
joint->refBody2 = b;
if ( joint->node[1].body )
{
if ( b )
dQMultiply1( joint->qrel2, joint->refBody2->q, joint->node[1].body->q );
else
{
// set qrel1 to the transpose of the first body q
joint->qrel2[0] = joint->node[1].body->q[0];
joint->qrel2[1] = joint->node[1].body->q[1];
joint->qrel2[2] = joint->node[1].body->q[2];
joint->qrel2[3] = joint->node[1].body->q[3];
}
}
else
{
if ( b )
{
// set qrel2 to the transpose of the second body q
joint->qrel2[0] = joint->refBody2->q[0];
joint->qrel2[1] = - joint->refBody2->q[1];
joint->qrel2[2] = - joint->refBody2->q[2];
joint->qrel2[3] = - joint->refBody2->q[3];
}
else
{
// both refBody2 and node[1].body are null, nothing happens
}
}
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
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];
}
}
