graph_search.h

#ifndef DMP_GRAPH_SEARCH_H
#define DMP_GRAPH_SEARCH_H

#include <boost/heap/d_ary_heap.hpp>      // boost::heap::d_ary_heap
#include <memory>                         // std::shared_ptr
#include <limits>                         // std::numeric_limits
#include <vector>                         // std::vector
#include <unordered_map>                  // std::unordered_map

namespace DMP
{
  template <class T>
  struct compare_state
  {
    bool operator()(T a1, T a2) const
    {
      double f1 = a1->g + a1->h;
      double f2 = a2->g + a2->h;
      if( ( f1 >= f2 - 0.000001) && (f1 <= f2 +0.000001) )
        return a1->g < a2->g; // if equal compare gvals
      return f1 > f2;
    }
  };


  struct State; // forward declaration
  using StatePtr = std::shared_ptr<State>;
  using priorityQueue = boost::heap::d_ary_heap<StatePtr, boost::heap::mutable_<true>,
                        boost::heap::arity<2>, boost::heap::compare< compare_state<StatePtr> >>;

  struct State
  {
    int id;
    int x, y, z = 0;
    int parentId = -1;

    priorityQueue::handle_type heapkey;

    double g = std::numeric_limits<double>::infinity();
    double h;
    bool opened = false;
    bool closed = false;

    State(int id, int x, int y)
      : id(id), x(x), y(y)
    {}

    State(int id, int x, int y, int z)
      : id(id), x(x), y(y), z(z)
    {}

  };


  class GraphSearch
  {
    public:
      GraphSearch(const int8_t* cMap, int xDim, int yDim, double eps = 1, double cweight = 0.1, bool verbose = false);
      GraphSearch(const int8_t* cMap, int xDim, int yDim, int zDim, double eps = 1, double cweight = 0.1, bool verbose = false);

      double plan(int xStart, int yStart, int xGoal, int yGoal, std::vector<bool> in_region = std::vector<bool>());
      double plan(int xStart, int yStart, int zStart, int xGoal, int yGoal, int zGoal, std::vector<bool> in_region = std::vector<bool>());

      std::vector<StatePtr> getPath() const;

      std::vector<StatePtr> getOpenSet() const;

      std::vector<StatePtr> getCloseSet() const;

      std::vector<StatePtr> getAllSet() const;

    private:
      double plan(StatePtr& currNode_ptr, int start_id, int goal_id);
      void getSucc(const StatePtr& curr, std::vector<int>& succ_ids, std::vector<double>& succ_costs);
      std::vector<StatePtr> recoverPath(StatePtr node, int id);

      int coordToId(int x, int y) const;
      int coordToId(int x, int y, int z) const;

      bool isFree(int x, int y) const;
      bool isFree(int x, int y, int z) const;

      double getHeur(int x, int y) const;
      double getHeur(int x, int y, int z) const;

      const int8_t* cMap_;
      int xDim_, yDim_, zDim_;
      double eps_;
      double cweight_;
      bool verbose_;

      const int8_t val_free_ = 0;
      const int8_t val_occ_ = 100;
      int xGoal_, yGoal_, zGoal_;
      bool use_2d_;
      bool global_;

      priorityQueue pq_;
      std::vector<StatePtr> hm_;
      std::vector<bool> seen_;
      std::vector<bool> in_region_;

      std::vector<StatePtr> path_;

      std::vector<std::vector<int>> ns_;
 };
}
#endif