예제 #1
0
void UniversalJoint::drawObject(const VisualizationParameterSet& visParams)
{
  if(visParams.surfaceStyle == VisualizationParameterSet::PHYSICS_WIREFRAME ||
     visParams.surfaceStyle == VisualizationParameterSet::PHYSICS_FLAT ||
     visParams.surfaceStyle == VisualizationParameterSet::PHYSICS_SMOOTH ||
     visParams.surfaceStyle == VisualizationParameterSet::MIXED_APPEARANCE ||
     visParams.surfaceStyle == VisualizationParameterSet::MIXED_PHYSICS)
  {
    dVector3 tempA;
    dJointGetUniversalAnchor(physicalJoint, tempA);
    Vector3d tempAnchor(tempA[0],tempA[1], tempA[2]);

    dJointGetUniversalAxis1(physicalJoint, tempA);
    Vector3d tempAxis1(tempA[0], tempA[1], tempA[2]);
    tempAxis1 *= 0.1;
    dJointGetUniversalAxis2(physicalJoint, tempA);
    Vector3d tempAxis2(tempA[0], tempA[1], tempA[2]);
    tempAxis2 *= 0.1;

    if(visParams.surfaceStyle == VisualizationParameterSet::MIXED_APPEARANCE)
    {
      glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
      glShadeModel(GL_FLAT);
      GLHelper::getGLH()->drawUniversalJoint(tempAnchor, tempAxis1, tempAxis2, true);
      glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
      glShadeModel(GL_SMOOTH);
    }
    else
    {
      GLHelper::getGLH()->drawUniversalJoint(tempAnchor, tempAxis1, tempAxis2, false);
    }
  }
}
예제 #2
0
파일: Joints.cpp 프로젝트: flair2005/inVRs
/**
 * This method is called if the joint should be detached.
 * It updates the new axis information since the axis can
 * change their orientation in UniversalJoints
 **/
