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);
}
}
}
/**
* 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
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];
}
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();
}
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;
}
