Hi i've been working on a piece of homework we're I've been asked to implement a 2d grid in opengl and extend Dijkstra's algorithm to A star, so far i've been able to get the 2d grid running with Dijkstra's algorithm with not too many problems but my current extenstion to A star has me scratching my head, i'm just wondering if I'm approaching it all wrong or if I'm going in the right direction with just a few careless mistakes. So my main question is can you see what I am doing wrong? Am I calculating the Manhatten distance correctly and is it something else that is causing the problem. If you can point me in the right direction it would be much appreciated here's my code :
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <GL\openglut.h>
#define OPEN 1 // status values for tiles
#define CLOSED 2
#define UNVISITED 3
#define BLOCKED 4
#define HIGHLIGHT 5
#define ONROUTE 6
#define GOAL 7
#define GSIZE 20 // size of tile grid
#define ISTART 15 // index position of starting tile in grid
#define JSTART 8 //
#define IGOAL 7 // index position of goal tile in grid
#define JGOAL 8
bool endsearch = false ; // flag to indeicate have reached goal
int icurrent = 15 ; // index position of the current tile
int jcurrent = 8 ;
int cursori = 10;
int cursorj = 10;
struct tile
{
int status ; // Is tile open, closed, unvisited etc.
int cost ; // accumulated cost from start to this tile
int iparent ; // index position in grid for tile previous to this one
int jparent ;
int h; //heuristic
int F;
};
tile grid[GSIZE][GSIZE] ; // 2d array of tiles. The main data structure
//Math.abs(start.x-destination.x) + Math.abs(start.y-destination.y))
int costinc = 10; // set cost at 10 for sideways and updown movements
//int heuristic = 10 *((abs(ISTART-IGOAL)/GSIZE) + (abs(JSTART-JGOAL)/GSIZE)) ;
void Initialize()
//
// Initialise all tiles to UNVISITED except those around the edges. Edge tiles are
// set to BLOCKED so the search cannot go over outside the grid
// Make the start tile the current OPEN tile with accumulated cost of zero
//
{
int i, j ;
for(i=0 ; i< GSIZE ; i++)
{
for(j = 0 ; j<GSIZE; j++)
{
if((i == 0) || (i==GSIZE-1) ||(j == 0) || (j==GSIZE-1))
{
grid[i][j].status = BLOCKED ;
}
else
{
grid[i][j].status = UNVISITED ;
}
}
}
grid[ISTART][JSTART].status = OPEN ;
grid[ISTART][JSTART].cost = 0 ;
grid[IGOAL][JGOAL].status = GOAL;
}
void getroute()
//
// The goal tile has been reached. Using the iparent, jparent fields trace the path
// back to the start. For each tile on the way back mark its status as ONROUTE. Do
//this until iparent = 0 so have reached start
//
{
int i = icurrent ;
int j = jcurrent ;
int itemp, jtemp ;
do
{
grid[i][j].status = ONROUTE ; // mark tile as on path
itemp = grid[i][j].iparent ; // dont overwrite i,j until we have finished using them
jtemp = grid[i][j].jparent ;
i = itemp ;
j = jtemp ;
}
while(i != 0) ; // reached start
endsearch = true ; // set finished flag
}
void findcheapest(void)
//
// Cycle through all the tiles(nodes) and look at those that are OPEN
// Find which OPEN tile has the lowest cost. This then becomes the current tile
//(icurrent, jcurrent). If the node selected is also the finishing point then the
// solution has been found so call getroute to list the route.
{
// arbitrary high initial value for lowcost comparison
int i, j ;
//int F = grid[i][j].cost + grid[i][j].h;
for(i=0 ; i< GSIZE ; i++)
{
for(j = 0 ; j<GSIZE; j++)
{
if (grid[i][j].status == OPEN)
{
if (grid[i][j].cost < grid[i][j].F) // is this the lowest cost so far?
{
grid[i][j].F = grid[i][j].cost ; // if yes then make this tile
icurrent = i ; // the current tile
jcurrent = j ;
}
}
}
}
// have we reached the destination ?. If yes then generate the route
if((icurrent == IGOAL) && (jcurrent == JGOAL))
getroute() ;
}
void update(int i, int j)
//
// Converts the node/tile [i][j] into an OPEN node and assigns
// an accumulated cost (parent cost + increment). Stores the path
// to this tile from the current tile in iparent, jparent
//
{
grid[i][j].status = OPEN ;
grid[i][j].cost = grid[icurrent][jcurrent].cost+ costinc;
grid[i][j].h = ((abs(icurrent-IGOAL) + (abs(jcurrent-JGOAL))));
grid[i][j].F = (grid[i][j].cost + costinc) + grid[i][j].h;
grid[i][j].iparent = icurrent ;
grid[i][j].jparent = jcurrent ;
}
void getconnection(int i, int j)
//
// Check if this path is BLOCKED. If it is then return and do nothing - we cannot
// go there. If not BLOCKED then check if it is as yet UNVISITED. If so then convert it to an OPEN
// node/tile. If the node/tile is already OPEN then compare the cost it would have when
// reached via this route against the cost that it already has (having previously been reached
// from a different tile). If the newcost(from current tile) is lower then replace the cost
// value and the previous tile link with the new values
//
{
int newcost ;
if (grid[i][j].status != BLOCKED)
{
switch (grid[i][j].status)
{
case UNVISITED:
update(i,j) ;
break ;
case HIGHLIGHT:
update(i,j) ;
break;
case GOAL:
update(i,j) ;
break;
case OPEN:
newcost = grid[icurrent][jcurrent].cost + costinc ;
if (newcost < grid[i][j].cost)
{
update(i, j) ;
}
break ;
case CLOSED:
newcost = grid[icurrent][jcurrent].cost + costinc ;
if (newcost < grid[i][j].cost)
{
update(i, j) ;
}
break ;
}
}
}
void walk(void)
//
// From the current tile examine all its neighbors for a possible pathway. Looks at
// left, right, up, down neighbours but also diagonal moves. Moving diagonally means
// moving a further in distance than when moving sideways etc. So make the cost higher.
// (change costinc.) A cost of 14 approximates the extra distance whilst avoiding use of floats.
// When we have processed all the possible pathways from the current tile we can then CLOSE it
//
{
int i = icurrent ;
int j = jcurrent ;
costinc = 10 ;
getconnection(i+1, j) ;
getconnection(i-1, j) ;
getconnection(i, j+1) ;
getconnection(i, j-1) ;
costinc = 14 ;
getconnection(i+1, j+1) ;
getconnection(i+1, j-1) ;
getconnection(i-1, j+1) ;
getconnection(i-1, j-1) ;
grid[i][j].status = CLOSED ;
}
void drawsquares (void) { //draws the entire grid interface and colours the grid
for (int i = 0; i < GSIZE; i++){
if(i == 0){
glTranslatef(-2.0 ,3.0, 0.0);
}else{
glTranslatef(-3.0 ,-0.15, 0.0);
}
for (int j = 0; j < GSIZE; j++){
glTranslatef(0.15,0.0,0.0);
switch (grid[i][j].status)
{
case 1:
glColor3f(0,1,0);
break;
case 2:
glColor3f(1,0,0);
break;
case 3:
glColor3f(0.3,0.3,0.3);
break;
case 4:
glColor3f(1,0,1);
break;
case 5:
glColor3f(1,1,1);
break;
case 6:
glColor3f(0,0,1);
break;
case 7:
glColor3f(1,1,0);
break;
}
if((icurrent == IGOAL) && (jcurrent == JGOAL))
{
grid[IGOAL][JGOAL].status = ONROUTE;
}
glPushMatrix(); //push the matrix so that our translations only affect this tile
glTranslatef(j, -i, 0); //translate the tile to where it should belong
glBegin (GL_QUADS); //begin drawing our quads
glVertex3f(0.0, 0.0, 0.0); //with our vertices we have to assign a texcoord
glVertex3f(1.0, 0.0, 0.0); //so that our texture has some points to draw to
glVertex3f(1.0, 1.0, 0.0);
glVertex3f(0.0, 1.0, 0.0);
glEnd();
glPopMatrix();
}
}
}
void display(void)
{
glClearColor (0.0,0.0,0.0,1.0);
glClear (GL_COLOR_BUFFER_BIT);
glLoadIdentity();
glTranslatef(-8, 6, -30); //translate back a bit to view the map correctly
drawsquares();
glFlush();
}
void reshape (int w, int h) {
glViewport (0, 0, (GLsizei)w, (GLsizei)h);
glMatrixMode (GL_PROJECTION);
glLoadIdentity();
gluPerspective (60, (GLfloat)w / (GLfloat)h, 1.0, 100.0);
glMatrixMode (GL_MODELVIEW);
}
void keyboard(unsigned char key, int i, int j)
{
// int i = cursori;
//int j = cursorj;
grid[cursori][cursorj];
glColor3f(1,1,1); ;
switch (key)
{
case 'p':
do
{
findcheapest() ;
walk() ;
}while (endsearch == false) ;
break ;
case 'd':
cursorj = cursorj + 1;
i= cursori;
j= cursorj;
grid[i][j].status = HIGHLIGHT;
break ;
case 'a':
cursorj = cursorj - 1 ;
i= cursori;
j= cursorj;
grid[i][j].status = HIGHLIGHT;
break ;
case 'w':
cursori = cursori - 1 ;
i= cursori;
j= cursorj;
grid[i][j].status = HIGHLIGHT;
break ;
case 's':
cursori = cursori + 1 ;
i= cursori;
j= cursorj;
grid[i][j].status = HIGHLIGHT;
break ;
case 'o':
i= cursori;
j= cursorj;
grid[i][j].status = BLOCKED ;
break ;
// case 'u':
// ISTART = cursori;
//JSTART = cursorj;
//grid[ISTART][JSTART].status = OPEN;
//grid[ISTART][JSTART].cost = 0 ;
// break;
case 'c':
i = cursori;
j = cursorj;
if(grid[i][j].status = HIGHLIGHT)
{
grid[i][j].status =UNVISITED;
}
break;
//case 'g':
//IGOAL = cursori;
//JGOAL = cursorj;
//grid[IGOAL][JGOAL].status = GOAL;
//break;
case 'l':
findcheapest();
walk();
break;
}
display() ;
// also process cursor keys in here
}
int main(int argc, char **argv)
//
// Initialise the grid then repeatedly do the following: find the lowest cost OPEN
// node/tile, get all its neigboring tiles and declare these OPEN (if not BLOCKED),
// assign a cost to all the newly OPEN tiles and store a link back to the current tile,
// declare the current tile closed.
// Do this until we reach the destination
// Finally print out the grid so that we can see the path
//
{
int i, j ;
Initialize() ;
glutInit(&argc, argv) ;
glutInitDisplayMode(GLUT_SINGLE) ;
glutInitWindowSize(GSIZE *32, GSIZE * 32);
glutInitWindowPosition(0,0);
glutCreateWindow("Assignment 2") ;
glutDisplayFunc(display) ;
glutKeyboardFunc(keyboard);
glutReshapeFunc (reshape);
glutMainLoop();
}