How does Terrain following work on height map?

Question

I'm writing a 3D world game which consists of a terrain map (generated by brownian motion algorithm) and a simple avatar. My goal is to make the avatar move smoothly between points of different height. Originally, I simply translate from start location to stop location, then set the height of this point based on the height value of a pair (x, z). Unfortunately the movement is completely off. I searched online I found several algorithms to create a smooth movement but I was not able to put them together. I just started to lean graphics for a month, so sorry for my dull knowledge.

My height map size is 512 * 512, and the spacing between each vertex is 150. Base on these values, I create a square around my destination as follows:

/*
                        * A            B
                        * --------------
                        * |            |
                        * |    x       |
                        * |            |
                        * |            |
                        * --------------
                        * C            D
                        */

                // build square ABCD around stopLocation
                int x = (int)stopLocation.X / 150;
                int z = (int)stopLocation.Z / 150;

                Vector3 A = new Vector3(x * 150, stage.surfaceHeight(x * 150, z * 150), z * 150);
                Vector3 B = new Vector3((x + 1) * 150, stage.surfaceHeight((x + 1) * 150, z * 150), z * 150);
                Vector3 C = new Vector3(x * 150, stage.surfaceHeight(x * 150, (z + 1) * 150), (z + 1) * 150);
                Vector3 D = new Vector3((x + 1) * 150, stage.surfaceHeight((x + 1) * 150, (z + 1) * 150), (z + 1) * 150);

I use a simple algorithm to determine my destination plane:

public bool isInUpperPlane(Vector3 A, Vector3 B, Vector3 C, Vector3 D, Vector3 point) {

            /*
                   ^ y
                   |
                   |
                   |
                   |  A       B
                   ----------------------------> x
                  /          /
                 /      x   /
              C /_________ / D
               /
              /
             /
            v z
            */

            // consider A as the origin
            if (Math.Abs(point.X - A.X) + Math.Abs(point.Z - A.Z) >= 150)
                return true;
            return false;
        }

Next I construct a plane the 3 vertices (either ABC or BCD):

     Plane destPlane;
     if (isInUpperPlane(A, B, C, D, stopLocation)) {
         destPlane = new Plane(A, B, C);
     }
     else {
         destPlane = new Plane(B, C, D);
     }

Then I project this my start location to this plane normal, and translate my avatar to this location:

Vector3 normal = destPlane.Normal;
Vector3 r = projectVectorToPlane(startLocation, normal);
Orientation *= Matrix.CreateTranslation(startLocation);

But doing this make my avatar stays still. It's very very wrong, but I really don't know how it actually works. The 3D world is just too confusing at this moment. Could anyone help me explain what's go wrong? An minimal working example would be greatly appreciated.

This is my original implementation, (not smooth following)

public void updateMovableObject() {
            Vector3 startLocation = Translation;
            Vector3 stopLocation = Translation;

            Orientation *= Matrix.CreateTranslation(-1 * Translation);        // move to origin
            Orientation *= Matrix.CreateRotationY(yaw);                       // rotate
            Orientation *= Matrix.CreateRotationX(pitch);
            Orientation *= Matrix.CreateRotationZ(roll);

            stopLocation += ((step * stepSize) * Forward);                    // move forward    

            // if collision, reset location and return
            if (model.IsCollidable && collision(stopLocation)) {
                Orientation *= Matrix.CreateTranslation(startLocation);        // don't move
                return;
            }

            // no collision test if move on terrain
            if (stage.withinRange(this.Name, stopLocation)) {
                Orientation *= Matrix.CreateTranslation(stopLocation);         // move forward
            }
            else { // off terrain, reset location
                Orientation *= Matrix.CreateTranslation(startLocation);        // don't move
            }
        }
        stage.setSurfaceHeight(this);


 public void setSurfaceHeight(Object3D anObject3D) {
            float terrainHeight = terrain.surfaceHeight((int)(anObject3D.Translation.X / spacing), (int)(anObject3D.Translation.Z / spacing));
            anObject3D.Translation = new Vector3(anObject3D.Translation.X, terrainHeight, anObject3D.Translation.Z);
        }

EDIT

 public float surfaceHeight(int x, int z) {
            if (x < 0 || x > 511 || z < 0 || z > 511) 
                return 0.0f;  // index valid ?
            return (float)terrainHeight[x, z];
        }

Answer 1

What you have seems like a lot of work to put your avatar on the terrain at a given point. Here's the method I use to calculate terrain height. Simply pass in your X and Z coordinates and it will return the terrain height. In this case, the scaleFactor you pass in should be 150. Given something like this you just pick two pairs of X,Z coordinates, get the terrain height at each, and set these points as your start and end points, and then call this method each frame to make sure the avatar stays on (or at least above) the terrain.

