Skip to content

Instantly share code, notes, and snippets.

@codemonkey-uk
Last active May 27, 2024 01:48
Show Gist options
  • Save codemonkey-uk/8240501 to your computer and use it in GitHub Desktop.
Save codemonkey-uk/8240501 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.
// -------------------------------------------------------------------------
// Filename: astar.h
// Version: 3
// Purpose: Provide template for a* algorithm
// (c) T.Frogley 1999-2021
// Stable and used in many projects 2003-2021+
// Updated: 2021 to better support behaviour controls, & unordered_set
// -------------------------------------------------------------------------
#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 <unordered_set>
#include <functional>
#include <algorithm>
#include <limits>
// #define ASTAR_STATS
#ifdef ASTAR_STATS
#include <time.h>
#include <iostream>
#endif
namespace astar
{
//from <vector>
using std::vector;
//from <unordered_set>
using std::unordered_set;
//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 examples, 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 (fabs(x1-x2)+fabs(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(fabs(x1-x2),fabs(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 T value_type;
typedef typename T::iterator::cost_type scalar;
node_link(){}
node_link(T n, scalar h, const node_link* p):
myNode(n),
myH(h),
myStatus(eNodeNotListed),
myG(),
myParent(&*p)
{ }
node_link(T n, scalar g, scalar h, const node_link* p):
myNode(n),
myH(h),
myStatus(eNodeNotListed),
myG(g),
myParent(&*p)
{ }
// for use by the node-set
inline bool operator<(const node_link<T>& rhs)const noexcept
{ return myNode.hash() < rhs.myNode.hash(); }
inline bool operator==(const node_link<T>& rhs)const noexcept
{ return myNode == rhs.myNode; }
// for sorting in the pending queue
inline bool costs_more(const node_link<T> &b)const noexcept
{ return (myG+myH > b.myG+b.myH); }
inline bool has_parent() const noexcept
{ return myParent!=nullptr; }
T myNode;
scalar myH;
enum NodeList { eNodePending, eNodeDone, eNodeNotListed };
mutable NodeList myStatus;
mutable scalar myG;
// mutable typename container::iterator myParent;
mutable const node_link* myParent;
static const size_t sOrphan = static_cast<size_t>(-1);
};
//binary_lookup functor (astar implemetatoin helper class)
// key : key to container
template<class key>
class binary_lookup : public binary_function<key, key, bool> {
public:
//constructor, take a copy of functor,
//and keep a reference to the container
binary_lookup()
{ }
//look up two values in contianer from keys a and b,
//pass values to binary functor, and return result
bool operator()(
const key& a,
const key& b )
//{ return fn(c[a], c[b]); }
{return a->costs_more(*b); }
};
//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<size_t>::max()),
cost_limit(std::numeric_limits<scalar>::max()),
result_nodes_examined(0),
result_nodes_pending(0),
result_nodes_opened(0),
route_length(0),
route_cost()
{ }
//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
size_t 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
size_t result_nodes_examined;
//result_nodes_pending
//astar sets this to the number of nodes still waiting
//to be examined when the search terminated
size_t result_nodes_pending;
//result_nodes_opened
//astar sets this to the total number of nodes it fetched
//should equal pending + examined
size_t 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 behaviour (providing 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 behaviourType, class nodeType, class container>
bool astar(const nodeType a, const nodeType b, container &results, behaviourType behaviour, config<nodeType> &cfg)
{
#ifdef ASTAR_STATS
clock_t time = clock();
#endif
typedef node_link<nodeType> node;
typedef unordered_set<node> node_container;
typedef vector<typename node_container::iterator> index_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
typename node_container::iterator index;
bool complete = false;
//reserve space in index vectors to avoid reallocation
if (cfg.node_limit != std::numeric_limits<size_t>::max()){
pending.reserve( cfg.node_limit / 2 );
done.reserve( cfg.node_limit / 2 );
}
//create the indirect comparison object
binary_lookup<
typename node_container::iterator
> sort_index_object;
//tempory storage for 'working' node data
node current;
nodeType next;
//stick the fist node into the list,
//and its index into the pending list
index = nodes.insert( nodes.begin(), node( a, behaviour.heuristic(a,b), nullptr ));
pending.push_back( index );
index->myStatus = node::eNodePending;
do{
//get top rated node
index = pending.front();
current = *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);
index->myStatus = node::eNodeDone;
//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();
if (behaviour.filter(next)) continue;
typename node_container::iterator inserted_at;
scalar g = i.cost()+current.myG;
inserted_at = nodes.insert( nodes.begin(), node(next, g, behaviour.heuristic(next, b), &*index ) );
//not in the pending list
if (inserted_at->myStatus != node::eNodePending){
//not in done list (or pending list)
if (inserted_at->myStatus != node::eNodeDone){
//add to pending list
pending.push_back( inserted_at );
inserted_at->myStatus = node::eNodePending;
push_heap( pending.begin(), pending.end(), sort_index_object );
}
}
else{
//its in the pending list, but is this a better version ?
if (g<(inserted_at)->myG){
// This is allowed but it's not obvious why:
//replace node cost with new path to this node
(inserted_at)->myG = g;
(inserted_at)->myParent = &*index;
// see: http://theory.stanford.edu/~amitp/GameProgramming/path.cpp
push_heap(
pending.begin(),
std::find(pending.begin(), pending.end(), inserted_at)+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 = *pending.front();
//search list of already explored nodes for "better" compromise
typename index_container::const_iterator j=done.begin();
typename index_container::const_iterator k=done.end();
for(;j!=k;++j){
if (current.myH>(*j)->myH){
current = **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.has_parent()){
current = *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
namespace std
{
template<typename T> struct hash< astar::node_link<T> >
{
std::size_t operator()(astar::node_link<T> const& node) const noexcept
{
return node.myNode.hash();
}
};
}
#endif
@zk4
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