Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
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.
// -------------------------------------------------------------------------
// 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

zk4 commented May 28, 2014

A little complicated ,but, great job!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment