[ODE] Ball joint vs universal joint

[Brad]

I made a simple demo of a user mode ball joint with stops. In the demo I 
sat a simple system of two cubes connected via one of these ball joints 
next to one connected via a universal joint.  The axes and stops for 
these joints are the same, so in theory the joints should behave the 
same, right? But when the universal joint reaches a "corner" and comes 
to rest, the ball joint bounces and just doesn't look as good.

Is there something wrong with my code? Or is this a "feature?"

// code uses the drawstuff thingy included with ODE
#define dSINGLE

#include <iostream>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#define GRAVITY -1
#define STEPSIZE 0.01
#define SIDE 0.5

//#define USE_EULER_AMOTOR

dWorldID myWorld;
dGeomID myFloor;

dBodyID myBase;
dBodyID myHead;

dBodyID myBase2;
dBodyID myHead2;

dJointID myBallJoint;
dJointID myFixedJoint;
dJointID myAngularMotor;

dJointID myFixedJoint2;
dJointID myUniJoint;

void setupTest()
{
    myWorld = dWorldCreate();
    dWorldSetERP(myWorld, 0.2f);
    dWorldSetCFM(myWorld, 0.0001);
    dWorldSetGravity(myWorld, 0, 0, GRAVITY);


    // create the bodies
    myBase = dBodyCreate(myWorld);
    myHead = dBodyCreate(myWorld);

    myBase2 = dBodyCreate(myWorld);
    myHead2 = dBodyCreate(myWorld);

    dBodySetPosition(myBase, 0, 0, 0.25);
    dBodySetPosition(myHead, 0, 0, 3);

    dBodySetPosition(myBase2, 2, 0, 0.25);
    dBodySetPosition(myHead2, 2, 0, 3);

    // create the joints
    myBallJoint = dJointCreateBall(myWorld, 0);
    myFixedJoint = dJointCreateFixed(myWorld, 0);
    myAngularMotor = dJointCreateAMotor(myWorld, 0);

    myUniJoint = dJointCreateUniversal(myWorld, 0);
    myFixedJoint2 = dJointCreateFixed(myWorld, 0);


    dJointAttach(myBallJoint, myBase, myHead);
    dJointAttach(myFixedJoint, myBase, 0);
    dJointAttach(myAngularMotor, myBase, myHead);

    dJointAttach(myUniJoint, myBase2, myHead2);
    dJointAttach(myFixedJoint2, myBase2, 0);

    dJointSetBallAnchor(myBallJoint, 0, 0, 2);
    dJointSetUniversalAnchor(myUniJoint, 2, 0, 2);

    dJointSetFixed(myFixedJoint);
    dJointSetFixed(myFixedJoint2);

    dJointSetUniversalAxis1(myUniJoint, 1, 0, 0);
    dJointSetUniversalAxis2(myUniJoint, 0, 1, 0);

    dJointSetUniversalParam(myUniJoint, dParamHiStop, 1);
    dJointSetUniversalParam(myUniJoint, dParamLoStop, -1);
    dJointSetUniversalParam(myUniJoint, dParamHiStop2, 1);
    dJointSetUniversalParam(myUniJoint, dParamLoStop2, -1);

    // setup the angular motor

#ifdef USE_EULER_AMOTOR
    dJointSetAMotorMode(myAngularMotor, dAMotorEuler);

    dJointSetAMotorAxis(myAngularMotor, 0, 1, 1, 0, 0);
    //dJointSetAMotorAxis(myAngularMotor, 1, 0, 0, 1, 0);
    dJointSetAMotorAxis(myAngularMotor, 2, 2, 0, 0, 1);

#else
    dJointSetAMotorNumAxes(myAngularMotor, 3);
    dJointSetAMotorAxis(myAngularMotor, 0, 0, 1, 0, 0);
    dJointSetAMotorAxis(myAngularMotor, 1, 0, 0, 1, 0);
    dJointSetAMotorAxis(myAngularMotor, 2, 2, 0, 0, 1);

    dJointSetAMotorAngle(myAngularMotor, 0, 0);
    dJointSetAMotorAngle(myAngularMotor, 1, 0);
    dJointSetAMotorAngle(myAngularMotor, 2, 0);
#endif

    // create a stop
    dJointSetAMotorParam(myAngularMotor, dParamHiStop, 1);
    dJointSetAMotorParam(myAngularMotor, dParamLoStop, -1);
    dJointSetAMotorParam(myAngularMotor, dParamHiStop, 1);
    dJointSetAMotorParam(myAngularMotor, dParamLoStop, -1);

    dJointSetAMotorParam(myAngularMotor, dParamHiStop2, 1);
    dJointSetAMotorParam(myAngularMotor, dParamLoStop2, -1);
    dJointSetAMotorParam(myAngularMotor, dParamHiStop2, 1);
    dJointSetAMotorParam(myAngularMotor, dParamLoStop2, -1);

    dJointSetAMotorParam(myAngularMotor, dParamHiStop3, 0);
    dJointSetAMotorParam(myAngularMotor, dParamLoStop3, 0);
    dJointSetAMotorParam(myAngularMotor, dParamHiStop3, 0);
    dJointSetAMotorParam(myAngularMotor, dParamLoStop3, 0);


}