void UniversalJoint::detachJoint() {
	TransformationData entityTrans;
	if (active && joint)
	{
		dVector3 vec;
		dJointGetUniversalAxis1(joint, vec);
		axis1[0] = vec[0];
		axis1[1] = vec[1];
		axis1[2] = vec[2];
		dJointGetUniversalAxis2(joint, vec);
		axis2[0] = vec[0];
		axis2[1] = vec[1];
		axis2[2] = vec[2];
		if (mainEntity != NULL)
		{
			entityTrans = mainEntity->getEnvironmentTransformation();
			gmtl::Quatf entityRot;
			gmtl::Vec3f scaleVec;
			gmtl::AxisAnglef axAng;

	// get the inverse scale values of the mainEntity
/*			scaleVec[0] = 1.0f/mainEntity->getXScale();
			scaleVec[1] = 1.0f/mainEntity->getYScale();
			scaleVec[2] = 1.0f/mainEntity->getZScale();*/
			scaleVec = entityTrans.scale;
	// get the inverse Rotation of the mainEntity
// 			axAng[0] = -mainEntity->getRotAngle();
// 			axAng[1] = mainEntity->getXRot();
// 			axAng[2] = mainEntity->getYRot();
// 			axAng[3] = mainEntity->getZRot();
// 			gmtl::set(entityRot, axAng);
			entityRot = entityTrans.orientation;
			gmtl::invert(entityRot);

			axis1 *= entityRot;
			axis2 *= entityRot;

			axis1[0] *= scaleVec[0];
			axis1[1] *= scaleVec[1];
			axis1[2] *= scaleVec[2];

			axis2[0] *= scaleVec[0];
			axis2[1] *= scaleVec[1];
			axis2[2] *= scaleVec[2];

			gmtl::normalize(axis1);
			gmtl::normalize(axis2);
		} // if
	} // if
} // detachJoint
예제 #3
0
파일: physics.c 프로젝트: ntoand/electro
static void get_phys_joint_axis_2(dJointID j, float *v)
{
    dVector3 V = { 0, 0, 0 };

    switch (dJointGetType(j))
    {
    case dJointTypeHinge2:    dJointGetHinge2Axis2   (j, V); break;
    case dJointTypeUniversal: dJointGetUniversalAxis2(j, V); break;
    default: break;
    }

    v[0] = (float) V[0];
    v[1] = (float) V[1];
    v[2] = (float) V[2];
}
예제 #4
0
void
dxJointUniversal::setRelativeValues()
{
    dVector3 anchor;
    dJointGetUniversalAnchor(this, anchor);
    setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );

    dVector3 ax1,ax2;
    dJointGetUniversalAxis1(this, ax1);
    dJointGetUniversalAxis2(this, ax2);

    if ( flags & dJOINT_REVERSE )
    {
        setAxes( this, ax1[0],ax1[1],ax1[2], NULL, axis2 );
        setAxes( this, ax2[0],ax2[1],ax2[2], axis1, NULL );
    }
    else
    {
        setAxes( this, ax1[0],ax1[1],ax1[2], axis1, NULL );
        setAxes( this, ax2[0],ax2[1],ax2[2], NULL, axis2 );
    }

    computeInitialRelativeRotations();
}
예제 #5
0
파일: Universal.c 프로젝트: RONNCC/pysoy
soyatomsAxis* soy_joints_universal_get_axis2 (soyjointsUniversal* self) {
	soyatomsAxis* result;
	dxVector3* _tmp0_;
	dxVector3* v;
	struct dxJoint* _tmp1_;
	dxVector3* _tmp2_;
	soyatomsAxis* _tmp3_;
	soyatomsAxis* _tmp4_;
	soyatomsAxis* value;
	gboolean _tmp5_ = FALSE;
	gboolean _tmp6_ = FALSE;
	gboolean _tmp7_ = FALSE;
	soyatomsAxis* _tmp8_;
	gboolean _tmp14_;
	gboolean _tmp20_;
	gboolean _tmp26_;
	g_return_val_if_fail (self != NULL, NULL);
	_tmp0_ = dvector3_new ();
	v = _tmp0_;
	_tmp1_ = ((soyjointsJoint*) self)->joint;
	_tmp2_ = v;
	dJointGetUniversalAxis2 ((struct dxJoint*) _tmp1_, _tmp2_);
	_tmp3_ = self->priv->_axis2_obj;
	_tmp4_ = _g_object_ref0 (_tmp3_);
	value = _tmp4_;
	_tmp8_ = value;
	if (_tmp8_ == NULL) {
		_tmp7_ = TRUE;
	} else {
		dxVector3* _tmp9_;
		dReal _tmp10_;
		soyatomsAxis* _tmp11_;
		gfloat _tmp12_;
		gfloat _tmp13_;
		_tmp9_ = v;
		_tmp10_ = _tmp9_->x;
		_tmp11_ = value;
		_tmp12_ = soy_atoms_axis_get_x (_tmp11_);
		_tmp13_ = _tmp12_;
		_tmp7_ = ((gfloat) _tmp10_) != _tmp13_;
	}
	_tmp14_ = _tmp7_;
	if (_tmp14_) {
		_tmp6_ = TRUE;
	} else {
		dxVector3* _tmp15_;
		dReal _tmp16_;
		soyatomsAxis* _tmp17_;
		gfloat _tmp18_;
		gfloat _tmp19_;
		_tmp15_ = v;
		_tmp16_ = _tmp15_->y;
		_tmp17_ = value;
		_tmp18_ = soy_atoms_axis_get_y (_tmp17_);
		_tmp19_ = _tmp18_;
		_tmp6_ = ((gfloat) _tmp16_) != _tmp19_;
	}
	_tmp20_ = _tmp6_;
	if (_tmp20_) {
		_tmp5_ = TRUE;
	} else {
		dxVector3* _tmp21_;
		dReal _tmp22_;
		soyatomsAxis* _tmp23_;
		gfloat _tmp24_;
		gfloat _tmp25_;
		_tmp21_ = v;
		_tmp22_ = _tmp21_->z;
		_tmp23_ = value;
		_tmp24_ = soy_atoms_axis_get_z (_tmp23_);
		_tmp25_ = _tmp24_;
		_tmp5_ = ((gfloat) _tmp22_) != _tmp25_;
	}
	_tmp26_ = _tmp5_;
	if (_tmp26_) {
		soyatomsAxis* _tmp27_;
		dxVector3* _tmp31_;
		dReal _tmp32_;
		dxVector3* _tmp33_;
		dReal _tmp34_;
		dxVector3* _tmp35_;
		dReal _tmp36_;
		soyatomsAxis* _tmp37_;
		soyatomsAxis* _tmp38_;
		soyatomsAxis* _tmp39_;
		soyatomsAxis* _tmp40_;
		_tmp27_ = value;
		if (_tmp27_ != NULL) {
			soyatomsAxis* _tmp28_;
			guint _tmp29_ = 0U;
			soyatomsAxis* _tmp30_;
			_tmp28_ = self->priv->_axis2_obj;
			g_signal_parse_name ("on-set", SOY_ATOMS_TYPE_AXIS, &_tmp29_, NULL, FALSE);
			g_signal_handlers_disconnect_matched (_tmp28_, G_SIGNAL_MATCH_ID | G_SIGNAL_MATCH_FUNC | G_SIGNAL_MATCH_DATA, _tmp29_, 0, NULL, (GCallback) __soy_joints_universal_axis2_set_soy_atoms_axis_on_set, self);
			_tmp30_ = self->priv->_axis2_obj;
			g_object_weak_unref ((GObject*) _tmp30_, __soy_joints_universal_axis2_weak_gweak_notify, self);
		}
		_tmp31_ = v;
		_tmp32_ = _tmp31_->x;
		_tmp33_ = v;
		_tmp34_ = _tmp33_->y;
		_tmp35_ = v;
		_tmp36_ = _tmp35_->z;
		_tmp37_ = soy_atoms_axis_new ((gfloat) _tmp32_, (gfloat) _tmp34_, (gfloat) _tmp36_);
		_g_object_unref0 (value);
		value = _tmp37_;
		_tmp38_ = value;
		g_signal_connect_object (_tmp38_, "on-set", (GCallback) __soy_joints_universal_axis2_set_soy_atoms_axis_on_set, self, 0);
		_tmp39_ = value;
		g_object_weak_ref ((GObject*) _tmp39_, __soy_joints_universal_axis2_weak_gweak_notify, self);
		_tmp40_ = value;
		self->priv->_axis2_obj = _tmp40_;
	}
	result = value;
	_dvector3_free0 (v);
	return result;
}
dReal doStuffAndGetError (int n)
{
  switch (n) {

  // ********** fixed joint

  case 0: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the orientations are the same
    const dReal *R1 = dBodyGetRotation (body[0]);
    const dReal *R2 = dBodyGetRotation (body[1]);
    dReal err1 = dMaxDifference (R1,R2,3,3);
    // check the body offset is correct
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return (err1 + length (pp)) * 300;
  }

  case 1: {			// 1 body to static env
    addOscillatingTorque (0.1);

    // check the orientation is the identity
    dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return (err1 + length (p)) * 1e6;
  }

  case 2: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the body offset is correct
    // Should really check body rotation too.  Oh well.
    const dReal *R1 = dBodyGetRotation (body[0]);
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return length(pp) * 300;
  }

  case 3: {			// 1 body to static env with relative rotation
    addOscillatingTorque (0.1);

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return  length (p) * 1e6;
  }


  // ********** hinge joint

  case 200:			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    return dInfinity;

  case 220:			// hinge angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHingeAngle (joint);
      if (a > 0.5 && a < 1) return 0; else return 10;
    }
    return 0;

  case 221: {			// hinge angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHingeAngle (joint);
    dReal r = dJointGetHingeAngleRate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er) * 4e4;
  }

  case 230:			// hinge motor rate (and polarity) test
  case 231: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHingeAngleRate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHingeParam (joint,dParamVel,cos(a));
    if (n==231) return dInfinity;
    return err * 1e6;
  }

  // ********** slider joint

  case 300:			// 2 body
    addOscillatingTorque (0.05);
    dampRotationalMotion (0.1);
    addSpringForce (0.5);
    return dInfinity;

  case 320:			// slider angle polarity test
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    if (iteration == 40) {
      dReal a = dJointGetSliderPosition (joint);
      if (a > 0.2 && a < 0.5) return 0; else return 10;
      return a;
    }
    return 0;

  case 321: {			// slider angle rate test
    static dReal last_pos = 0;
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    dReal p = dJointGetSliderPosition (joint);
    dReal r = dJointGetSliderPositionRate (joint);
    dReal er = (p-last_pos)/STEPSIZE;	// estimated rate (almost exact)
    last_pos = p;
    return fabs(r-er) * 1e9;
  }

  case 330:			// slider motor rate (and polarity) test
  case 331: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetSliderPositionRate (joint);
    dReal err = fabs (0.7*cos(a) - r);
    if (a < 0.04) err = 0;
    a += 0.03;
    dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
    if (n==331) return dInfinity;
    return err * 1e6;
  }

  // ********** hinge-2 joint

  case 420:			// hinge-2 steering angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHinge2Angle1 (joint);
      if (a > 0.5 && a < 0.6) return 0; else return 10;
    }
    return 0;

  case 421: {			// hinge-2 steering angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHinge2Angle1 (joint);
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er)*2e4;
  }

  case 430:			// hinge 2 steering motor rate (+polarity) test
  case 431: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel,cos(a));
    if (n==431) return dInfinity;
    return err * 1e6;
  }

  case 432: {			// hinge 2 wheel motor rate (+polarity) test
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle2Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel2,cos(a));
    return err * 1e6;
  }

  // ********** angular motor joint

  case 600: {			// test euler angle calculations
    // desired euler angles from last iteration
    static dReal a1,a2,a3;

    // find actual euler angles
    dReal aa1 = dJointGetAMotorAngle (joint,0);
    dReal aa2 = dJointGetAMotorAngle (joint,1);
    dReal aa3 = dJointGetAMotorAngle (joint,2);
    // printf ("actual  = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);

    dReal err = dInfinity;
    if (iteration > 0) {
      err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
      err *= 1e10;
    }

    // get random base rotation for both bodies
    dMatrix3 Rbase;
    dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
			3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
    dBodySetRotation (body[0],Rbase);

    // rotate body 2 by random euler angles w.r.t. body 1
    a1 = 3.14 * 2 * (dRandReal()-0.5);
    a2 = 1.57 * 2 * (dRandReal()-0.5);
    a3 = 3.14 * 2 * (dRandReal()-0.5);
    dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
    dRFromAxisAndAngle (R1,0,0,1,-a1);
    dRFromAxisAndAngle (R2,0,1,0,a2);
    dRFromAxisAndAngle (R3,1,0,0,-a3);
    dMultiply0 (Rtmp1,R2,R3,3,3,3);
    dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
    dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
    dBodySetRotation (body[1],Rtmp1);
    // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);

    return err;
  }

  // ********** universal joint

  case 700: {		// 2 body: joint constraint
    dVector3 ax1, ax2;

    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 701: {		// 2 body: angle 1 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 702: {		// 2 body: angle 2 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 720: {		// universal transmit torque test: constraint error
    dVector3 ax1, ax2;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 721: {		// universal transmit torque test: angle1 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 722: {		// universal transmit torque test: angle2 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 730:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 731:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 2e3;
  }

  case 732:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 740:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 741:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 742:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e4;
  }
  }

  return dInfinity;
}