I have a grid set up, and had Dijkstra's algorithm working to slowly pathfind throughout the grid. I am now wanting to convert it to use A-Star, but am running into problems. My code is shown below, can anyone please give me some help? Thanks
Code:
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <GL\glut.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("Pathfinding") ;
glutDisplayFunc(display) ;
glutKeyboardFunc(keyboard);
glutReshapeFunc (reshape);
glutMainLoop();
}