GeneralizedAStar.h

#ifndef AI_GENERALIZED_ASTAR_H
#define AI_GENERALIZED_ASTAR_H

#include <vector>
#include <map>
#include <KrisLibrary/utils/stl_tr1.h>
#include <KrisLibrary/structs/IndexedPriorityQueue.h>

namespace AI {

template <class S,class C>
struct GeneralizedAStar
{
  //a node in the tree
  struct Node
  {
    Node():parent(NULL) {}
    ~Node() { for(size_t i=0;i<children.size();i++) delete children[i]; }

    C g;
    C f;
    S data;
    Node* parent;
    std::vector<Node*> children;
  };

  GeneralizedAStar();
  GeneralizedAStar(const S& start);
  virtual ~GeneralizedAStar() {}
  void SetStart(const S& start);
  bool Search();
  bool SearchStep();
  bool SearchFailed();
  int NumNodes() const;
  int NumExpanded() const;
  int NumDescendents(const Node& n) const;
  C TopPriority() const;

  inline bool GoalFound() const { return !path.empty(); }
  inline C GoalCost() const { if(goal != NULL) return goal->g; return zero; }
  inline const std::vector<S>& GoalPath() const { return path; }

  virtual bool IsGoal(const S& s) =0;
  virtual void Successors(const S& s,std::vector<S>& successors,std::vector<C>& costs) =0;
  virtual C Heuristic(const S& s) { return zero; }
  virtual void ClearVisited() {}
  virtual void Visit(const S& s,Node* n) {}
  virtual Node* VisitedStateNode(const S& s) { return NULL; }
  virtual bool OnExpand(Node* n) { return true; }

  bool testGoalOnGeneration;

  C zero;

  Node root;

  IndexedPriorityQueue<Node*,std::pair<C,C> > fringe;
  std::vector<S> successors;
  std::vector<C> costs;
  int numNodes;

  Node* goal;
  std::vector<S> path;
};

template <class S>
struct AStar : public GeneralizedAStar<S,double>
{
  typedef struct GeneralizedAStar<S,double>::Node Node;
  AStar() { this->zero = 0.0; }
  virtual ~AStar() {}
};

template <class S>
struct IntegerAStar : public GeneralizedAStar<S,int>
{
  typedef struct GeneralizedAStar<S,int>::Node Node;
  IntegerAStar() { this->zero = 0; }
  virtual ~IntegerAStar() {}
};

template <class S,class C>
class GeneralizedAStarWithMap : public GeneralizedAStar<S,C>
{
 public:
  typedef struct GeneralizedAStar<S,C>::Node Node;
  std::map<S,Node*> visited;
  virtual ~GeneralizedAStarWithMap() {}
  virtual void ClearVisited() { visited.clear(); }
  virtual void Visit(const S& s,Node* n) { visited[s]=n;}
  virtual Node* VisitedStateNode(const S& s) {
    typename std::map<S,Node*>::const_iterator i=visited.find(s);
    if(i==visited.end()) return NULL;
    return i->second;
  }
};

template <class S,class C>
class GeneralizedAStarWithHashMap : public GeneralizedAStar<S,C>
{
 public:
  typedef struct GeneralizedAStar<S,C>::Node Node;
  UNORDERED_MAP_TEMPLATE<S,Node*> visited;
  virtual ~GeneralizedAStarWithHashMap() {}
  virtual void ClearVisited() { visited.clear(); }
  virtual void Visit(const S& s,Node* n) { visited[s]=n;}
  virtual Node* VisitedStateNode(const S& s) {
    typename UNORDERED_MAP_TEMPLATE<S,Node*>::const_iterator i=visited.find(s);
    if(i==visited.end()) return NULL;
    return i->second;
  }
};


template <class S,class C>
GeneralizedAStar<S,C>::GeneralizedAStar()
  :testGoalOnGeneration(false),numNodes(0),goal(NULL)
{
}

template <class S,class C>
GeneralizedAStar<S,C>::GeneralizedAStar(const S& start)
  :testGoalOnGeneration(false),numNodes(0),goal(NULL)
{
  SetStart(start);
}

template <class S,class C>
void GeneralizedAStar<S,C>::SetStart(const S& start)
{  
  ClearVisited();
  fringe.clear();
  goal = NULL;
  path.resize(0);
  numNodes = 1;

  if(testGoalOnGeneration) {
    if(IsGoal(start)) {
      path.push_back(start);
      return;
    }
  }

  //initialize with the root node
  root.g=zero;
  root.f=Heuristic(start);
  root.data = start;
  root.parent = NULL;
  for(size_t i=0;i<root.children.size();i++)
    delete root.children[i];
  root.children.clear();
  fringe.insert(&root,std::pair<C,C>(root.f,-root.g));
  Visit(start,&root);
}

template <class S,class C>
bool GeneralizedAStar<S,C>::Search()
{
  while(!fringe.empty()) {
    bool res=SearchStep();
    if(res) return true;
  }
  return false;
}

template <class S,class C>
bool GeneralizedAStar<S,C>::SearchStep()
{
  if(fringe.empty()) return false;

  Node* n = fringe.top().second;
  fringe.pop();

  //give the subclass optional feedback
  if(!OnExpand(n)) return false;

  if(!testGoalOnGeneration) {
    if(IsGoal(n->data)) {  //reached the goal!
      goal = n;
      path.resize(0);
      while(n != NULL) {
        path.push_back(n->data);
        n = n->parent;
      }
      //path is in backwards order, reverse it
      std::reverse(path.begin(),path.end());  
      return true;
    }
  }

  successors.resize(0);
  costs.resize(0);
  Successors(n->data,successors,costs);
  Assert(successors.size()==costs.size());
  /*
  for(size_t i=0;i<costs.size();i++)
    Assert(costs[i] >= zero);
  */

  n->children.resize(0);
  n->children.reserve(successors.size());
  for(size_t i=0;i<successors.size();i++) {
    if(testGoalOnGeneration) {
      if(IsGoal(successors[i])) {  //reached the goal!
        goal = NULL;
        path.resize(0);
        path.push_back(successors[i]);
        while(n != NULL) {
          path.push_back(n->data);
          n = n->parent;
        }
        //path is in backwards order, reverse it
        std::reverse(path.begin(),path.end());  
        return true;
      }
    }
    Node* visited = VisitedStateNode(successors[i]);
    if(visited) {
      if(n->g + costs[i] < visited->g) {
        //cost is lower than previous, keep new state
        //reparent the node
        Node* p=visited->parent;
        Assert(p!=NULL);
        for(size_t c=0;c<p->children.size();c++)
          if(visited == p->children[c]) {
            p->children[c] = p->children.back();
            p->children.resize(p->children.size()-1);
            break;
          }
        n->children.push_back(visited);
        visited->g = n->g + costs[i];
        visited->f = visited->g + Heuristic(successors[i]);
        visited->parent = n;
        fringe.refresh(visited,std::pair<C,C>(visited->f,-visited->g));
      }
    }
    else {
      //add successors[i] to the child list
      numNodes ++;
      n->children.push_back(new Node);
      Node* child = n->children.back();
      child->data = successors[i];
      child->parent = n;
      child->g = n->g + costs[i];
      child->f = child->g + Heuristic(successors[i]);

      //add successors[i] to the fringe and mark as visited
      fringe.insert(child,std::pair<C,C>(child->f,-child->g));
      Visit(successors[i],child);
    }
  }
  return false;
}

template <class S,class C>
bool GeneralizedAStar<S,C>::SearchFailed()
{
  return fringe.empty();
}

template <class S,class C>
int GeneralizedAStar<S,C>::NumNodes() const
{
  return numNodes;
  //return 1 + NumDescendents(root);
}

template <class S,class C>
int GeneralizedAStar<S,C>::NumExpanded() const
{
  return NumNodes()-(int)fringe.size();
}

template <class S,class C>
int GeneralizedAStar<S,C>::NumDescendents(const Node& n) const
{
  int count=(int)n.children.size();
  for(size_t i=0;i<n.children.size();i++)
    count += NumDescendents(*n.children[i]);
  return count;
}

template <class S,class C>
C GeneralizedAStar<S,C>::TopPriority() const
{
  if(fringe.empty()) return zero;
  return fringe.top().first.first;
}

} //namespace AI

#endif