api/rrtext.h

   1 #ifndef RRTEXT_H
   2 #define RRTEXT_H
   3
   4 #include "rrts.h"
   5
   6 // ext2
   7 #include "cute_c2.h"
   8
   9 // ext4
  10 #define GRID 1 // in meters
  11 #define GRID_WIDTH 20 // in meters
  12 #define GRID_HEIGHT 50 // in meters
  13 #define GRID_MAX_XI (GRID_WIDTH / GRID) // min is 0
  14 #define GRID_MAX_YI (GRID_HEIGHT / GRID) // min is 0
  15
  16 /*! \brief Reeds and Shepp cost for building and search.
  17 */
  18 class RRTExt6 : public virtual RRTS {
  19         public:
  20                 /*! \brief Reeds and Shepp path length.
  21                 */
  22                 double cost_build(RRTNode &f, RRTNode &t);
  23                 /*! \brief Reeds and Shepp path length.
  24                 */
  25                 double cost_search(RRTNode &f, RRTNode &t);
  26 };
  27
  28 /*! \brief Different costs extension.
  29
  30 Use different cost for bulding tree data structure and searching in the
  31 structure. This one is complementary to `rrtext1.cc`.
  32 */
  33 class RRTExt5 : public virtual RRTS {
  34         public:
  35                 /*! \brief Reeds and Shepp path length.
  36                 */
  37                 double cost_build(RRTNode &f, RRTNode &t);
  38                 /*! \brief Euclidean distance.
  39                 */
  40                 double cost_search(RRTNode &f, RRTNode &t);
  41 };
  42
  43 /*! \brief Use grid data structure to store RRT nodes.
  44
  45 This approach speeds up the search process for the nearest neighbor and
  46 the near vertices procedures.
  47 */
  48 class RRTExt4 : public virtual RRTS {
  49         private:
  50                 class Cell {
  51                         private:
  52                                 bool changed_ = false;
  53                                 std::vector<RRTNode *> nodes_;
  54                         public:
  55                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
  56                                 void store_node(RRTNode *n);
  57
  58                                 // getter, setter
  59                                 bool changed() const
  60                                 {
  61                                         return this->changed_;
  62                                 }
  63                                 std::vector<RRTNode *> &nodes()
  64                                 {
  65                                         return this->nodes_;
  66                                 }
  67
  68                                 Cell();
  69                 };
  70                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
  71                 unsigned int x_min_ = 0;
  72                 unsigned int x_max_ = 0;
  73                 unsigned int y_min_ = 0;
  74                 unsigned int y_max_ = 0;
  75
  76                 unsigned int xi(RRTNode n);
  77                 unsigned int yi(RRTNode n);
  78         public:
  79                 void init();
  80                 void deinit();
  81                 void store_node(RRTNode n);
  82                 RRTNode *nn(RRTNode &t);
  83                 std::vector<RRTNode *> nv(RRTNode &t);
  84 };
  85
  86 /*! \brief Use Dijkstra algorithm to find the shorter path.
  87 */
  88 class RRTExt3 : public virtual RRTS {
  89         private:
  90                 std::vector<RRTNode *> orig_path_;
  91                 double orig_path_cost_;
  92         public:
  93                 std::vector<RRTNode *> path();
  94
  95                 // getter, setter
  96                 std::vector<RRTNode *> &orig_path()
  97                 {
  98                         return this->orig_path_;
  99                 };
 100                 double &orig_path_cost() { return this->orig_path_cost_; }
 101                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 102 };
 103
 104 /*! \brief Use cute_c2 for collision detection.
 105
 106 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 107 */
 108 class RRTExt2 : public virtual RRTS {
 109         private:
 110                 c2Poly c2_bc_;
 111                 c2x c2x_bc_;
 112                 std::vector<c2Poly> c2_obstacles_;
 113         public:
 114                 void init();
 115                 void deinit();
 116
 117                 // Collide RRT procedures
 118                 std::tuple<bool, unsigned int, unsigned int>
 119                 collide_steered_from(RRTNode &f);
 120
 121                 std::tuple<bool, unsigned int, unsigned int>
 122                 collide_two_nodes(RRTNode &f, RRTNode &t);
 123
 124                 // getters, setters
 125                 c2Poly &c2_bc() { return this->c2_bc_; }
 126                 c2x &c2x_bc() { return this->c2x_bc_; }
 127                 std::vector<c2Poly> &c2_obstacles() {
 128                         return this->c2_obstacles_;
 129                 };
 130 };
 131
 132 /*! \brief Different costs extension.
 133
 134 Use different cost for bulding tree data structure and searching in the
 135 structure.
 136 */
 137 class RRTExt1 : public virtual RRTS {
 138         public:
 139                 /*! \brief Reeds and Shepp path length.
 140                 */
 141                 double cost_build(RRTNode &f, RRTNode &t);
 142                 /*! \brief Matej's heuristics.
 143                 */
 144                 double cost_search(RRTNode &f, RRTNode &t);
 145 };
 146
 147 #endif /* RRTEXT_H */