Calculating a matrix to have the model follow terrain

Question

So I've been messing around with 3D graphics in Java (using lwjgl), and I've hit a bit of a roadblock. I have a heightmap and a cube, and I want to find a way to have the cube rotate to follow the terrain smoothly. At the moment, the cube can rotate freely around the y axis (up is 0,1,0), and it can move directly in the direction it's facing.

Here's my idea: find the plane formed by projecting the bottom corners of the cube onto the terrain. That's the plane that I want to rotate my cube's bottom face into, so I took a cross product with the up vector and the normal vector of that plane. Then I took the angle between the two, and calculated an axis-angle rotation matrix around that plane. I mulitplied the y rotation matrix by that matrix, and used that as my model matrix:

    Matrix4 upTransform = Matrix4.createRotateY(yrot);
    // calculate plane of cube
    Vector3 bl = new Vector3();
    bl.x -= SCALE / 2;
    bl.z -= SCALE / 2;
    bl = upTransform.transform(bl);
    bl.y = game.getMap().getY(position.x + bl.x, position.z + bl.z);
    Vector3 tl = new Vector3();
    tl.x -= SCALE / 2;
    tl.z += SCALE / 2;
    tl = upTransform.transform(tl);
    tl.y = game.getMap().getY(position.x + tl.x, position.z + tl.z);
    Vector3 br = new Vector3();
    br.x += SCALE / 2;
    br.z -= SCALE / 2;
    br = upTransform.transform(br);
    br.y = game.getMap().getY(position.x + br.x, position.z + br.z);
    Vector3 normal = Vector3.cross(Vector3.diff(br, bl),
                                   Vector3.diff(tl, bl)).normalize();
    // calculate rotation axis
    Vector3 axis = Vector3.cross(normal, up).normalize();
    // calculate angle between plane of cube and plane of texture
    float angle = (float)Math.acos(Vector3.dot(up, normal));
    // calculate rotation
    Matrix4 terrainTransform = new Matrix4().setRotate(axis, angle);
    Matrix4 rot = Matrix4.multiply(upTransform, terrainTransform);

game.getMap().getY(x, z) returns the height of the map at the x,z coordinate provided. Vector3.diff(a, b) returns a - b, and Matrix4.multiply(A, B) returns A * B. My issue is that the cube rotates weirdly and has strange shear effects. Does anyone have comments on my general approach, or on my implementation? I'm really not sure what to do here, as this is pretty much where my linear algebra knowledge reaches its limits.

Answer 1

Solved my problem! I read an approach in XNA 3.0 Game Programming Recipes: A Problem-Solution Approach, and adapted it to my needs. Here's the code if anybody is curious:

    Matrix4 upTransform = Matrix4.createRotateY(yrot);
    // find the height at the front, back, left, and right
    Vector3 frontLeft = upTransform.transform(new Vector3(-SCALE / 2, 0, SCALE / 2))
                        .add(position);
    Vector3 frontRight = upTransform.transform(new Vector3(SCALE / 2, 0, SCALE / 2))
                        .add(position);
    Vector3 backLeft = upTransform.transform(new Vector3(-SCALE / 2, 0, -SCALE / 2))
                        .add(position);
    Vector3 backRight = upTransform.transform(new Vector3(SCALE / 2, 0, -SCALE / 2))
                        .add(position);
    // calculate front and back rotation
    Vector3 front = Vector3.sum(frontLeft, frontRight).scale(1 / 2.0f);
    Vector3 back = Vector3.sum(backLeft, backRight).scale(1 / 2.0f);
    Vector3 backToFront = Vector3.diff(front, back);
    float frontHeight = game.getMap().getY(front.x, front.z);
    float backHeight = game.getMap().getY(back.x, back.z);
    float frontBackDiff = frontHeight - backHeight;
    float frontBackAngle = (float)Math.atan2(frontBackDiff, backToFront.length());
    Matrix4 frontBackRot = Matrix4.createRotateX(frontBackAngle);
    // calculate left and right rotation
    Vector3 left = Vector3.sum(frontLeft, backLeft).scale(1 / 2.0f);
    Vector3 right = Vector3.sum(frontRight, backRight).scale(1 / 2.0f);
    Vector3 rightToLeft = Vector3.diff(right, left);
    float leftHeight = game.getMap().getY(left.x, left.z);
    float rightHeight = game.getMap().getY(right.x, right.z);
    float leftRightDiff = leftHeight - rightHeight;
    float leftRightAngle = (float)Math.atan2(leftRightDiff, rightToLeft.length());
    Matrix4 leftRightRot = Matrix4.createRotateZ(leftRightAngle);
    Matrix4 terrainTransform = Matrix4.multiply(frontBackRot, leftRightRot);
    Matrix4 rot = Matrix4.multiply(upTransform, terrainTransform);