]> rtime.felk.cvut.cz Git - hubacji1/rrts.git/blob - api/rrtext.h
Add ext5 (RS build, Euclid. search)
[hubacji1/rrts.git] / 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 */