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