Difference between revisions of "Pathfinding"
Jump to navigation
Jump to search
(fixed < and >) |
|||
Line 22: | Line 22: | ||
/* pathfind.c - simple path finder - public domain */ | /* pathfind.c - simple path finder - public domain */ | ||
#include | #include <stdio.h> | ||
#include | #include <stdlib.h> | ||
#include | #include <time.h> | ||
#include | #include <string.h> | ||
#define MAP_WIDTH 70 | #define MAP_WIDTH 70 | ||
Line 324: | Line 324: | ||
{ | { | ||
#ifdef ASTAR | #ifdef ASTAR | ||
if ( !element- | if ( !element->estimate_ ) | ||
{ | { | ||
element- | element->estimate_ = | ||
path_distance( element- | path_distance( element->x_pos_, element->y_pos_, goalx, goaly ); | ||
} | } | ||
#endif | #endif | ||
return element- | return element->cost_ + element->estimate_; | ||
} | } | ||
Line 350: | Line 350: | ||
#if 0 | #if 0 | ||
if ( path_distance( next- | if ( path_distance( next->x_pos_, next->y_pos_, goalx, goaly ) < | ||
path_distance( current- | path_distance( current->x_pos_, current->y_pos_, goalx, | ||
goaly ) ) | goaly ) ) | ||
#else | #else | ||
Line 419: | Line 419: | ||
{ | { | ||
/* If not processed yet, add to open set */ | /* If not processed yet, add to open set */ | ||
if ( pos- | if ( pos->state_ == path_element_empty ) | ||
{ | { | ||
pos- | pos->state_ = path_element_open; | ||
pos- | pos->parent_ = parent; | ||
path_update_cost( parent, pos ); | path_update_cost( parent, pos ); | ||
Line 437: | Line 437: | ||
{ | { | ||
/* New path is better than old. */ | /* New path is better than old. */ | ||
int was_open = ( pos- | int was_open = ( pos->state_ == path_element_open ); | ||
*pos = tmp; | *pos = tmp; | ||
pos- | pos->parent_ = parent; | ||
pos- | pos->state_ = path_element_open; | ||
if ( was_open ) | if ( was_open ) | ||
Line 481: | Line 481: | ||
{ | { | ||
/* Push first element to stack. */ | /* Push first element to stack. */ | ||
pos- | pos->state_ = path_element_open; | ||
path_push_open( pos, real_goal_x, real_goal_y ); | path_push_open( pos, real_goal_x, real_goal_y ); | ||
Line 489: | Line 489: | ||
/* Did we reach target? */ | /* Did we reach target? */ | ||
if ( ( current- | if ( ( current->x_pos_ == real_goal_x ) && | ||
( current- | ( current->y_pos_ == real_goal_y ) ) | ||
{ | { | ||
found = 1; | found = 1; | ||
Line 497: | Line 497: | ||
/* Current is closed. */ | /* Current is closed. */ | ||
current- | current->state_ = path_element_closed; | ||
/* Generate positions reachable from current position. */ | /* Generate positions reachable from current position. */ | ||
path_check( current, | path_check( current, | ||
current- | current->x_pos_ + 1, current->y_pos_, | ||
real_goal_x, real_goal_y ); | real_goal_x, real_goal_y ); | ||
path_check( current, | path_check( current, | ||
current- | current->x_pos_ - 1, current->y_pos_, | ||
real_goal_x, real_goal_y ); | real_goal_x, real_goal_y ); | ||
path_check( current, | path_check( current, | ||
current- | current->x_pos_, current->y_pos_ + 1, | ||
real_goal_x, real_goal_y ); | real_goal_x, real_goal_y ); | ||
path_check( current, | path_check( current, | ||
current- | current->x_pos_, current->y_pos_ - 1, | ||
real_goal_x, real_goal_y ); | real_goal_x, real_goal_y ); | ||
/* So if you want to allow diagonal movement, add four | /* So if you want to allow diagonal movement, add four | ||
Line 528: | Line 528: | ||
int result = 0; | int result = 0; | ||
struct path_element* pos = path_element_map( *nextx, *nexty ); | struct path_element* pos = path_element_map( *nextx, *nexty ); | ||
if ( pos- | if ( pos->parent_ ) | ||
{ | { | ||
*nextx = pos- | *nextx = pos->parent_->x_pos_; | ||
*nexty = pos- | *nexty = pos->parent_->y_pos_; | ||
result = ( pos- | result = ( pos->parent_->parent_ != NULL ); | ||
} | } | ||
Line 553: | Line 553: | ||
{ | { | ||
#ifndef MAP_VARIYING_COST | #ifndef MAP_VARIYING_COST | ||
pos- | pos->cost_ = parent->cost_ + 1; | ||
#else | #else | ||
int dx = pos- | int dx = pos->x_pos_ - parent->x_pos_ + 1; /* dx = 0, 1, 2 */ | ||
int dy = pos- | int dy = pos->y_pos_ - parent->y_pos_ + 1; /* dy = 0, 1, 2 */ | ||
if ( pos- | if ( pos->x_pos_ & 1 ) | ||
{ | { | ||
pos- | pos->cost_ = parent->cost_ + cost_table_a[dx][dy]; | ||
} | } | ||
else | else | ||
{ | { | ||
pos- | pos->cost_ = parent->cost_ + cost_table_b[dx][dy]; | ||
} | } | ||
#endif // MAP_VARIYING_COST | #endif // MAP_VARIYING_COST |
Latest revision as of 04:31, 7 January 2008
The most generic algorithm is something like: 1. Start with empty priority queue P 2. Start with empty set of previously visited nodes V 3. Add start node s to P with cost f(s) 4. If P is empty, then stop, no solution. 5. Remove node x with lowest f(x) from P. 6. If x is goal, stop with success. 7. For each nx in successors of x: a) calculate f(nx) (= f(x) + delta) b) if nx not visited yet OR nx is in the priority queue P but with higher f(nx) OR nx has been visited but with higher f(nx) then place/update nx to P with new cost and update V to include (nx, f(nx), x) 8. Go to 4 Then if f(x) equals to cost from start node, it is djikstra f(x) equals to cost from start node + estimate to goal, the algorithm is A*. /* pathfind.c - simple path finder - public domain */ #include <stdio.h> #include <stdlib.h> #include <time.h> #include <string.h> #define MAP_WIDTH 70 #define MAP_HEIGHT 25 #define MAP_SIZE MAP_WIDTH*MAP_HEIGHT #define MAP_BLOCKERS 500 #define MAP_BLOCKERS_POS_TRIES 100 #define MAP_PATHS 10000 #define MAP_PATHS_POS_TRIES 100 //#define ASTAR //#define MAP_VARIYING_COST /* Map - 1 means blocked, 0 means passable */ static int map[ MAP_SIZE ]; /* States of path elements */ enum path_element_state { path_element_empty, path_element_open, path_element_closed }; /* Structure for path elements */ struct path_element { int x_pos_; int y_pos_; int cost_; int estimate_; enum path_element_state state_; struct path_element* parent_; }; /* Nodes - one per cell in map */ static struct path_element path_nodes[ MAP_SIZE ]; /* Open nodes */ static struct path_element* path_open[ MAP_SIZE ]; /* Top index */ static int path_open_top; /* Calculates distance between two points */ int path_distance( int x0, int y0, int x1, int y1 ); /* Checks if position is blocked */ int path_blocked( int x, int y ); /* Initialises path structures */ void path_init( void ); /* Maps coordinate to one dimensional array */ int path_map( int x, int y ); /* Returns pointer to path element, or NULL if position is invalid */ struct path_element* path_element_map( int x, int y ); /* Calculates cost from element to goal. */ int path_cost( struct path_element* element, int goalx, int goaly ); /* Pushes element to open stack */ void path_push_open( struct path_element* element, int goalx, int goaly ); /* Pops element from open stack */ struct path_element* path_pop_open( void ); /* Removes element from stack */ void path_remove( struct path_element* element ); /* Checks if there are elements in open stack */ int path_open_not_empty( void ); /* Pushes position to open stack when needed */ void path_check( struct path_element* parent, int posx, int posy, int goalx, int goaly ); /* Finds path from start towards goal */ int path_find( int startx, int starty, int goalx, int goaly ); /* Gets next step towards goal. */ int path_get_next( int* nextx, int* nexty ); /* Updates cost to pos which is adjacent to parent */ void path_update_cost( struct path_element* parent, struct path_element* pos ); int main() { int i, j, k; int seed = time( NULL ); memset( map, 0, MAP_SIZE*sizeof( int ) ); /* Put some dirt to the map */ for ( i = 0; i < MAP_WIDTH; i++ ) { map[ path_map( i, 0 ) ] = 1; map[ path_map( i, MAP_HEIGHT - 1 ) ] = 1; } for ( j = 0; j < MAP_HEIGHT; j++ ) { map[ path_map( 0, j ) ] = 1; map[ path_map( MAP_WIDTH - 1, j ) ] = 1; } for ( i = 0; i < MAP_BLOCKERS; i++ ) { for ( j = 0; j < MAP_BLOCKERS_POS_TRIES; j++ ) { int x = rand() % MAP_WIDTH; int y = rand() % MAP_HEIGHT; if ( !path_blocked( x, y ) ) { map[ path_map( x, y ) ] = 1; break; } } } srand( seed ); /* Show map */ printf( "Map with seed = %x:\n", seed ); k = 0; for ( j = 0; j < MAP_HEIGHT; j++ ) { if ( j ) { printf( "\n" ); } for ( i = 0; i < MAP_WIDTH; i++ ) { if ( !map[ k ] ) { printf( "." ); } else { printf( "#" ); } k++; } } printf( "\n" ); /* Generate some paths */ for ( k = 0; k < MAP_PATHS; k++ ) { int start_x; int start_y; int goal_x; int goal_y; int path; int steps; printf( "Path %d:\n", k + 1 ); for ( i = 0; i < MAP_PATHS_POS_TRIES; i++ ) { start_x = rand() % MAP_WIDTH; start_y = rand() % MAP_HEIGHT; if ( !path_blocked( start_x, start_y ) ) { break; } } for ( j = 0; j < MAP_PATHS_POS_TRIES; j++ ) { goal_x = rand() % MAP_WIDTH; goal_y = rand() % MAP_HEIGHT; if ( !path_blocked( goal_x, goal_y ) ) { break; } } if ( ( i == MAP_PATHS_POS_TRIES ) || ( j == MAP_PATHS_POS_TRIES ) ) { printf( "\tFailed to generate start & goal\n" ); continue; } /* Goal & start location are Ok, generate path */ printf( "\tStart = (%d, %d), goal = (%d, %d)\n", start_x, start_y, goal_x, goal_y ); path = path_find( start_x, start_y, goal_x, goal_y ); if ( path ) { /* Print out steps to goal. */ steps = 0; while ( path ) { int prev_x = start_x; int prev_y = start_y; path = path_get_next( &start_x, &start_y ); if ( !steps ) { printf( "\t(%2d, %2d) -> (%2d, %2d)", prev_x, prev_y, start_x, start_y ); } else { if ( ( steps + 1 ) & 7 ) { printf( " -> (%2d, %2d)", start_x, start_y ); } else { printf( " ->\n\t(%2d, %2d)", start_x, start_y ); } } steps++; } if ( ( start_x == goal_x ) && ( start_y == goal_y ) ) { printf( "\n\tFound target!\n" ); } } else { printf( "\tCould not find path\n" ); } } return EXIT_SUCCESS; } /* Calculate distance between two points */ int path_distance( int x0, int y0, int x1, int y1 ) { int xd = ( x0 - x1 ); int yd = ( y0 - y1 ); if ( xd < 0 ) { xd = -xd; } if ( yd < 0 ) { yd = -yd; } return xd + yd; } /* Checks if position is blocked */ int path_blocked( int x, int y ) { return map[ path_map( x, y ) ]; } /* Initialises path structures. */ void path_init( void ) { int i, j; int p; /* Everything done here is not absolutely necessary. */ memset( path_nodes, 0, MAP_SIZE * sizeof( struct path_element ) ); memset( path_open, 0, MAP_SIZE * sizeof( struct path_element* ) ); path_open_top = 0; p = 0; for ( j = 0; j < MAP_HEIGHT; j++ ) { for ( i = 0; i < MAP_WIDTH; i++ ) { path_nodes[ p ].x_pos_ = i; path_nodes[ p ].y_pos_ = j; p++; } } } /* Maps coordinates to one dimensional array indices. */ int path_map( int x, int y ) { return y * MAP_WIDTH + x; } /* Returns pointer to path element structure or NULL if invalid position. */ struct path_element* path_element_map( int x, int y ) { struct path_element* result = NULL; if ( ( x >= 0 && x < MAP_WIDTH ) && ( y >= 0 && y < MAP_HEIGHT ) ) { result = &path_nodes[ path_map( x, y ) ]; } return result; } /* Returns cost from position defined by element to goal.*/ int path_cost( struct path_element* element, int goalx, int goaly ) { #ifdef ASTAR if ( !element->estimate_ ) { element->estimate_ = path_distance( element->x_pos_, element->y_pos_, goalx, goaly ); } #endif return element->cost_ + element->estimate_; } /* Pushes pointer to one element to the open stack */ void path_push_open( struct path_element* element, int goalx, int goaly ) { int i; path_open[ path_open_top ] = element; path_open_top++; /* Keep elements in ascending order by distance to goal - we want to find one of the shortest paths */ for ( i = path_open_top - 1; i >= 1; i-- ) { struct path_element* current = path_open[ i ]; struct path_element* next = path_open[ i - 1 ]; #if 0 if ( path_distance( next->x_pos_, next->y_pos_, goalx, goaly ) < path_distance( current->x_pos_, current->y_pos_, goalx, goaly ) ) #else int next_total = path_cost( next, goalx, goaly ); int current_total = path_cost( current, goalx, goaly ); if ( next_total < current_total ) { #endif /* Swap pointers */ path_open[ i ] = next; path_open[ i - 1 ] = current; } } } /* Pops one element from the open stack */ struct path_element* path_pop_open( void ) { path_open_top--; struct path_element* result = path_open[ path_open_top ]; path_open[ path_open_top ] = NULL; return result; } /* Removes element from stack */ void path_remove( struct path_element* element ) { int i; for ( i = 0; i < path_open_top; i++ ) { if ( element == path_open[ i ] ) { break; } } if ( i != path_open_top ) { /* Element was found. */ path_open[ i ] = NULL; for ( ; i < ( path_open_top - 1 ); i++ ) { path_open[ i ] = path_open[ i + 1 ]; } path_open_top--; } } /* Checks that open stack is not empty. */ int path_open_not_empty( void ) { return path_open_top; } /* Pushes to open stack unless already open, closed or impassable */ void path_check( struct path_element* parent, int posx, int posy, int goalx, int goaly ) { struct path_element* pos = path_element_map( posx, posy ); if ( pos ) { /* We can ignore blocked positions (consider that cost is infinite).*/ if ( !path_blocked( posx, posy ) ) { /* If not processed yet, add to open set */ if ( pos->state_ == path_element_empty ) { pos->state_ = path_element_open; pos->parent_ = parent; path_update_cost( parent, pos ); path_push_open( pos, goalx, goaly ); } else { /* Now element is either in open or closed set. */ const int orig_cost = path_cost( pos, goalx, goaly ); struct path_element tmp = *pos; path_update_cost( parent, &tmp ); if ( orig_cost > path_cost( &tmp, goalx, goaly ) ) { /* New path is better than old. */ int was_open = ( pos->state_ == path_element_open ); *pos = tmp; pos->parent_ = parent; pos->state_ = path_element_open; if ( was_open ) { path_remove( pos ); } path_push_open( pos, goalx, goaly ); } } } } } /* Updates startx and starty one step towards (goalx, goaly). */ int path_find( int startx, int starty, int goalx, int goaly ) { int found = 0; /* Parent points towards start of search. So if we start from goal, then those parent pointers are automatically ok. Of course, the following assumption is made (reflexivity): if there is path from A to B, then there is path from B to A. (this holds in this application) */ const int real_goal_x = startx; const int real_goal_y = starty; const int real_start_x = goalx; const int real_start_y = goaly; path_init(); struct path_element* pos = path_element_map( real_start_x, real_start_y ); struct path_element* start = path_element_map( real_goal_x, real_goal_y ); if ( pos && start ) /* Both should be acceptable */ { /* Push first element to stack. */ pos->state_ = path_element_open; path_push_open( pos, real_goal_x, real_goal_y ); while ( path_open_not_empty() ) { struct path_element* current = path_pop_open(); /* Did we reach target? */ if ( ( current->x_pos_ == real_goal_x ) && ( current->y_pos_ == real_goal_y ) ) { found = 1; break; } /* Current is closed. */ current->state_ = path_element_closed; /* Generate positions reachable from current position. */ path_check( current, current->x_pos_ + 1, current->y_pos_, real_goal_x, real_goal_y ); path_check( current, current->x_pos_ - 1, current->y_pos_, real_goal_x, real_goal_y ); path_check( current, current->x_pos_, current->y_pos_ + 1, real_goal_x, real_goal_y ); path_check( current, current->x_pos_, current->y_pos_ - 1, real_goal_x, real_goal_y ); /* So if you want to allow diagonal movement, add four additional path_check calls. */ } } return found; } /* Gets next step towards goal - path_find must have been called beforehand. Results are erased after next path_find call. */ int path_get_next( int* nextx, int* nexty ) { int result = 0; struct path_element* pos = path_element_map( *nextx, *nexty ); if ( pos->parent_ ) { *nextx = pos->parent_->x_pos_; *nexty = pos->parent_->y_pos_; result = ( pos->parent_->parent_ != NULL ); } return result; } const static int cost_table_a[3][3] = { { 1, 1, 1 }, { 3, 1, 3 }, { 1, 1, 1 } }; const static int cost_table_b[3][3] = { { 1, 3, 1 }, { 1, 1, 1 }, { 1, 3, 1 } }; /* Updates cost to pos which is adjacent to parent. */ void path_update_cost( struct path_element* parent, struct path_element* pos ) { #ifndef MAP_VARIYING_COST pos->cost_ = parent->cost_ + 1; #else int dx = pos->x_pos_ - parent->x_pos_ + 1; /* dx = 0, 1, 2 */ int dy = pos->y_pos_ - parent->y_pos_ + 1; /* dy = 0, 1, 2 */ if ( pos->x_pos_ & 1 ) { pos->cost_ = parent->cost_ + cost_table_a[dx][dy]; } else { pos->cost_ = parent->cost_ + cost_table_b[dx][dy]; } #endif // MAP_VARIYING_COST } /* End of file */ Pekka Nurminen