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 ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
  14 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
  15
  16 /*! \brief Use grid data structure to store RRT nodes.
  17
  18 This approach speeds up the search process for the nearest neighbor and
  19 the near vertices procedures.
  20 */
  21 class RRTExt9 : public virtual RRTS {
  22         private:
  23                 class Cell {
  24                         private:
  25                                 bool changed_ = false;
  26                                 std::vector<RRTNode *> nodes_;
  27                         public:
  28                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
  29                                 void store_node(RRTNode *n);
  30
  31                                 // getter, setter
  32                                 bool changed() const
  33                                 {
  34                                         return this->changed_;
  35                                 }
  36                                 std::vector<RRTNode *> &nodes()
  37                                 {
  38                                         return this->nodes_;
  39                                 }
  40
  41                                 Cell();
  42                 };
  43                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
  44                 unsigned int x_min_ = 0;
  45                 unsigned int x_max_ = 0;
  46                 unsigned int y_min_ = 0;
  47                 unsigned int y_max_ = 0;
  48
  49                 unsigned int xi(RRTNode n);
  50                 unsigned int yi(RRTNode n);
  51         public:
  52                 void init();
  53                 void deinit();
  54                 void store_node(RRTNode n);
  55                 RRTNode *nn(RRTNode &t);
  56                 std::vector<RRTNode *> nv(RRTNode &t);
  57 };
  58
  59 /*! \brief Use k-d tree data structure to store RRT nodes.
  60
  61 This approach speeds up the search process for the nearest neighbor and
  62 the near vertices procedures. This extension implements 3D K-d tree.
  63
  64 \see https://en.wikipedia.org/wiki/K-d_tree
  65 */
  66 class RRTExt8 : public virtual RRTS {
  67         private:
  68                 class KdNode {
  69                         private:
  70                                 RRTNode *node_ = nullptr;
  71                                 KdNode *left_ = nullptr;
  72                                 KdNode *right_ = nullptr;
  73                         public:
  74                                 // getter, setter
  75                                 RRTNode *node() const { return this->node_; }
  76                                 KdNode *&left() { return this->left_; }
  77                                 KdNode *&right() { return this->right_; }
  78                                 KdNode(RRTNode *n);
  79                 };
  80                 KdNode *kdroot_ = nullptr;
  81                 void delete_kd_nodes(KdNode *n);
  82                 void store_node(RRTNode *n, KdNode *&r, int l);
  83                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
  84         public:
  85                 void init();
  86                 void deinit();
  87                 void store_node(RRTNode n);
  88                 RRTNode *nn(RRTNode &t);
  89                 std::vector<RRTNode *> nv(RRTNode &t);
  90 };
  91
  92 /*! \brief Use k-d tree data structure to store RRT nodes.
  93
  94 This approach speeds up the search process for the nearest neighbor and
  95 the near vertices procedures. This extension implements 2D K-d tree.
  96
  97 \see https://en.wikipedia.org/wiki/K-d_tree
  98 */
  99 class RRTExt7 : public virtual RRTS {
 100         private:
 101                 class KdNode {
 102                         private:
 103                                 RRTNode *node_ = nullptr;
 104                                 KdNode *left_ = nullptr;
 105                                 KdNode *right_ = nullptr;
 106                         public:
 107                                 // getter, setter
 108                                 RRTNode *node() const { return this->node_; }
 109                                 KdNode *&left() { return this->left_; }
 110                                 KdNode *&right() { return this->right_; }
 111                                 KdNode(RRTNode *n);
 112                 };
 113                 KdNode *kdroot_ = nullptr;
 114                 void delete_kd_nodes(KdNode *n);
 115                 void store_node(RRTNode *n, KdNode *&r, int l);
 116                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 117         public:
 118                 void init();
 119                 void deinit();
 120                 void store_node(RRTNode n);
 121                 RRTNode *nn(RRTNode &t);
 122                 std::vector<RRTNode *> nv(RRTNode &t);
 123 };
 124
 125 /*! \brief Reeds and Shepp cost for building and search.
 126 */
 127 class RRTExt6 : public virtual RRTS {
 128         public:
 129                 /*! \brief Reeds and Shepp path length.
 130                 */
 131                 double cost_build(RRTNode &f, RRTNode &t);
 132                 /*! \brief Reeds and Shepp path length.
 133                 */
 134                 double cost_search(RRTNode &f, RRTNode &t);
 135 };
 136
 137 /*! \brief Different costs extension.
 138
 139 Use different cost for bulding tree data structure and searching in the
 140 structure. This one is complementary to `rrtext1.cc`.
 141 */
 142 class RRTExt5 : public virtual RRTS {
 143         public:
 144                 /*! \brief Reeds and Shepp path length.
 145                 */
 146                 double cost_build(RRTNode &f, RRTNode &t);
 147                 /*! \brief Euclidean distance.
 148                 */
 149                 double cost_search(RRTNode &f, RRTNode &t);
 150 };
 151
 152 /*! \brief Use grid data structure to store RRT nodes.
 153
 154 This approach speeds up the search process for the nearest neighbor and
 155 the near vertices procedures.
 156 */
 157 class RRTExt4 : public virtual RRTS {
 158         private:
 159                 class Cell {
 160                         private:
 161                                 bool changed_ = false;
 162                                 std::vector<RRTNode *> nodes_;
 163                         public:
 164                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 165                                 void store_node(RRTNode *n);
 166
 167                                 // getter, setter
 168                                 bool changed() const
 169                                 {
 170                                         return this->changed_;
 171                                 }
 172                                 std::vector<RRTNode *> &nodes()
 173                                 {
 174                                         return this->nodes_;
 175                                 }
 176
 177                                 Cell();
 178                 };
 179                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
 180                 unsigned int x_min_ = 0;
 181                 unsigned int x_max_ = 0;
 182                 unsigned int y_min_ = 0;
 183                 unsigned int y_max_ = 0;
 184
 185                 unsigned int xi(RRTNode n);
 186                 unsigned int yi(RRTNode n);
 187         public:
 188                 void init();
 189                 void deinit();
 190                 void store_node(RRTNode n);
 191                 RRTNode *nn(RRTNode &t);
 192                 std::vector<RRTNode *> nv(RRTNode &t);
 193 };
 194
 195 /*! \brief Use Dijkstra algorithm to find the shorter path.
 196 */
 197 class RRTExt3 : public virtual RRTS {
 198         private:
 199                 std::vector<RRTNode *> orig_path_;
 200                 double orig_path_cost_;
 201         public:
 202                 std::vector<RRTNode *> path();
 203
 204                 // getter, setter
 205                 std::vector<RRTNode *> &orig_path()
 206                 {
 207                         return this->orig_path_;
 208                 };
 209                 double &orig_path_cost() { return this->orig_path_cost_; }
 210                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 211 };
 212
 213 /*! \brief Use cute_c2 for collision detection.
 214
 215 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 216 */
 217 class RRTExt2 : public virtual RRTS {
 218         private:
 219                 c2Poly c2_bc_;
 220                 c2x c2x_bc_;
 221                 std::vector<c2Poly> c2_obstacles_;
 222         public:
 223                 void init();
 224                 void deinit();
 225
 226                 // Collide RRT procedures
 227                 std::tuple<bool, unsigned int, unsigned int>
 228                 collide_steered_from(RRTNode &f);
 229
 230                 std::tuple<bool, unsigned int, unsigned int>
 231                 collide_two_nodes(RRTNode &f, RRTNode &t);
 232
 233                 // getters, setters
 234                 c2Poly &c2_bc() { return this->c2_bc_; }
 235                 c2x &c2x_bc() { return this->c2x_bc_; }
 236                 std::vector<c2Poly> &c2_obstacles() {
 237                         return this->c2_obstacles_;
 238                 };
 239 };
 240
 241 /*! \brief Different costs extension.
 242
 243 Use different cost for bulding tree data structure and searching in the
 244 structure.
 245 */
 246 class RRTExt1 : public virtual RRTS {
 247         public:
 248                 /*! \brief Reeds and Shepp path length.
 249                 */
 250                 double cost_build(RRTNode &f, RRTNode &t);
 251                 /*! \brief Matej's heuristics.
 252                 */
 253                 double cost_search(RRTNode &f, RRTNode &t);
 254 };
 255
 256 #endif /* RRTEXT_H */