-
-
Save MORTAL2000/4ab1b4baa9e6cc5e622967b19874d399 to your computer and use it in GitHub Desktop.
The C++ header file "astar.h" implements the A* graph search (route finding) algorithm using a C++ function template. The interface is written in an STL like style.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// ------------------------------------------------------------------------- | |
// Filename: astar.h | |
// Version: 1.24 | |
// Date: 2002/03/08 | |
// Purpose: Provide template for a* algorythm | |
// (c) T.Frogley 1999-2002 | |
// ------------------------------------------------------------------------- | |
#ifndef ASTAR_H | |
#define ASTAR_H | |
#ifdef _MSC_VER | |
//identifier was truncated to '255' characters in the browser information | |
#pragma warning (disable : 4786) | |
#endif | |
//include standard library code | |
#include <vector> | |
#include <deque> | |
#include <functional> | |
#include <algorithm> | |
#include <limits> | |
#ifdef ASTAR_STATS | |
#include <time.h> | |
#include <iostream> | |
#endif | |
namespace astar | |
{ | |
//from <vector> | |
using std::vector; | |
//from <deque> | |
using std::deque; | |
//from <functional> | |
using std::binary_function; | |
using std::greater; | |
//from <algorithm> | |
using std::pop_heap; | |
using std::push_heap; | |
//max should be in <algorithm> | |
//see: http://www.sgi.com/Technology/STL/max.html | |
//unfortunatly the Microsoft compiler\headers arn't standard compliant | |
//see: http://x42.deja.com/getdoc.xp?AN=520752890 | |
//thus: | |
#ifdef _MSC_VER | |
#ifdef max | |
#undef max | |
#endif | |
template<class T> inline | |
const T& max(const T& a, const T& b) | |
{ return (a>b)?a:b; } | |
#else | |
using std::max; | |
#endif | |
//node = class encapsulating a location on the graph | |
/* example node class for use with astar, | |
minimal interface - | |
note it is recomended that in most cases nodes are implemented as pointers to data, | |
struct example_node{ | |
//default, & copy constructor, & | |
//assignment operator should be available | |
//example_node::iterator | |
//for fetching connected nodes, and costs | |
struct iterator{ | |
//copy constructor and assignment operator should be available | |
typedef double cost_type; //typedef required, must be scalar type | |
example_node value()const; //node | |
cost_type cost()const; //cost/distance to node | |
iterator& operator++(); //next node | |
bool operator!=(iterator v);//used by search | |
}; | |
//Get first, and past-end iterators | |
iterator begin()const; | |
iterator end()const; | |
//equality operator, required | |
//note: fuzzy equality may be useful | |
bool operator==(const xynode b); | |
}; | |
*/ | |
//heuristic = binary functor, estimates of cost from node A to node B | |
//use this heuristic when costs don't apply | |
template<class T> | |
struct no_heuristic{ | |
//heuristic(); | |
typename T::iterator::cost_type operator()(const T a, const T b) | |
{ return 1; } | |
}; | |
//some useful template functions for creating heuristics for movement on a 2d plane | |
//reference: http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html | |
//The standard heuristic is the Manhattan distance. | |
//Look at your cost function and see what the least cost is | |
//for moving from one space to another. | |
//The heuristic should be cost times manhattan distance: | |
template<class T> inline | |
const T manhattan_distance(const T& x1, const T& y1, const T& x2, const T& y2) | |
{ | |
return (abs(x1-x2)+abs(y1-y2)); | |
} | |
//If on your map you allow diagonal movement, then you need a different heuristic. | |
//The Manhattan distance for (4 east, 4 north) will be 8. | |
//However, you could simply move (4 northeast) instead, so the heuristic should be 4. | |
//This function handles diagonals: | |
template<class T> inline | |
const T diagonal_distance(const T& x1, const T& y1, const T& x2, const T& y2) | |
{ | |
return (max(abs(x1-x2),abs(y1-y2))); | |
} | |
//If your units can move at any angle (instead of grid directions), | |
//then you should probably use a straight line distance: | |
template<class T> inline | |
const T straight_distance(const T& x1, const T& y1, const T& x2, const T& y2) | |
{ | |
T dx = (x1-x2); | |
T dy = (y1-y2); | |
return ( sqrt(dx*dx + dy*dy) ); | |
} | |
//One thing that can lead to poor performance is ties in the heuristic. | |
//When several paths have the same f value, they are all explored, even | |
//though we only need to explore one of them. To solve this problem, we | |
//can add a small tie-breaker to the heuristic. | |
//This tie breaker also can give us nicer looking paths: | |
//x1,y1 = start position | |
//x2,y2 = current position | |
//x3,y3 = target position | |
template<class T> inline | |
const T amits_modifier(const T& x1, const T& y1, const T& x2, const T& y2, const T& x3, const T& y3) | |
{ | |
T dx1 = x2 - x3; | |
T dy1 = y2 - y3; | |
T dx2 = x1 - x3; | |
T dy2 = y1 - y3; | |
T cross = dx1*dy2 - dx2*dy1; | |
if( cross<0 ) cross = -cross; | |
return cross; | |
} | |
//node_link (astar implementation helper class) | |
//wraps up a node with movement cost / heuristic info | |
//and an index to its parent node in the nodes list | |
template<class T> | |
struct node_link{ | |
typedef typename T::iterator::cost_type scalar; | |
node_link(){} | |
node_link(T n, scalar g, scalar h, int p=-1): | |
myNode(n), | |
myG(g), | |
myH(h), | |
myParent(p) | |
{ } | |
inline bool operator>(const node_link<T> &b)const | |
{ return (myG+myH > b.myG+b.myH); } | |
T myNode; | |
scalar myG, myH; | |
int myParent; | |
}; | |
//binary_lookup functor (astar implemetatoin helper class) | |
// bfn : binary functor to pass value to | |
// key : key to container | |
// con : random access container, must support [ key ] | |
//??? Why doesn't this work with c-style arrays ??? | |
template<class bfn, class key, class con> | |
class binary_lookup : public binary_function<key, key, typename bfn::result_type> { | |
public: | |
//constructor, take a copy of functor, | |
//and keep a reference to the container | |
binary_lookup(const bfn& f, con& _c): | |
fn(f), | |
c(_c) | |
{ } | |
//look up two values in contianer from keys a and b, | |
//pass values to binary functor, and return result | |
result_type operator()( | |
const key& a, | |
const key& b ) | |
{ return fn(c[a], c[b]); } | |
protected: | |
bfn fn; | |
con& c; | |
}; | |
//configuration info for astar algorythm | |
//also used to return some additional information about the finished search | |
template<class nodeType> | |
struct config{ | |
typedef typename nodeType::iterator::cost_type scalar; | |
//construct with sensable defaults / empty results | |
config(): | |
node_limit(std::numeric_limits<unsigned int>::max()), | |
cost_limit(std::numeric_limits<scalar>::max()), | |
result_nodes_opened(0), | |
result_nodes_pending(0), | |
result_nodes_examined(0), | |
route_length(0), | |
route_cost(0) | |
{ } | |
//configuration variables | |
//node_limit | |
//set this to restrict the number of nodes astar will open | |
//has the effect of limiting the amount of time spent searching | |
unsigned int node_limit; | |
//cost limit | |
//set this to restrict the maximum distance considered | |
//acceptable for a route | |
//if astar cannot find a shorter route than this it will fail | |
scalar cost_limit; | |
//result variables | |
//result_nodes_examined | |
//astart sets the to the total number of nodes it | |
//'looked at' when the search terminated | |
unsigned int result_nodes_examined; | |
//result_nodes_pending | |
//astar sets this to the number of nodes still waiting | |
//to be examined when the search terminated | |
unsigned int result_nodes_pending; | |
//result_nodes_opened | |
//astar sets this to the total number of nodes it fetched | |
//should equal pending + examined | |
unsigned int result_nodes_opened; | |
//route_length | |
//astar sets this to equal the total number of nodes | |
//used to construct the returned route | |
unsigned int route_length; | |
//route_cost | |
//astart sets the to equal the total "cost" of | |
//the returned route | |
scalar route_cost; | |
}; | |
//astar algorithm, as template, | |
//find a path from a to b, | |
//using the given heuristic, | |
//places results in container [ any class that can push_front( nodeType ) ] | |
//returns flase if no route exists | |
//returns true if a complete route is found | |
//returns true if it exceeds node_limit, but a partial route is found | |
//returns false (aborts with a partial route) if it exceeds cost_limit | |
template<class heuristicFunctor, class nodeType, class container> | |
bool astar(const nodeType a, const nodeType b, container &results, heuristicFunctor heuristic, config<nodeType> &cfg) | |
{ | |
#ifdef ASTAR_STATS | |
clock_t time = clock(); | |
#endif | |
typedef node_link<nodeType> node; | |
typedef vector<int> index_container; | |
typedef deque<node> node_container; | |
typedef typename nodeType::iterator node_iterator; | |
typedef typename nodeType::iterator::cost_type scalar; | |
node_container nodes; //all nodes opened | |
index_container pending; //sorted index to pending nodes | |
index_container done; //unsorted index to nodes already explored | |
index_container::iterator j; | |
index_container::const_iterator k; | |
int index; | |
bool complete = false; | |
//reserve space in index vectors to avoid reallocation | |
if (cfg.node_limit != std::numeric_limits<unsigned int>::max()){ | |
pending.reserve( cfg.node_limit / 2 ); | |
done.reserve( cfg.node_limit / 2 ); | |
} | |
//create the indirect comparison object | |
greater<node> aFunctor; | |
binary_lookup< | |
greater<node>, | |
int, | |
node_container | |
> sort_index_object(aFunctor, nodes); | |
//tempory storage for 'working' node data | |
node current; | |
nodeType next; | |
//stick the fist node into the list, | |
//and its index into the pending list | |
nodes.push_back(node( a, 0, heuristic(a,b), -1 )); | |
pending.push_back(nodes.size()-1); | |
do{ | |
//get top rated node | |
index = pending.front(); | |
current = nodes[index]; | |
//remove it from pending list | |
pop_heap( pending.begin(), pending.end(), sort_index_object ); | |
pending.pop_back(); | |
//stick it in the list of examined nodes | |
done.push_back(index); | |
//found target? | |
complete = current.myNode==b; | |
if (complete) break; | |
//failed (based on distance) | |
if (current.myG+current.myH>cfg.cost_limit) break; | |
//for each node connected to the current one | |
node_iterator i(current.myNode.begin()); | |
const node_iterator end(current.myNode.end()); | |
for (;i!=end;++i){ | |
next = i.value(); | |
//!!! the client code might have a faster | |
//!!! way of checking if the node had already been visited | |
//search pending list for "the same" node | |
j=pending.begin(); | |
k=pending.end(); | |
for(;j!=k;++j){ | |
if (nodes[*j].myNode==next){ | |
break; | |
} | |
} | |
//not in the pending list | |
if (j==k){ | |
//search list of already explored nodes for "the same" node | |
j=done.begin(); | |
k=done.end(); | |
for(;j!=k;++j){ | |
if (nodes[*j].myNode==next){ | |
break; | |
} | |
} | |
//not in done list (or pending list) | |
if (j==k){ | |
//add to pending list | |
nodes.push_back( node(next, i.cost()+current.myG, heuristic(next, b), index ) ); | |
pending.push_back( nodes.size()-1 ); | |
push_heap( pending.begin(), pending.end(), sort_index_object ); | |
} | |
} | |
else{ | |
//its in the pending list, but is this a better version ? | |
if (i.cost()+current.myG<nodes[*j].myG){ | |
//replace node with this one | |
nodes[*j] = node(next, i.cost()+current.myG, nodes[*j].myH, index ); | |
// This is allowed but it's not obvious why: | |
// see: http://theory.stanford.edu/~amitp/GameProgramming/path.cpp | |
push_heap( pending.begin(), j+1, sort_index_object ); | |
} | |
} | |
} | |
}while (!pending.empty() && nodes.size()<cfg.node_limit); | |
#ifdef ASTAR_STATS | |
clock_t elapsed1 = clock() - time; | |
#endif | |
cfg.route_length = 0; | |
//did not exit because a route was found | |
if (!complete){ | |
//ran out of time | |
if (nodes.size()>=cfg.node_limit && !pending.empty()){ | |
//best potential | |
current = nodes[pending.front()]; | |
//search list of already explored nodes for "better" compromise | |
j=done.begin(); | |
k=done.end(); | |
for(;j!=k;++j){ | |
if (current.myH>nodes[*j].myH){ | |
current = nodes[*j]; | |
} | |
} | |
//partial routes should not be considered a failure | |
complete = true; | |
} | |
} | |
#ifdef ASTAR_STATS | |
elapsed1 = clock() - time; | |
#endif | |
//store route length | |
//(including estimate of remaining distance for partial routes) | |
cfg.route_cost = current.myG+current.myH; | |
//store results of search in "container" | |
++cfg.route_length; | |
results.push_front(current.myNode); | |
while(current.myParent!=-1){ | |
current = nodes[current.myParent]; | |
results.push_front(current.myNode); | |
++cfg.route_length; | |
} | |
//store stats for calling code | |
cfg.result_nodes_opened = nodes.size(); | |
cfg.result_nodes_pending = pending.size(); | |
cfg.result_nodes_examined = done.size(); | |
//report stats | |
#ifdef ASTAR_STATS | |
clock_t elapsed2 = clock()-time; | |
std::cout << "astar stats\n"; | |
std::cout << "ticks:\t\t" << elapsed2 << " (" << elapsed1 << ", " << elapsed2-elapsed1 << ")\n"; | |
std::cout << "(seconds):\t" << (float)elapsed2/CLOCKS_PER_SEC << "\n"; | |
std::cout << "route length:\t" << cfg.route_length << " nodes (" << cfg.route_cost << " units)\n"; | |
std::cout << "nodes examined:\t" << cfg.result_nodes_examined << "\n"; | |
std::cout << "nodes pending:\t" << cfg.result_nodes_pending << "\n"; | |
std::cout << "(total):\t" << cfg.result_nodes_opened << "\n"; | |
#endif | |
return complete; | |
}//void astar(...); | |
}//namespace astar | |
#endif | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment