Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

AStar.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_AStar_h_
00003 #define INCLUDED_AStar_h_
00004 
00005 #include <vector>
00006 #include <deque>
00007 #include <set>
00008 #include <algorithm>
00009 #include <functional>
00010 #include <tr1/unordered_set>
00011 
00012 //! Holds data structures for search context, results, and implementation of A★ path planning algorithm, see AStar::astar
00013 namespace AStar {
00014 
00015   //! Stores user state along with search context
00016   template<class State>
00017   struct Node {
00018     //! constructor, pass parent node @a p, cost so far @a c, remaining cost heuristic @a r, and user state @a st
00019     Node(const Node* p, float c, float r, const State& st) : parent(p), cost(c), remain(r), total(c+r), state(st) {}
00020     
00021     const Node* parent; //!< source for this search node
00022     float cost; //!< cost to reach this node from start state
00023     float remain; //!< estimated cost remaining to goal
00024     float total; //!< cached value of #cost + #remain
00025     State state; //!< user state
00026     
00027     //! Search nodes should be sorted based on total cost (#total)
00028     /*! Note the inverted comparison, STL heap operations put max at the heap root, but we want the min */
00029     struct CostCmp : public std::binary_function<Node*, Node*, bool> {
00030       bool operator()(const Node* left, const Node* right) const {
00031         //return left->total > right->total; // or should cost so far be a tie breaker?
00032         return left->total > right->total || ( left->total==right->total && left->cost < right->cost );
00033       }
00034     };
00035   };
00036 
00037   //! For efficient lookup of existance of states in open or closed list, uses user's Cmp on the user state within the search node
00038   template<class State, class Cmp>
00039   struct StateCmp : public std::binary_function<Node<State>*, Node<State>*, bool> {
00040     bool operator()(const Node<State>* left, const Node<State>* right) const {
00041       return Cmp()(left->state,right->state);
00042     }
00043   };
00044   
00045   //! Calls State::hash(), which should return value distributed over size_t
00046   template<class State>
00047   struct StateHash : public std::unary_function<Node<State>*,size_t> {
00048     size_t operator()(const Node<State>* n) const {
00049       return n->state.hash();
00050     }
00051   };
00052   
00053   //! Tests equality of states using State::operator==
00054   template<class State>
00055   struct StateEq : public std::binary_function<Node<State>*, Node<State>*, bool> {
00056     bool operator()(const Node<State>* left, const Node<State>* right) const {
00057       return left->state == right->state;
00058     }
00059   };
00060   
00061   //! Search results, including unexpanded nodes
00062   /*! Returning full search state for reporting of search statistics and 
00063    *  also future expansion to support efficient re-planning, as well
00064    *  as to allow user to free resources in State (e.g. if State is a pointer type) */
00065   template<class State, class Cmp=std::less<State> >
00066   struct Results {
00067     typedef Node<State> Node;
00068     typedef std::tr1::unordered_set<Node*, StateHash<State>, StateEq<State> > NodeSet;
00069     
00070     typedef typename std::vector<State>::iterator path_iterator;
00071     typedef typename std::vector<State>::const_iterator path_const_iterator;
00072     typedef typename NodeSet::iterator set_iterator;
00073     typedef typename NodeSet::const_iterator set_const_iterator;
00074     typedef typename std::deque<Node*>::iterator priority_iterator;
00075     typedef typename std::deque<Node*>::const_iterator priority_const_iterator;
00076     
00077     float cost;
00078     std::vector<State> path;
00079     NodeSet closed;
00080     NodeSet open;
00081     std::deque<Node*> priorities;
00082     ~Results() {
00083       // don't need to delete from priorities... everything is either open or closed
00084       /*for(typename std::deque<Node*>::const_iterator it=priorities.begin(); it!=priorities.end(); ++it)
00085         delete *it;*/
00086       priorities.clear();
00087       for(set_const_iterator it=open.begin(); it!=open.end(); ++it)
00088         delete *it;
00089       open.clear();
00090       for(set_const_iterator it=closed.begin(); it!=closed.end(); ++it)
00091         delete *it;
00092       closed.clear();
00093     }
00094   };
00095   
00096   //! A★ search using custom comparison on State type
00097   /*! Expand is expected to be compatible with const std::vector<std::pair<float,State> >& (Context::*expand)(const State& st, const State& goal) const \n
00098    * Heuristic is expected to be compatible with float (Context::*heuristic)(const State& st, const State& goal) const \n
00099    * The Cmp argument is not actually used, but the function accepts an instance so you can avoid specifying all of the template parameters */
00100   template<class Context, class State, class Expand, class Heuristic, class Validate, class Cmp>
00101   Results<State,Cmp>
00102   astar(const Context& ctxt, const State& initial, const State& goal, Expand expand, Heuristic heuristic, Validate validate, const Cmp&, float bound=0) {
00103     typedef Node<State> Node;
00104     typedef StateCmp<State,Cmp> StateCmp;
00105     Results<State,Cmp> results;
00106     typename Results<State,Cmp>::NodeSet& closed = results.closed;
00107     typename Results<State,Cmp>::NodeSet& open = results.open;
00108     std::deque<Node*>& priorities = results.priorities;
00109     typename Node::CostCmp costCmp;
00110     
00111     {
00112       Node* n = new Node(NULL,0,(ctxt.*heuristic)(initial,goal),initial);
00113       open.insert(n);
00114       priorities.push_back(n);
00115     }
00116     
00117     while(!open.empty()) {
00118       Node* cur = priorities.front();
00119       bool valid = (ctxt.*validate)(cur->state);
00120       
00121       if(valid && cur->state == goal) { // or test (cur->remain==0) ?
00122         reconstruct(cur,results.path);
00123         results.cost = cur->cost;
00124         return results;
00125       }
00126       
00127       std::pop_heap(priorities.begin(),priorities.end(),costCmp);
00128       priorities.pop_back();
00129       open.erase(cur);
00130       closed.insert(cur);
00131       if(!valid)
00132         continue;
00133       //std::cout << "Closing " << cur->state << " cost " << cur->cost << " total " << cur->total << std::endl;
00134       
00135       const State* parentState = (cur->parent!=NULL) ? &cur->parent->state : static_cast<State*>(NULL);
00136       const std::vector<std::pair<float,State> >& neighbors = (ctxt.*expand)(parentState,cur->state, goal);
00137       for(typename std::vector<std::pair<float,State> >::const_iterator it=neighbors.begin(); it!=neighbors.end(); ++it) {
00138         Node n(cur, cur->cost+it->first, (ctxt.*heuristic)(it->second,goal), it->second);
00139         if(closed.count(&n) > 0 || (bound>0 && n.total>bound))
00140           continue;
00141         typename Results<State,Cmp>::set_const_iterator op = open.find(&n);
00142         if(op == open.end()) {
00143           // new node, add it
00144           Node * hn = new Node(n); // clone to get heap storage
00145           open.insert(hn);
00146           //std::cout << "open insert " << hn->cost << ' ' << hn->remain << ' ' << hn->total << ' ' << open.size() << std::endl;
00147           priorities.push_back(hn);
00148           std::push_heap(priorities.begin(), priorities.end(),costCmp);
00149         } else if(n.cost < (*op)->cost) {
00150           // better path to previous node, update it
00151           **op = n;
00152           std::make_heap(priorities.begin(), priorities.end(),costCmp); // can we do something more efficient?
00153         } else {
00154           // we can already do better, drop new node
00155           // since we used stack allocation for n, this is a no-op
00156         }
00157       }
00158     }
00159     return results;
00160   }
00161   
00162   //! A★ search using operator< to sort user State type
00163   /*! Expand is expected to be compatible with const std::vector<std::pair<float,State> >& (Context::*expand)(const State& st, const State& goal) const \n
00164    * Heuristic is expected to be compatible with float (Context::*heuristic)(const State& st, const State& goal) const */
00165   template<class Context, class State, class Expand, class Heuristic, class Validate>
00166   Results<State, std::less<State> >
00167   astar(const Context& ctxt, const State& initial, const State& goal, Expand expand, Heuristic heuristic, Validate validate, float bound=0) {
00168     return astar(ctxt,initial,goal,expand,heuristic,validate,std::less<State>(),bound);
00169   }
00170   
00171   //! A★ search using operator< to sort user State type, assumes Context has functions named "expand" and "heuristic"
00172   template<class Context, class State>
00173   Results<State, std::less<State> >
00174   astar(const Context& ctxt, const State& initial, const State& goal, float bound=0) {
00175     return astar(ctxt,initial,goal,&Context::expand,&Context::heuristic,&Context::validate,std::less<State>(),bound);
00176   }
00177   
00178   //! constructs @a path by following parent pointers from @a n; the specified node is included in the path
00179   template<class Node, class State>
00180   void reconstruct(const Node* n, std::vector<State>& path, size_t depth=1) {
00181     if(n->parent != NULL)
00182       reconstruct(n->parent, path, depth+1);
00183     else
00184       path.reserve(depth);
00185     path.push_back(n->state);
00186   }
00187 
00188 }
00189 
00190 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:36 2016 by Doxygen 1.6.3