www.pudn.com > search_short_path.zip > shortcut.cpp
//////////////////////////////////////////////////////////////////////
// Amit's Path-finding (A*) code.
//
// The main items of interest in my code are:
//
// 1. I'm using a hexagonal grid instead of a square grid. Since A*
// on a square grid works better with the "Manhattan" distance than with
// straight-line distance, I wrote a "Manhattan" distance on a hexagonal
// grid. I also added in a penalty for paths that are not straight
// lines. This makes lines turn out straight in the simplest case (no
// obstacles) without using a straight-line distance function (which can
// make the path finder much slower).
//
// To see the distance function, search for UnitMovement and look at
// its 'dist' function.
//
// 2. The cost function is adjustable at run-time, allowing for a
// sort of "slider" that varies from "Fast Path Finder" to "Good Path
// Quality". (My notes on A* have some ways in which you can use this.)
//
// 3. I'm using a data structure called a "heap" instead of an array
// or linked list for my OPEN set. Using lists or arrays, an
// insert/delete combination takes time O(N); with heaps, an
// insert/delete combination takes time O(log N). When N (the number of
// elements in OPEN) is large, this can be a big win. However, heaps
// by themselves are not good for one particular operation in A*.
// The code here avoids that operation most of the time by using
// a "Marking" array. For more information about how this helps
// avoid a potentially expensive operation, see my Implementation
// Nodes in my notes on A*.
//////////////////////////////////////////////////////////////////////
#include "Path.h"
// The mark array marks directions on the map. The direction points
// to the spot that is the previous spot along the path. By starting
// at the end, we can trace our way back to the start, and have a path.
// It also stores 'f' values for each space on the map. These are used
// to determine whether something is in OPEN or not. It stores 'g'
// values to determine whether costs need to be propagated down.
struct Marking
{
HexDirection direction:4; // !DirNone means OPEN || CLOSED
int f:14; // >= 0 means OPEN
int g:14; // >= 0 means OPEN || CLOSED
Marking(): direction(DirNone), f(-1), g(-1) {}
};
static MapArray& mark = *(new MapArray(Marking()));
// Path_div is used to modify the heuristic. The lower the number,
// the higher the heuristic value. This gives us worse paths, but
// it finds them faster. This is a variable instead of a constant
// so that I can adjust this dynamically, depending on how much CPU
// time I have. The more CPU time there is, the better paths I should
// search for.
int path_div = 6;
struct Node
{
HexCoord h; // location on the map, in hex coordinates
int gval; // g in A* represents how far we've already gone
int hval; // h in A* represents an estimate of how far is left
Node(): h(0,0), gval(0), hval(0) {}
~Node() {}
};
bool operator < ( const Node& a, const Node& b )
{
// To compare two nodes, we compare the `f' value, which is the
// sum of the g and h values.
return (a.gval+a.hval) < (b.gval+b.hval);
}
bool operator == ( const Node& a, const Node& b )
{
// Two nodes are equal if their components are equal
return (a.h == b.h) && (a.gval == b.gval) && (a.hval == b.hval);
}
inline HexDirection ReverseDirection( HexDirection d )
{
// With hexagons, I'm numbering the directions 0 = N, 1 = NE,
// and so on (clockwise). To flip the direction, I can just
// add 3, mod 6.
return HexDirection( ( 3+int(d) ) % 6 );
}
// greater is an STL thing to create a 'comparison' object out of
// the greater-than operator, and call it comp.
typedef vector Container;
greater comp;
// I'm using a priority queue implemented as a heap. STL has some nice
// heap manipulation functions. (Look at the source to `priority_queue'
// for details.) I didn't use priority_queue because later on, I need
// to traverse the entire data structure to update certain elements; the
// abstraction layer on priority_queue wouldn't let me do that.
inline void get_first( Container& v, Node& n )
{
n = v.front();
pop_heap( v.begin(), v.end(), comp );
v.pop_back();
}
// Here's the class that implements A*. I take a map, two points
// (A and B), and then output the path in the `path' vector, when
// find_path is called.
template
struct AStar
{
PathStats stats;
Heuristic& heuristic;
// Remember which nodes are in the OPEN set
Container open;
// Remember which nodes we visited, so that we can clear the mark array
// at the end. This is the 'CLOSED' set plus the 'OPEN' set.
Container visited;
Map& map;
HexCoord A, B;
AStar( Heuristic& h, Map& m, HexCoord a, HexCoord b )
: heuristic(h), map(m), A(a), B(b) {}
~AStar();
// Main function:
void find_path( vector& path );
// Helpers:
void propagate_down( Node H );
Container::iterator find_in_open( HexCoord h );
inline bool in_open( const HexCoord& h )
{
return mark.data[h.m][h.n].f != -1;
}
};
template
AStar::~AStar()
{
for( int m = 1; m <= Map::MSize; ++m )
for( int n = 1; n <= Map::NSize; ++n )
dirs.data[m][n] = mark.data[m][n].direction;
// Erase the mark array, for all items in open or visited
for( Container::iterator o = open.begin(); o != open.end(); ++o )
{
HexCoord h = (*o).h;
mark.data[h.m][h.n].direction = DirNone;
mark.data[h.m][h.n].f = -1;
mark.data[h.m][h.n].g = -1;
}
for( Container::iterator v = visited.begin(); v != visited.end(); ++v )
{
HexCoord h = (*v).h;
mark.data[h.m][h.n].direction = DirNone;
mark.data[h.m][h.n].g = -1;
assert( !in_open( h ) );
}
}
template
Container::iterator AStar::find_in_open( HexCoord hn )
{
// Only search for this node if we know it's in the OPEN set
if( Map::valid(hn) && in_open(hn) )
{
for( Container::iterator i = open.begin(); i != open.end(); ++i )
{
stats.nodes_searched++;
if( (*i).h == hn )
return i;
}
}
return open.end();
}
// This is the 'propagate down' stage of the algorithm, which I'm not
// sure I did right.
template
void AStar::propagate_down( Node H )
{
// Keep track of the nodes that we still have to consider
Container examine;
while( !examine.empty() )
{
// Remove one node from the list
Node N = examine.back();
examine.pop_back();
// Examine its neighbors
for( int dir = 0; dir < 6; ++dir )
{
HexDirection d = HexDirection(dir);
HexCoord hn = Neighbor( N.h, d );
if( in_open(hn) )
{
// This node is in OPEN
int new_g = N.gval + heuristic.kost( map, N.h, d, hn );
// Compare this `g' to the stored `g' in the array
if( new_g < mark.data[hn.m][hn.n].g )
{
Container::iterator i = find_in_open( hn );
assert( i != open.end() );
assert( mark.data[hn.m][hn.n].g == (*i).gval );
// Push this thing UP in the heap (only up allowed!)
(*i).gval = new_g;
push_heap( open.begin(), i+1, comp );
// Set its direction to the parent node
mark.data[hn.m][hn.n].g = new_g;
mark.data[hn.m][hn.n].f = new_g + (*i).hval;
mark.data[hn.m][hn.n].direction = ReverseDirection(d);
// Now reset its parent
examine.push_back( (*i) );
}
else
{
// The new node is no better, so stop here
}
}
else
{
// Either it's in closed, or it's not visited yet
}
}
}
}
template
void AStar::find_path( vector& path )
{
Node N;
{
// insert the original node
N.h = A;
N.gval = 0;
N.hval = heuristic.dist(map,A,B);
open.push_back(N);
mark.data[A.m][A.n].f = N.gval+N.hval;
mark.data[A.m][A.n].g = N.gval;
stats.nodes_added++;
}
// * Things in OPEN are in the open container (which is a heap),
// and also their mark[...].f value is nonnegative.
// * Things in CLOSED are in the visited container (which is unordered),
// and also their mark[...].direction value is not DirNone.
// While there are still nodes to visit, visit them!
while( !open.empty() )
{
get_first( open, N );
mark.data[N.h.m][N.h.n].f = -1;
visited.push_back( N );
stats.nodes_removed++;
// If we're at the goal, then exit
if( N.h == B )
break;
// If we've looked too long, then exit
if( stats.nodes_removed >= heuristic.abort_path )
{
// Select a good element of OPEN
for( Container::iterator i = open.begin(); i != open.end(); ++i )
{
if( (*i).hval*2 + (*i).gval < N.hval*2 + N.gval )
N = *i;
}
B = N.h;
break;
}
// Every other column gets a different order of searching dirs
// (Alternatively, you could pick one at random). I don't want
// to be too biased by my choice of order in which I look at the
// neighboring grid spots.
int directions1[6] = {0,1,2,3,4,5};
int directions2[6] = {5,4,3,2,1,0};
int *directions;
if( (N.h.m+N.h.n) % 2 == 0 )
directions = directions1;
else
directions = directions2;
// Look at your neighbors.
for( int dci = 0; dci < 6; ++dci )
{
HexDirection d = HexDirection(directions[dci]);
HexCoord hn = Neighbor( N.h, d );
// If it's off the end of the map, then don't keep scanning
if( !map.valid(hn) )
continue;
int k = heuristic.kost(map, N.h, d, hn);
Node N2;
N2.h = hn;
N2.gval = N.gval + k;
N2.hval = heuristic.dist( map, hn, B );
// If this spot (hn) hasn't been visited, its mark is DirNone
if( mark.data[hn.m][hn.n].direction == DirNone )
{
// The space is not marked
mark.data[hn.m][hn.n].direction = ReverseDirection(d);
mark.data[hn.m][hn.n].f = N2.gval+N2.hval;
mark.data[hn.m][hn.n].g = N2.gval;
open.push_back( N2 );
push_heap( open.begin(), open.end(), comp );
stats.nodes_added++;
}
else
{
// We know it's in OPEN or CLOSED...
if( in_open(hn) )
{
// It's in OPEN, so figure out whether g is better
if( N2.gval < mark.data[hn.m][hn.n].g )
{
// Search for hn in open
Container::iterator find1 = find_in_open( hn );
assert( find1 != open.end() );
// Replace *find1's gval with N2.gval in the list&map
mark.data[hn.m][hn.n].direction = ReverseDirection(d);
mark.data[hn.m][hn.n].g = N2.gval;
mark.data[hn.m][hn.n].f = N2.gval+N2.hval;
(*find1).gval = N2.gval;
// This is allowed but it's not obvious why:
push_heap( open.begin(), find1+1, comp );
// (ask Amit if you're curious about it)
// This next step is not needed for most games
propagate_down( *find1 );
}
}
}
}
}
if( N.h == B && N.gval < MAXIMUM_PATH_LENGTH )
{
stats.path_cost = N.gval;
// We have found a path, so let's copy it into `path'
HexCoord h = B;
while( h != A )
{
HexDirection dir = mark.data[h.m][h.n].direction;
path.push_back( h );
h = Neighbor( h, dir );
stats.path_length++;
}
path.push_back( A );
// path now contains the hexes in which the unit must travel ..
// backwards (like a stack)
}
else
{
// No path
}
stats.nodes_left = open.size();
stats.nodes_visited = visited.size();
}
////////////////////////////////////////////////////////////////////////
// Specific instantiations of A* for different purposes
// UnitMovement is for moving units (soldiers, builders, firefighters)
struct UnitMovement
{
HexCoord source;
Unit* unit;
int abort_path;
inline static int dist( const HexCoord& a, const HexCoord& b )
{
// The **Manhattan** distance is what should be used in A*'s heuristic
// distance estimate, *not* the straight-line distance. This is because
// A* wants to know the estimated distance for its paths, which involve
// steps along the grid. (Of course, if you assign 1.4 to the cost of
// a diagonal, then you should use a distance function close to the
// real distance.)
// Here I compute a ``Manhattan'' distance for hexagons. Nifty, eh?
int a1 = 2*a.m;
int a2 = 2*a.n+a.m%2 - a.m;
int a3 = -2*a.n-a.m%2 - a.m; // == -a1-a2
int b1 = 2*b.m;
int b2 = 2*b.n+b.m%2 - b.m;
int b3 = -2*b.n-b.m%2 - b.m; // == -b1-b2
// One step on the map is 10 in this function
return 5*max(abs(a1-b1), max(abs(a2-b2), abs(a3-b3)));
}
inline int dist( Map& m, const HexCoord& a, const HexCoord& b )
{
double dx1 = a.x() - b.x();
double dy1 = a.y() - b.y();
double dx2 = source.x() - b.x();
double dy2 = source.y() - b.y();
double cross = dx1*dy2-dx2*dy1;
if( cross < 0 ) cross = -cross;
return dist( a, b ) + int(cross/20000);
}
inline int kost( Map& m, const HexCoord& a,
HexDirection d, const HexCoord& b, int pd = -1 )
{
// This is the cost of moving one step. To get completely accurate
// paths, this must be greater than or equal to the change in the
// distance function when you take a step.
if( pd == -1 ) pd = path_div;
// Check for neighboring moving obstacles
int occ = m.occupied_[b];
if( ( occ != -1 && m.units[occ] != unit ) &&
( !m.units[occ]->moving() || ( source == a && d != DirNone ) ) )
return MAXIMUM_PATH_LENGTH;
// Roads are faster (twice as fast), and cancel altitude effects
Terrain t1 = m.terrain(a);
Terrain t2 = m.terrain(b);
// int rd = int((t2==Road||t2==Bridge)&&(t1==Road||t2==Bridge));
// It'd be better theoretically for roads to work only when both
// hexes are roads, BUT the path finder works faster when
// it works just when the destination is a road, because it can
// just step onto a road and know it's going somewhere, as opposed
// to having to step on the road AND take another step.
int rd = int(t2==Road || t2==Bridge);
int rdv = ( 5 - 10 * rd ) * (pd - 3) / 5;
// Slow everyone down on gates, canals, or walls
if( t2 == Gate || t2 == Canal )
rdv += 50;
if( t2 == Wall )
rdv += 150;
// Slow down everyone on water, unless it's on a bridge
if( t2 != Bridge && m.water(b) > 0 )
rdv += 30;
// If there's no road, I take additional items into account
if( !rd )
{
// One thing we can do is penalize for getting OFF a road
if( t1==Road || t1==Bridge )
rdv += 15;
// I take the difference in altitude and use that as a cost,
// rounded down, which means that small differences cost 0
int da = (m.altitude(b)-m.altitude(a))/ALTITUDE_SCALE;
if( da > 0 )
rdv += da * (pd-3);
}
return 10 + rdv;
}
};
// Some useful functions are exported to be used without the pathfinder
int hex_distance( HexCoord a, HexCoord b )
{
return UnitMovement::dist( a, b );
}
int movement_cost( Map& m, HexCoord a, HexCoord b, Unit* unit )
{
UnitMovement um;
um.unit = unit;
return um.kost( m, a, DirNone, b, 8 );
}
// BuildingMovement is for drawing straight lines (!)
struct BuildingMovement
{
HexCoord source;
int abort_path;
inline int dist( Map& m, const HexCoord& a, const HexCoord& b )
{
double dx1 = a.x() - b.x();
double dy1 = a.y() - b.y();
double dd1 = dx1*dx1+dy1*dy1;
// The cross product will be high if two vectors are not colinear
// so we can calculate the cross product of [current->goal] and
// [source->goal] to see if we're staying along the [source->goal]
// vector. This will help keep us in a straight line.
double dx2 = source.x() - b.x();
double dy2 = source.y() - b.y();
double cross = dx1*dy2-dx2*dy1;
if( cross < 0 ) cross = -cross;
return int( dd1 + cross );
}
inline int kost( Map& m, const HexCoord& a,
HexDirection d, const HexCoord& b )
{
return 0;
}
};
// Flat Canal movement tries to find a path for a canal that is not too steep
struct FlatCanalPath: public UnitMovement
{
int kost( Map& m, const HexCoord& a,
HexDirection d, const HexCoord& b )
{
// Try to minimize the slope
int a0 = m.altitude(a);
int bda = 0;
for( int dir = 0; dir < 6; ++dir )
{
int da = a0-m.altitude( Neighbor(a,HexDirection(dir)) );
if( da > bda ) bda = da;
}
return 1 + 100*bda*bda;
}
};
//////////////////////////////////////////////////////////////////////
// These functions call AStar with the proper heuristic object
PathStats FindUnitPath( Map& map, HexCoord A, HexCoord B,
vector& path, Unit* unit, int cutoff )
{
UnitMovement um;
um.source = A;
um.unit = unit;
um.abort_path = cutoff * hex_distance(A,B) / 10;
AStar finder( um, map, A, B );
// If the goal node is unreachable, don't even try
if( um.kost( map, A, DirNone, B ) == MAXIMUM_PATH_LENGTH )
{
// Check to see if a neighbor is reachable. This is specific
// to SimBlob and not something for A* -- I want to find a path
// to a neighbor if the original goal was unreachable (perhaps it
// is occupied or unpassable).
int cost = MAXIMUM_PATH_LENGTH;
HexCoord Bnew = B;
for( int d = 0; d < 6; ++d )
{
HexCoord hn = Neighbor( B, HexDirection(d) );
int c = um.kost( map, A, DirNone, hn );
if( c < cost )
{
// This one is closer, hopefully
Bnew = B;
cost = c;
}
}
// New goal
B = Bnew;
if( cost == MAXIMUM_PATH_LENGTH )
{
// No neighbor was good
return finder.stats;
}
}
finder.find_path( path );
return finder.stats;
}
PathStats FindBuildPath( Map& map, HexCoord A, HexCoord B,
vector& path )
{
BuildingMovement bm;
bm.source = A;
AStar finder( bm, map, A, B );
finder.find_path( path );
return finder.stats;
}
PathStats FindCanalPath( Map& map, HexCoord A, HexCoord B,
vector& path )
{
FlatCanalPath fcp;
fcp.source = A;
AStar finder( fcp, map, A, B );
finder.find_path( path );
return finder.stats;
}