I'm trying to align a vehicle to a heightmap, I spent the whole day reading lots of threads here and on other places, and while I've got almost near the solution, it still doest not work correctly.

My current solution is to align the vehicle with a rotation matrix constructed from pitch,yaw and roll angles.
The yaw is just the direction the player is heading, I first use a simple rotation around the Y with the current yaw to calculate the 4 wheel positions.
Then I calculate the terrain height at each wheel.
Finally the pitch and roll are calculated from the angles between wheels:
pitch: angle between back left wheel and front left wheel
roll: angle between back left wheel and right left wheel

The problem is that this works in some cases, in others the vehicle aligns correclty, but flipped (under the ground).
And in other cases, the angle seems to be negated, for example, it returns 11º when it should be -11º.

I guess the problem has something to do with the atan2 results and trignometry quadrants, and also the fact that I don't take into the account all of the wheels.
What is the perfect way to adjust a vehicle to a heightmap?

Here is my angle calculation function
float Kart::GetWheelAngle(int A, int B)
{
float U, V;
float X, Y;
U = (_Wheels->Position.X - _Wheels[A].Position->X);
V = (_Wheels->Position.Z - _Wheels[A].Position->Z);
if (Abs(U)>Abs(V))
X = U;
else
X = V;
Y = (_Wheels->Position.Y - _Wheels[A].Position.Y);

Result = atan2(Y, Abs(X));
}


And I know that using a physics engine would help with this, but want to avoid that, as this will be an iPhone game with very cartoony physics so I want to recreate them from scratch, avoiding the cpu overhead of a physics engine and the whole frustrating tuning that comes with it (actually I've got a version of this demo running with Newton, that I'm going to continue in case I can't get a solution for this).

Here is a pic of my current demo (in this screenshot the vehicle is correclty aligned using my code, but sadly it does not always work)

What if you just look at the terrain under the center of the vehicle and match your orientation to that? Take the heightmap and generate a normal map, then sample the derivative of the normal in the 'right' and 'backward' direction along the heightmap to get your right and back vectors, then take the cross product to get the normal (up) vector. Normalize and that should get you a coordinate frame to orient the vehicle with.

Or try doing a raycast straight down onto the geometry and get the orientation of the triangle you're above

If you want to sample at each wheel location you could do that and average the results to orient the whole car

Strangely I have tried that before, and it didnt look good, however, it was probably some error in my code, I tried it today, and yes, sampling the normal at the wheels works fine, the only problem is with movement, I need to smooth it out maybe.