Pathfinding
Jump to navigation
Jump to search
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