/// <summary>
/// Get the height of the terrain at given horizontal coordinates.
/// </summary>
/// <param name="xPos">X coordinate</param>
/// <param name="zPos">Z coordinate</param>
/// <returns>Height at given coordinates</returns>
public float GetTerrainHeight(float xPos, float zPos, float scaleFactor)
{
    // we first get the height of four points of the quad underneath the point
    // Check to make sure this point is not off the map at all
    int x = (int)(xPos / scaleFactor);      
    int z = (int)(zPos / scaleFactor);      

    int xPlusOne = x + 1;
    int zPlusOne = z + 1;

    float triZ0 = (this.heightData[x, z]);
    float triZ1 = (this.heightData[xPlusOne, z]);
    float triZ2 = (this.heightData[x, zPlusOne]);
    float triZ3 = (this.heightData[xPlusOne, zPlusOne]);

    float height = 0.0f;
    float sqX = (xPos / scaleFactor) - x;
    float sqZ = (zPos / scaleFactor) - z;
    if ((sqX + sqZ) < 1)
    {
        height = triZ0;
        height += (triZ1 - triZ0) * sqX;
        height += (triZ2 - triZ0) * sqZ;
    }
    else
    {
        height = triZ3;
        height += (triZ1 - triZ3) * (1.0f - sqZ);
        height += (triZ2 - triZ3) * (1.0f - sqX);
    }
    return height;
}

EDIT:

Here's the code to calculate the surface normal at any point on the terrain.

/// <summary>
/// Setup <see cref="Terrain"/> normals. Normals are used for lighting, normal mapping, and physics with terrain.
/// </summary>
private void SetupTerrainNormals(float scaleFactor)
{
    VertexTerrain[] terrainVertices = new VertexTerrain[this.size * this.size];
    this.normals = new Vector3[this.size, this.size];

    // Determine vertex positions so we can figure out normals in section below.
    for(int x = 0; x < this.size; ++x)
        for(int z = 0; z < this.size; ++z)
        {
            terrainVertices[x + z * this.size].Position = new Vector3(x * scaleFactor, this.heightData[x, z], z * scaleFactor);
        }

    // Setup normals for lighting and physics (Credit: Riemer's method)
    int sizeMinusOne = this.size - 1;
    for (int x = 1; x < sizeMinusOne; ++x)
        for (int z = 1; z < sizeMinusOne; ++z)
        {
            int ZTimesSize = (z * this.size);
            Vector3 normX = new Vector3((terrainVertices[x - 1 + ZTimesSize].Position.Y - terrainVertices[x + 1 + ZTimesSize].Position.Y) / 2, 1, 0);
            Vector3 normZ = new Vector3(0, 1, (terrainVertices[x + (z - 1) * this.size].Position.Y - terrainVertices[x + (z + 1) * this.size].Position.Y) / 2);

            // We inline the normalize method here since it is used alot, this is faster than calling Vector3.Normalize()
            Vector3 normal = normX + normZ;
            float length = (float)Math.Sqrt( (float)((normal.X * normal.X) + (normal.Y * normal.Y) + (normal.Z * normal.Z)) );
            float num = 1f / length;
            normal.X *= num;
            normal.Y *= num;
            normal.Z *= num;

            this.normals[x, z] = terrainVertices[x + ZTimesSize].Normal = normal;    // Stored for use in physics and for the
                                                                                     // quad-tree component to reference.
        }
}

And here's the code to retrieve the normal at any point.

/// <summary>
/// Gets the normal of a position on the heightmap.
/// </summary>
/// <param name="xPos">X position on the map</param>
/// <param name="zPos">Z position on the map</param>
/// <returns>Normal vector of this spot on the terrain</returns>
public Vector3 GetNormal(float xPos, float zPos, float scaleFactor)
{
    int x = (int)(xPos / scaleFactor);
    int z = (int)(zPos / scaleFactor);

    int xPlusOne = x + 1;
    int zPlusOne = z + 1;

    Vector3 triZ0 = (this.normals[x, z]);
    Vector3 triZ1 = (this.normals[xPlusOne, z]);
    Vector3 triZ2 = (this.normals[x, zPlusOne]);
    Vector3 triZ3 = (this.normals[xPlusOne, zPlusOne]);

    Vector3 avgNormal;
    float sqX = (xPos / scaleFactor) - x;
    float sqZ = (zPos / scaleFactor) - z;
    if ((sqX + sqZ) < 1)
    {
        avgNormal = triZ0;
        avgNormal += (triZ1 - triZ0) * sqX;
        avgNormal += (triZ2 - triZ0) * sqZ;
    }
    else
    {
        avgNormal = triZ3;
        avgNormal += (triZ1 - triZ3) * (1.0f - sqZ);
        avgNormal += (triZ2 - triZ3) * (1.0f - sqX);
    }
    return avgNormal;
}