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