Pathfinding help

I am currently trying to implement a pathfinding algorithm using c++ and opengl. My code so far is shown below, could someone please help me figure out how to make it work properly? Thanks in advance!

#define OPEN 1
#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
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);
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);
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 '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)
{
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();
}

Sorry for the double post, the character limit is too short :(
pathFinding is a hard and complicated topic.

I modified an A* algorithm to accept pretty much any type of map, maybe it'll be of good use to you:
it returns a string of movement (0 down right, 1 down, 2 down left, 3 left, 4 up left, 5 up, 6, up right, 7 right).
The function accepts a function which returns true upon collision and false when there's no collision.

 ``1234567891011121314151617`` ``````// pathFind.hpp #ifndef PATHFIND_HPP_INCLUDED #define PATHFIND_HPP_INCLUDED #include #include std::string pathFind ( const int xStart, const int yStart, const int xFinish, const int yFinish, std::function collision ); #endif // PATHFIND_HPP_INCLUDED ``````

 ``123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193`` ``````// pathFind.cpp #include "PathFind.hpp" #include // Using std::priority_queue #include // Using std::map #include // Using std::unique_ptr #include // Using std::abs and std::sqrt [code] std::string pathFind ( const int xStart, const int yStart, const int xFinish, const int yFinish, std::function collision ) { enum {dir = 8}; // number of possible directions to go at any position class node { // current position int xPos; int yPos; // total distance already travelled to reach the node int level; // priority=level+remaining distance estimate int priority; // smaller: higher priority public: node(int xp, int yp, int d, int p) : xPos(xp), yPos(yp), level(d), priority(p) {} inline int getxPos() const {return xPos;} inline int getyPos() const {return yPos;} inline int getLevel() const {return level;} inline int getPriority() const {return priority;} inline void updatePriority(const int xDest, const int yDest) { priority = level + estimate(xDest, yDest) * 10; //A* } // give better priority to going strait instead of diagonally inline void nextLevel(const int i) // i: direction { level += (dir == 8?(i % 2 == 0 ? 10 : 14) : 10); } // Estimation function for the remaining distance to the goal. inline const int estimate(const int xDest, const int yDest) const { static int xd, yd, d; xd = xDest - xPos; yd = yDest - yPos; // Euclidian Distance //d = static_cast(std::sqrt(xd * xd + yd * yd)); // Manhattan distance d = std::abs(xd) + std::abs(yd); // Chebyshev distance //d = std::max(std::abs(xd), std::abs(yd)); return d; } inline bool operator<(const node &rhs) const { return this->getPriority() > rhs.getPriority(); } }; const static int dx[dir] = {1, 1, 0, -1, -1, -1, 0, 1}; const static int dy[dir] = {0, 1, 1, 1, 0, -1, -1, -1}; std::priority_queue pq[2]; // pqueue of open (not-yet-tried) nodes int pqi = 0; // pq index std::unique_ptr n0(new node(xStart, yStart, 0, 0)); // mapp std::unique_ptr m0; int i, j, x, y, xdx, ydy; char c; std::map> closed_nodes_map; // map of closed (tried-out) nodes std::map> open_nodes_map; // map of open (not-yet-tried) nodes std::map> dir_map; // map of directions // create the start node and push into list of open nodes n0->updatePriority(xFinish, yFinish); pq[pqi].push(*n0); open_nodes_map[x][y] = n0->getPriority(); // mark it on the open nodes map // A* search while(!pq[pqi].empty()) { // get the current node w/ the highest priority // from the list of open nodes n0.reset(new node(pq[pqi].top().getxPos(), pq[pqi].top().getyPos(), pq[pqi].top().getLevel(), pq[pqi].top().getPriority())); x = n0->getxPos(); y = n0->getyPos(); pq[pqi].pop(); // remove the node from the open list open_nodes_map[x][y] = 0; // mark it on the closed nodes map closed_nodes_map[x][y] = 1; // quit searching when the goal state is reached //if((*n0).estimate(xFinish, yFinish) == 0) if(x == xFinish && y == yFinish) { // generate the path from finish to start // by following the directions std::string path = ""; while(!(x == xStart && y == yStart)) { j = dir_map[x][y]; c = '0' + (j + dir / 2) % dir; path = c + path; x += dx[j]; y += dy[j]; } // empty the leftover nodes //while(!pq[pqi].empty()) pq[pqi].pop(); /// No need to, object is local. Let STD handle it. return path; } // generate moves (child nodes) in all possible directions for(i = 0; i < dir; ++i) { xdx = x + dx[i]; ydy = y + dy[i]; if(!(/*xdx < lower_x_bound *//*|| xdx > upper_x_bound - 1 *//*We have unlimited size*//*|| ydy < lower_y_bound *//*|| ydy > upper_y_bound - 1*//*Unlimited size ||*/ collision(xdx, ydy) == true || closed_nodes_map[xdx][ydy] == 1)) { // generate a child node m0.reset(new node(xdx, ydy, n0->getLevel(), n0->getPriority())); m0->nextLevel(i); m0->updatePriority(xFinish, yFinish); // if it is not in the open list then add into that if(open_nodes_map[xdx][ydy] == 0) { open_nodes_map[xdx][ydy]=m0->getPriority(); pq[pqi].push(*m0); // mark its parent node direction dir_map[xdx][ydy] = (i + dir / 2 )%dir; } else if(open_nodes_map[xdx][ydy]>m0->getPriority()) { // update the priority info open_nodes_map[xdx][ydy] = m0->getPriority(); // update the parent direction info dir_map[xdx][ydy] = (i + dir / 2) % dir; // replace the node // by emptying one pq to the other one // except the node to be replaced will be ignored // and the new node will be pushed in instead while(!(pq[pqi].top().getxPos() == xdx && pq[pqi].top().getyPos() == ydy)) { pq[1 - pqi].push(pq[pqi].top()); pq[pqi].pop(); } pq[pqi].pop(); // remove the wanted node // empty the larger size pq to the smaller one if(pq[pqi].size() > pq[1 - pqi].size()) pqi = 1 - pqi; while(!pq[pqi].empty()) { pq[1 - pqi].push(pq[pqi].top()); pq[pqi].pop(); } pqi = 1 - pqi; pq[pqi].push(*m0); // add the better node instead } } } // Possible child nodes in all directions } // while pq is not empty. return ""; // no route found }``````
Topic archived. No new replies allowed.