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
17 #define GRID_MAX_HI 60
19 /*! \brief Different costs extension.
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.
28 class RRTExt10 : public virtual RRTS {
30 /*! \brief Reeds and Shepp path length.
32 double cost_build(RRTNode &f, RRTNode &t);
33 /*! \brief Heuristics distance.
35 double cost_search(RRTNode &f, RRTNode &t);
38 /*! \brief Use grid data structure to store RRT nodes.
40 This approach speeds up the search process for the nearest neighbor and
41 the near vertices procedures.
43 class RRTExt9 : public virtual RRTS {
47 bool changed_ = false;
48 std::vector<RRTNode *> nodes_;
50 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
51 void store_node(RRTNode *n);
56 return this->changed_;
58 std::vector<RRTNode *> &nodes()
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;
73 unsigned int xi(RRTNode n);
74 unsigned int yi(RRTNode n);
75 unsigned int hi(RRTNode n);
79 void store_node(RRTNode n);
80 RRTNode *nn(RRTNode &t);
81 std::vector<RRTNode *> nv(RRTNode &t);
84 /*! \brief Use k-d tree data structure to store RRT nodes.
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.
89 \see https://en.wikipedia.org/wiki/K-d_tree
91 class RRTExt8 : public virtual RRTS {
95 RRTNode *node_ = nullptr;
96 KdNode *left_ = nullptr;
97 KdNode *right_ = nullptr;
100 RRTNode *node() const { return this->node_; }
101 KdNode *&left() { return this->left_; }
102 KdNode *&right() { return this->right_; }
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);
112 void store_node(RRTNode n);
113 RRTNode *nn(RRTNode &t);
114 std::vector<RRTNode *> nv(RRTNode &t);
117 /*! \brief Use k-d tree data structure to store RRT nodes.
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.
122 \see https://en.wikipedia.org/wiki/K-d_tree
124 class RRTExt7 : public virtual RRTS {
128 RRTNode *node_ = nullptr;
129 KdNode *left_ = nullptr;
130 KdNode *right_ = nullptr;
133 RRTNode *node() const { return this->node_; }
134 KdNode *&left() { return this->left_; }
135 KdNode *&right() { return this->right_; }
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);
145 void store_node(RRTNode n);
146 RRTNode *nn(RRTNode &t);
147 std::vector<RRTNode *> nv(RRTNode &t);
150 /*! \brief Reeds and Shepp cost for building and search.
152 class RRTExt6 : public virtual RRTS {
154 /*! \brief Reeds and Shepp path length.
156 double cost_build(RRTNode &f, RRTNode &t);
157 /*! \brief Reeds and Shepp path length.
159 double cost_search(RRTNode &f, RRTNode &t);
162 /*! \brief Different costs extension.
164 Use different cost for bulding tree data structure and searching in the
165 structure. This one is complementary to `rrtext1.cc`.
167 class RRTExt5 : public virtual RRTS {
169 /*! \brief Reeds and Shepp path length.
171 double cost_build(RRTNode &f, RRTNode &t);
172 /*! \brief Euclidean distance.
174 double cost_search(RRTNode &f, RRTNode &t);
177 /*! \brief Use grid data structure to store RRT nodes.
179 This approach speeds up the search process for the nearest neighbor and
180 the near vertices procedures.
182 class RRTExt4 : public virtual RRTS {
186 bool changed_ = false;
187 std::vector<RRTNode *> nodes_;
189 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
190 void store_node(RRTNode *n);
195 return this->changed_;
197 std::vector<RRTNode *> &nodes()
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;
210 unsigned int xi(RRTNode n);
211 unsigned int yi(RRTNode n);
215 void store_node(RRTNode n);
216 RRTNode *nn(RRTNode &t);
217 std::vector<RRTNode *> nv(RRTNode &t);
220 /*! \brief Use Dijkstra algorithm to find the shorter path.
222 class RRTExt3 : public virtual RRTS {
224 std::vector<RRTNode *> orig_path_;
225 double orig_path_cost_;
227 std::vector<RRTNode *> path();
230 std::vector<RRTNode *> &orig_path()
232 return this->orig_path_;
234 double &orig_path_cost() { return this->orig_path_cost_; }
235 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
238 /*! \brief Use cute_c2 for collision detection.
240 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
242 class RRTExt2 : public virtual RRTS {
246 std::vector<c2Poly> c2_obstacles_;
251 // Collide RRT procedures
252 std::tuple<bool, unsigned int, unsigned int>
253 collide_steered_from(RRTNode &f);
255 std::tuple<bool, unsigned int, unsigned int>
256 collide_two_nodes(RRTNode &f, RRTNode &t);
259 c2Poly &c2_bc() { return this->c2_bc_; }
260 c2x &c2x_bc() { return this->c2x_bc_; }
261 std::vector<c2Poly> &c2_obstacles() {
262 return this->c2_obstacles_;
266 /*! \brief Different costs extension.
268 Use different cost for bulding tree data structure and searching in the
271 class RRTExt1 : public virtual RRTS {
273 /*! \brief Reeds and Shepp path length.
275 double cost_build(RRTNode &f, RRTNode &t);
276 /*! \brief Matej's heuristics.
278 double cost_search(RRTNode &f, RRTNode &t);
281 #endif /* RRTEXT_H */