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