OpenGL calculate normal vectors for heightmap

//http://www.youtube.com/user/thecplusplusguy
//calculate the normal vectors for the terrain
#include <iostream>
#include <SDL/SDL.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include "camera.h"
#include "functions.h"
#include <vector>

camera cam;
std::vector<std::vector<float> > heights;
std::vector<std::vector<vector3d> > normal;
bool drawNormal=true;   //draw out the normals as lines, if you want.

void loadHeightmap(const char* name)
{
    SDL_Surface* img=SDL_LoadBMP(name);
    if(!img)
    {
        std::cout << "image is not loaded" << std::endl;
        return;
    }
    std::vector<float> tmp;
    for(int i=0;i<img->h;i++)
    {
        tmp.clear();
        for(int j=0;j<img->w;j++)
        {
            Uint32 pixel=((Uint32*)img->pixels)[i*img->pitch/4 + j];
            Uint8 r,g,b;    //unsigned char
            SDL_GetRGB(pixel,img->format,&r,&g,&b);
            tmp.push_back((float)r/255.0);  //0.0,1.0
        }
        heights.push_back(tmp);
    }
}

void renderHeightmap(float size,float h)
{
    float dif[]={0.3,0.8,0.3};
    glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,dif);
    for(int i=0;i<heights.size()-1;i++)
        for(int j=0;j<heights[0].size()-1;j++)
        {
                vector3d v1(i*size,heights[i][j]*h,j*size);             //3 point for a triangle in the terrain
                vector3d v2((i+1)*size,heights[i+1][j]*h,j*size);
                vector3d v3(i*size,heights[i][j+1]*h,(j+1)*size);
                vector3d vec1=v2-v1;    //two vectors from the triangle
                vector3d vec2=v3-v1;
            //  vec1.normalize();
            //  vec2.normalize();
                vector3d normal=vec2.crossProduct(vec1);    //cross product gives the normal vector
                normal.normalize();
            glBegin(GL_TRIANGLE_STRIP);
                //glColor3f(heights[i][j],heights[i][j],heights[i][j]);
                glNormal3f(normal.x,normal.y,normal.z);
                glVertex3f(i*size,heights[i][j]*h,j*size);
                glVertex3f((i+1)*size,heights[i+1][j]*h,j*size);
                glVertex3f(i*size,heights[i][j+1]*h,(j+1)*size);
                glVertex3f((i+1)*size,heights[i+1][j+1]*h,(j+1)*size);
            glEnd();
            if(drawNormal)
            {
                glDisable(GL_LIGHTING);
                glBegin(GL_LINES);
                    glColor3f(1.0,0.0,0.0);
                    glVertex3f(i*size,heights[i][j]*h,j*size);
                    normal/=10.0;
                    v1+=normal;
                    glVertex3f(v1.x,v1.y,v1.z);
                glEnd();
                glEnable(GL_LIGHTING);
            }
        }
}

void init(float angle)
{
    glClearColor(0,0,0,1);
    glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(angle,640.0/480.0,0.1,1000);
    glMatrixMode(GL_MODELVIEW);
    //initskybox();
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);

    glEnable(GL_DEPTH_TEST);
    loadHeightmap("heightmap.bmp");
}

void display()
{
    glLoadIdentity();
    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
    cam.Control();
    //drawSkybox(50);

    cam.UpdateCamera();
    //float pos[]={0,10,0};
    //glLightfv(GL_LIGHT0,GL_POSITION,pos);
    renderHeightmap(0.1,1.0);

}

int main()
{
    SDL_Init(SDL_INIT_EVERYTHING);
    SDL_SetVideoMode(640,480,32,SDL_OPENGL);
    Uint32 start;
    SDL_Event event;
    bool running=true;
    float angle=50;
    init(angle);
    bool b=false;
    while(running)
    {
        start=SDL_GetTicks();
        while(SDL_PollEvent(&event))
        {
            switch(event.type)
            {
                case SDL_QUIT:
                    running=false;
                    break;
                case SDL_KEYDOWN:
                    switch(event.key.keysym.sym)
                    {
                        case SDLK_ESCAPE:
                            running=false;
                            break;
                    }
                    break;
                case SDL_MOUSEBUTTONDOWN:
                    cam.mouseIn(true);
                    break;

            }
        }
        display();
        SDL_GL_SwapBuffers();
        if(1000.0/30>SDL_GetTicks()-start)
            SDL_Delay(1000.0/30-(SDL_GetTicks()-start));
    }
    SDL_Quit();
}