static void start()
{
    static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
    static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
    dsSetViewpoint (xyz,hpr);
}

static void command (int cmd)
{
    switch  (cmd)
    {
        case 'a': case 'A':
            dJointAddAMotorTorques(myAngularMotor, 10, 0, 0);
            dJointAddUniversalTorques(myUniJoint, 10, 0);
            break;
        case 'd': case 'D':
            dJointAddAMotorTorques(myAngularMotor, -10, 0, 0);
            dJointAddUniversalTorques(myUniJoint, -10, 0);
            break;
        case 'w': case 'W':
            dBodyAddRelTorque(myHead, 0, 0, 10);
           
            break;
        case 's': case 'S':
            dBodyAddRelTorque(myHead, 0, 0, -10);
           
            break;
        case 'z': case 'Z':
            dJointAddAMotorTorques(myAngularMotor, 0, 10, 0);
            dJointAddUniversalTorques(myUniJoint, 0, 10);
            break;
        case 'x': case 'X':
            dJointAddAMotorTorques(myAngularMotor, 0, -10, 0);
            dJointAddUniversalTorques(myUniJoint, 0, -10);
           
    }
}

static void simLoop (int pause)
{

    // take a step
    dWorldStep (myWorld,STEPSIZE);

    // draw!
    dsSetTexture(DS_WOOD);
    dsSetColor(1,1,0);

    dReal sides1[3] = {SIDE,SIDE,SIDE};
    dsDrawBox(dBodyGetPosition(myBase), dBodyGetRotation(myBase), sides1);
    dsDrawBox(dBodyGetPosition(myHead), dBodyGetRotation(myHead), sides1);

    dsDrawBox(dBodyGetPosition(myBase2), dBodyGetRotation(myBase2), sides1);
    dsDrawBox(dBodyGetPosition(myHead2), dBodyGetRotation(myHead2), sides1);

    // draw the joints
    dsSetTexture(DS_NONE);
    dsSetColor(0, 0, 1);
    float pos1[3] = {0,0,0};
    float pos2[3] = {0, 0, 2};
    const dReal *bpos = dBodyGetPosition(myHead);
    float pos3[3];
    pos3[0] = bpos[0]; pos3[1] = bpos[1]; pos3[2] = bpos[2];
    dsDrawLine(pos1, pos2);
    dsDrawLine(pos2, pos3);

    // calculate the angles for the angular motor
    const dReal * bpos1 = dBodyGetPosition(myHead);

    // relative to the ball joint anchor
    dVector3 bpos2;
    bpos2[0] = 0;
    bpos2[1] = 0;
    bpos2[2] = 2;

    // first axis (X)
    float rx = bpos1[1] - bpos2[1];
    float ry = bpos1[2] - bpos2[2];

    float restx = 0;
    float resty = 2;

    float angle = atan(rx / ry);

    dJointSetAMotorAngle(myAngularMotor, 0, angle);

    // Y axis
    rx = bpos1[0] - bpos2[0];
    ry = bpos1[2] - bpos2[2];

    angle = atan(rx / ry);

    dJointSetAMotorAngle(myAngularMotor, 1, -angle);
   
}

void doTest (int argc, char **argv)
{
   
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = 0;
    fn.path_to_textures = "./";

    // run simulation
    dsSimulationLoop (argc,argv,352,288,&fn);
 
    dWorldDestroy (myWorld);
 
}

int main(int argc, char* argv[])
{
    setupTest();
    doTest(argc, argv);

    return 0;
}