A* algorithm (graphical) implementation

//A* algorithm (might poor) implementation by http://www.youtube.com/user/thecplusplusguy
//graphical, press s, to place a start point (use mouse, to place it), e for end point, w for walls, press u, and it will search the shortest path between the 2 point around the walls
//SDL library used
#include <iostream>
#include <fstream>
#include <SDL/SDL.h>
#include <vector>
#include <cmath>
#include <map>

const int TILE_SIZE=5;
const int WIDTH=128;
const int HEIGHT=96;

struct Matrixindex{
    int x,y;
    Matrixindex(int yi,int xi) : y(yi),x(xi){};
    Matrixindex() : y(0),x(0) {};
    bool operator<(const Matrixindex& m2) const {return (true);};
};


std::vector<Matrixindex> comefrom;
std::map<Matrixindex,Matrixindex> comefrom2;
struct tableElement{
    char tile;
    int g_score;
    int h_score;
    int f_score;
};

int getDistance(int x1,int y1,int x2,int y2)
{
    //Manhattan distance
//  return abs(x2-x1)+abs(y2-y1);
    return sqrt((x2-x1)*(x2-x1) + (y2-y1) * (y2-y1));
}

int getSmallest(tableElement table[HEIGHT][WIDTH],std::vector<Matrixindex> mi,Matrixindex& matrixindex)
{
    if(mi.size()==1)
    {
        matrixindex=mi[0];
        return 0;
    }
    int smallest=0;
    for(int i=1;i<mi.size();i++)
    {
        if(table[mi[i].y][mi[i].x].f_score<table[mi[smallest].y][mi[smallest].x].f_score)
            smallest=i;
    }
    matrixindex=mi[smallest];
    return smallest;
}

bool inVector(std::vector<Matrixindex> mi,Matrixindex matrixindex)
{
    for(int i=0;i<mi.size();i++)
        if(mi[i].x==matrixindex.x && mi[i].y==matrixindex.y)
            return true;
    return false;
}
int elementInVector(std::vector<Matrixindex> mi,Matrixindex key)
{
    for(int i=0;i<mi.size();i++)
        if(mi[i].y==key.y && mi[i].x==key.x)
            return i;
    return -1;
}

std::vector<Matrixindex> child;
std::vector<Matrixindex> father;


void reconstruct_path(Matrixindex current,int starty,int startx)
{
    Matrixindex tmp=current;
    while(tmp.y!=starty || tmp.x!=startx)
    {
        comefrom.push_back(tmp);
        tmp=father[elementInVector(child,tmp)];
    }
}





void findPath(tableElement table[HEIGHT][WIDTH],int startx,int starty,int endx,int endy)
{
        child.clear();
        father.clear();
        std::vector<Matrixindex> closedset;
        std::vector<Matrixindex> openset;
        openset.push_back(Matrixindex(starty,startx));
        table[starty][startx].f_score=getDistance(endx,endy,startx,starty);
        table[starty][startx].h_score=0;
        table[starty][startx].g_score=table[starty][startx].f_score+table[starty][startx].h_score;
        Matrixindex current(0,0);
        int curindex;
        while(openset.size()!=0)
        {
            curindex=getSmallest(table,openset,current);
            if(endx==openset[curindex].x && endy==openset[curindex].y)
            {
                reconstruct_path(current,starty,startx);
                std::cout << "found" << std::endl;
                break;
            }
            closedset.push_back(current);
            openset.erase(openset.begin()+curindex);
            int g_score;
            bool better;
            for(int i=-1;i<=1;i+=1)
                for(int j=-1;j<=1;j+=1)
                {
                    Matrixindex neighbor=Matrixindex(current.y+i,current.x+j);
                    if(neighbor.x<0 || neighbor.x>=WIDTH || neighbor.y<0 || neighbor.y>=HEIGHT)
                        continue;
                    if(inVector(closedset,neighbor))
                        continue;
                    if(table[neighbor.y][neighbor.x].tile==1)
                        continue;
                    //comment out, if diagonal movement allowed
                    if(i!=0 && j!=0)
                        continue;
                    if(current.x==neighbor.x || current.y==neighbor.y)
                        g_score=table[current.y][current.x].g_score+10;
                    else
                        g_score=table[current.y][current.x].g_score+14;
                    if(!inVector(openset,neighbor))
                    {
                        openset.push_back(neighbor);
                        table[neighbor.y][neighbor.x].h_score=getDistance(endx,endy,neighbor.x,neighbor.y)*10;
                        better=true;
                    }else if(g_score<table[neighbor.y][neighbor.x].g_score)
                        better=true;
                    else
                        better=false;
                    if(better)
                    {
                        child.push_back(neighbor);
                        father.push_back(current);
                        table[neighbor.y][neighbor.x].g_score=g_score;
                        table[neighbor.y][neighbor.x].f_score=table[neighbor.y][neighbor.x].g_score+table[neighbor.y][neighbor.x].h_score;
                    }
                }
        }
        return;
}


