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