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 // ext7
  17 #define MAX_DIM 2
  18
  19 /*! \brief Use k-d tree 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. This extension implements 2D K-d tree.
  23
  24 \see https://en.wikipedia.org/wiki/K-d_tree
  25 */
  26 class RRTExt7 : public virtual RRTS {
  27         private:
  28                 class KdNode {
  29                         private:
  30                                 RRTNode *node_ = nullptr;
  31                                 KdNode *left_ = nullptr;
  32                                 KdNode *right_ = nullptr;
  33                         public:
  34                                 // getter, setter
  35                                 RRTNode *node() const { return this->node_; }
  36                                 KdNode *&left() { return this->left_; }
  37                                 KdNode *&right() { return this->right_; }
  38                                 KdNode(RRTNode *n);
  39                 };
  40                 KdNode *kdroot_ = nullptr;
  41                 void store_node(RRTNode *n, KdNode *&r, int l);
  42                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
  43         public:
  44                 void init();
  45                 void deinit();
  46                 void store_node(RRTNode n);
  47                 RRTNode *nn(RRTNode &t);
  48                 std::vector<RRTNode *> nv(RRTNode &t);
  49 };
  50
  51 /*! \brief Reeds and Shepp cost for building and search.
  52 */
  53 class RRTExt6 : public virtual RRTS {
  54         public:
  55                 /*! \brief Reeds and Shepp path length.
  56                 */
  57                 double cost_build(RRTNode &f, RRTNode &t);
  58                 /*! \brief Reeds and Shepp path length.
  59                 */
  60                 double cost_search(RRTNode &f, RRTNode &t);
  61 };
  62
  63 /*! \brief Different costs extension.
  64
  65 Use different cost for bulding tree data structure and searching in the
  66 structure. This one is complementary to `rrtext1.cc`.
  67 */
  68 class RRTExt5 : public virtual RRTS {
  69         public:
  70                 /*! \brief Reeds and Shepp path length.
  71                 */
  72                 double cost_build(RRTNode &f, RRTNode &t);
  73                 /*! \brief Euclidean distance.
  74                 */
  75                 double cost_search(RRTNode &f, RRTNode &t);
  76 };
  77
  78 /*! \brief Use grid data structure to store RRT nodes.
  79
  80 This approach speeds up the search process for the nearest neighbor and
  81 the near vertices procedures.
  82 */
  83 class RRTExt4 : public virtual RRTS {
  84         private:
  85                 class Cell {
  86                         private:
  87                                 bool changed_ = false;
  88                                 std::vector<RRTNode *> nodes_;
  89                         public:
  90                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
  91                                 void store_node(RRTNode *n);
  92
  93                                 // getter, setter
  94                                 bool changed() const
  95                                 {
  96                                         return this->changed_;
  97                                 }
  98                                 std::vector<RRTNode *> &nodes()
  99                                 {
 100                                         return this->nodes_;
 101                                 }
 102
 103                                 Cell();
 104                 };
 105                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
 106                 unsigned int x_min_ = 0;
 107                 unsigned int x_max_ = 0;
 108                 unsigned int y_min_ = 0;
 109                 unsigned int y_max_ = 0;
 110
 111                 unsigned int xi(RRTNode n);
 112                 unsigned int yi(RRTNode n);
 113         public:
 114                 void init();
 115                 void deinit();
 116                 void store_node(RRTNode n);
 117                 RRTNode *nn(RRTNode &t);
 118                 std::vector<RRTNode *> nv(RRTNode &t);
 119 };
 120
 121 /*! \brief Use Dijkstra algorithm to find the shorter path.
 122 */
 123 class RRTExt3 : public virtual RRTS {
 124         private:
 125                 std::vector<RRTNode *> orig_path_;
 126                 double orig_path_cost_;
 127         public:
 128                 std::vector<RRTNode *> path();
 129
 130                 // getter, setter
 131                 std::vector<RRTNode *> &orig_path()
 132                 {
 133                         return this->orig_path_;
 134                 };
 135                 double &orig_path_cost() { return this->orig_path_cost_; }
 136                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 137 };
 138
 139 /*! \brief Use cute_c2 for collision detection.
 140
 141 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 142 */
 143 class RRTExt2 : public virtual RRTS {
 144         private:
 145                 c2Poly c2_bc_;
 146                 c2x c2x_bc_;
 147                 std::vector<c2Poly> c2_obstacles_;
 148         public:
 149                 void init();
 150                 void deinit();
 151
 152                 // Collide RRT procedures
 153                 std::tuple<bool, unsigned int, unsigned int>
 154                 collide_steered_from(RRTNode &f);
 155
 156                 std::tuple<bool, unsigned int, unsigned int>
 157                 collide_two_nodes(RRTNode &f, RRTNode &t);
 158
 159                 // getters, setters
 160                 c2Poly &c2_bc() { return this->c2_bc_; }
 161                 c2x &c2x_bc() { return this->c2x_bc_; }
 162                 std::vector<c2Poly> &c2_obstacles() {
 163                         return this->c2_obstacles_;
 164                 };
 165 };
 166
 167 /*! \brief Different costs extension.
 168
 169 Use different cost for bulding tree data structure and searching in the
 170 structure.
 171 */
 172 class RRTExt1 : public virtual RRTS {
 173         public:
 174                 /*! \brief Reeds and Shepp path length.
 175                 */
 176                 double cost_build(RRTNode &f, RRTNode &t);
 177                 /*! \brief Matej's heuristics.
 178                 */
 179                 double cost_search(RRTNode &f, RRTNode &t);
 180 };
 181
 182 #endif /* RRTEXT_H */