HELP! 3d rotations, quaternions and stuff

[xDan]

I'm going insane here I've wasted my weekend on this

A want to make an angular spring in a physics engine. This is applied to a body floating in space, in order to maintain a specific orientation.

- So I have the current rotation of the physics body, as vector3d euler angles (from a matrix4.getRotationDegrees())
- And I have the desired rotation, also as eulers.

I want to apply the correct torque forces (about X,Y,Z) to rotate the object from its current rotation to the desired rotation. Following the shortest possible path.

I've been trying various stuff... I tried just subtracting the euler angles... but then you have to do things with fmod() and if (angle < -180) angle += 360 sort of special case things. It did sort of work, but odd things were happening at some angles... to do with gimbal lock? I don't know. I could post the code I used for that, but hopefully there is a better way to do it.

I've also been trying to use quaternions too... This for example:

Code: Select all

    core::vector3df torque;
    
    // make quaterions from the euler angles in degrees
    core::quaternion qCurrent(currentRot);
    core::quaternion qDesired(desiredRot);
    
    // supposed to get the difference between the rotations?
    qCurrent.makeInverse();
    core::quaternion delta = qCurrent * qDesired;
    
    delta.toEuler(torque);
    
    torque *= springConstant;
    
    dBodyAddTorque(body, torque.X,torque.Y,torque.Z);

Bleh. That doesn't seem to do *anything*, except the object twitches occasionally. (btw I have no real understanding of quaternions or anything, I was just following snippets found through google)

I also tried using the quaternion.rotationFromTo() function:

Code: Select all

    core::vector3df torque;
    
    core::vector3df cr(1,0,0);
    core::vector3df dr(1,0,0);
    
    core::matrix4 m1,m2;
    
    m1.setRotationDegrees(currentRot);
    m1.rotateVect(cr);
    
    m2.setRotationDegrees(desiredRot);
    m2.rotateVect(dr);
    
    core::quaternion delta;
    delta.rotationFromTo(cr,dr);
    delta.toEuler(torque);
    
	torque *= springConstant*100.0;
	dBodyAddTorque(body, torque.X,torque.Y,torque.Z);

Once again, this *sort of* works. The body maintains the correct rotation about the Y axis. But the other axis seem unchanged (or are maintaining incorrect rotations... I'm not sure)

Does anyone know some completely fool proof method of doing this? That is guaranteed to work? So that when it doesn't I know it can only be my fault...

[xDan]

You know what the problem was? CONTRARY TO STANDARD BEHAVIOUR, quaternions take the euler angles in RADIANS, not degrees (which I assumed since scene nodes etc.. use degrees).

I spent HOURS on this.

So for the record this works:

Code: Select all

    core::matrix4 mCurrent;
    ODEGetBodyTransformation(body, mCurrent);
    
    core::quaternion qCurrent(core::DEGTORAD * mCurrent.getRotationDegrees());
    core::quaternion qDesired(core::DEGTORAD * desiredRot);
    
    core::vector3df torque;
    
    qCurrent.makeInverse();
    core::quaternion delta = qCurrent * qDesired;
    delta.toEuler(torque);
    
    torque *= springConstant;
    
    // Damping
    const dReal *rVel = dBodyGetAngularVel(body);
    torque.X -= rVel[0] * springDamping;
    torque.Y -= rVel[1] * springDamping;
    torque.Z -= rVel[2] * springDamping;
    
    dBodyAddTorque(body, torque.X,torque.Y,torque.Z);