Skip to content

Instantly share code, notes, and snippets.

Embed
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

This comment has been minimized.

Copy link

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
You can’t perform that action at this time.