int main()
{
    SDL_Init(SDL_INIT_EVERYTHING);
    SDL_Surface* screen=SDL_SetVideoMode(TILE_SIZE*WIDTH,TILE_SIZE*HEIGHT,32,SDL_SWSURFACE);
    tableElement table[HEIGHT][WIDTH];
    for(int i=0;i<HEIGHT;i++)
        for(int j=0;j<WIDTH;j++)
        {
            table[i][j].tile=0;
            table[i][j].g_score=table[i][j].h_score=table[i][j].f_score=0;
        }
    char curtile=1;
    bool running=true;
    SDL_Event event;
    Uint32 start;
    SDL_Rect curcoord={0,0,TILE_SIZE,TILE_SIZE};
    Uint32 curcolor=SDL_MapRGB(screen->format,128,128,128);
    int x=0,y=0;
    bool clicked=false;
    int startx=-1;
    int starty=-1;
    int endx=-1;
    int endy=-1;
    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;
                        case SDLK_w:
                            curtile=1;
                            break;
                        case SDLK_s:
                            curtile=2;
                            break;
                        case SDLK_e:
                            curtile=3;
                            break;
                        case SDLK_n:
                            curtile=0;
                            break;
                        case SDLK_u:
                            comefrom.clear();
                            findPath(table,startx,starty,endx,endy);
                            break;
                    }
                    break;
                case SDL_MOUSEMOTION:
                    x=event.motion.x/TILE_SIZE;
                    y=event.motion.y/TILE_SIZE;
                    if(clicked)
                    {
                        if(curtile==2)
                        {
                            startx=x;
                            starty=y;
                        }else if(curtile==3)
                        {
                            endx=x;
                            endy=y;
                        }else
                            table[y][x].tile=curtile;
                    }
                    curcoord.x=x*TILE_SIZE;
                    curcoord.y=y*TILE_SIZE;
                    if(curtile==0)
                        curcolor=SDL_MapRGB(screen->format,255,255,255);
                    else if(curtile==1)
                        curcolor=SDL_MapRGB(screen->format,128,128,128);
                    else if(curtile==2)
                        curcolor=SDL_MapRGB(screen->format,0,255,0);
                    else
                        curcolor=SDL_MapRGB(screen->format,255,0,0);
                    SDL_FillRect(screen,&curcoord,curcolor);
                    break;
                case SDL_MOUSEBUTTONDOWN:
                    clicked=true;
                    if(curtile==2)
                    {
                        startx=x;
                        starty=y;
                    }else if(curtile==3)
                    {
                        endx=x;
                        endy=y;
                    }else
                        table[y][x].tile=curtile;
                    break;
                case SDL_MOUSEBUTTONUP:
                    clicked=false;
                    break;
            }
        }
        for(int i=0;i<HEIGHT;i++)
            for(int j=0;j<WIDTH;j++)
            {
                if(i==y && j==x)
                    continue;
                curcoord.x=j*TILE_SIZE;
                curcoord.y=i*TILE_SIZE;
                if(table[i][j].tile==0)
                    curcolor=SDL_MapRGB(screen->format,255,255,255);
                else
                    curcolor=SDL_MapRGB(screen->format,128,128,128);
                SDL_FillRect(screen,&curcoord,curcolor);
            }
        if(startx!=-1 && starty!=-1)
        {
            curcolor=SDL_MapRGB(screen->format,0,255,0);
            curcoord.x=startx*TILE_SIZE;
            curcoord.y=starty*TILE_SIZE;
            SDL_FillRect(screen,&curcoord,curcolor);
        }
        if(endx!=-1 && endy!=-1)
        {
            curcolor=SDL_MapRGB(screen->format,255,0,0);
            curcoord.x=endx*TILE_SIZE;
            curcoord.y=endy*TILE_SIZE;
            SDL_FillRect(screen,&curcoord,curcolor);
        }

        for(int i=0;i<comefrom.size();i++)
        {
                curcoord.x=comefrom[i].x*TILE_SIZE;
                curcoord.y=comefrom[i].y*TILE_SIZE;
                curcolor=SDL_MapRGB(screen->format,5,128,25);
                SDL_FillRect(screen,&curcoord,curcolor);
        }

        SDL_Flip(screen);
        if(1000/30>(SDL_GetTicks()-start))
            SDL_Delay(1000/30-(SDL_GetTicks()-start));
    }
    SDL_Quit();
    return 0;
}