[ODE] A fix for the bendy wheels (with code)

[Michael Bailey]

Hello fellow wheel benders!

I've got a fix that corrects much of the wheel bending problem.  This little
fix makes my wheels 85% better and maybe with a little group effort we can
perfect the other %15 of this problem that's plagued so many of us.

Fist let me say, I'm convinced dBodySetFiniteRotationMode() and
dBodySetFiniteRotationAxis() are broken.  So if your trying to correct wheel
problems with these functions, don't bother!  (Feel free to prove me wrong
and post some code that works.)

Bellow you'll will find a small function dJointCorrectHinge2(), this
function can be placed at the bottom of joint.cpp, and the prototype can be
put in objects.h.  This function should be called each step and passed all
the car's wheel Hinge2 joints (so 4 calls per step for a four wheeled car).
It will correct the bent wheels by turning them back into the dParamLoStop
and dParamHiStop limits.

There are 3 ways wheels get out of whack, they are as follows:

1. The wheel develops un-wanted left/right turning along the Hinge2's axis
1.  This seems to be the worst of the wheel bending problem, and looks
*really* bad on rear wheels.  The below function corrects this issue.

2. The wheels position gets pushed away and toward the cars body, along the
wheels axle.  This effect is clear when you implement the below function.
I've experimented with a bunch of code to correct this problem, but had poor
results.  The problem is ODE doesn't store the original location of bodies,
so its hard to calculate the NON-pushed wheel position away from the car's
body.  Also, you still need to let the wheel move up and down, so an exact
offset from the car's body won't work.  Any ideas?

3. A very high speed, the wheels develop a "wobble" as if they are out of
balance.  This wobble is along the Hinge2's axis 2.  I think the below
function can be modified to correct his issue too, but first we may need a
dJointGetHinge2Angle2() function.  Anyone smart enough to tackle this issue?

Anyone up for working with me on further perfecting this little function?
Feel free to email me with comments or suggestions.

Russ - is this function worthy of being added to the next version of ODE?

Regards,
Mike
Radish Works

------------------------ Code below here ---------------------------

// Proto
void dJointCorrectHinge2(dJointID);

// Function
extern "C" void dJointCorrectHinge2(dxJointHinge2 *joint) {

	// A fix to correct wheel bending, it works by turning a hinge2's axis 1
	// back within the specified dParamLoStop and dParamHiStop limits.
	// Call this function in EVERY step with each car wheel hinge2 joint.
	// By: Mike Bailey - Radish Works (mike@radishworks.com)

	// Just checking for valid passed params
	dUASSERT(joint,"bad joint argument");
	dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");

	if(joint->node[1].body) {

		// Get the current turn angle of the joint
		dReal rAng=dJointGetHinge2Angle1(joint);

		// Get the high and low turning stops for the joint
		dReal rLoStop=dJointGetHinge2Param(joint,dParamLoStop);
		dReal rHiStop=dJointGetHinge2Param(joint,dParamHiStop);

		// Get the (transformed) axis the wheel left/right turn.
		dVector3 Axis;
		dJointGetHinge2Axis1(joint,Axis);

		// Calculate the angle the wheel has be turned PAST its limit.
		if(rAng<rLoStop) {
			rAng=rAng-rLoStop;
		} else if(rAng>rHiStop) {
			rAng=rAng-rHiStop;
		} else {
			rAng=0.0;
		}

		if(rAng) {
			// Here's the fix!
			// If there's a value in rAng, this angle is the angle PAST the wheel
stops
			// Create a matrix the that's the inverse of the turn angle we DON'T
want.
			dMatrix3 matODE;
			dRFromAxisAndAngle(matODE,Axis[0],Axis[1],Axis[2],rAng);

			// Multiply this matrix by the wheels body.
			// The wheel body is now in the range we want.
			// This works quite well on a cars un-turning rear wheels.
			dMultiply0(joint->node[1].body->R,matODE,joint->node[1].body->R,3,3,3);
		}
	}
}