2 This file is part of I am car.
4 I am car is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 3 of the License, or
7 (at your option) any later version.
9 I am car is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
23 #define IS_NEAR(a, b) ({ \
24 __typeof__ (a) _a = (a); \
25 __typeof__ (b) _b = (b); \
26 pow(pow(_b->x() - _a->x(), 2) \
27 + pow(_b->y() - _a->y(), 2), 0.5) < 0.2 \
28 && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
31 #define GOAL_IS_NEAR(a, b) ({ \
32 __typeof__ (a) _a = (a); \
33 __typeof__ (b) _b = (b); \
34 pow(pow(_b->x() - _a->x(), 2) \
35 + pow(_b->y() - _a->y(), 2), 0.5) < 0.05 \
36 && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
39 template <typename T> int sgn(T val) {
40 return (T(0) < val) - (val < T(0));
45 float dcost_ = 0; // direct cost (of edge) to parent
46 float ccost_ = 0; // cumulative cost from root
47 float ocost_ = 0; // distance to the nearest obstacle
48 char tree_ = '0'; // tree affinity
50 RRTNode *parent_ = nullptr;
51 std::vector<RRTNode *> children_;
53 bool visited_ = false; // for DFS
61 RRTNode *rs_ = nullptr; // random sample of added point
64 RRTNode(float x, float y);
65 RRTNode(float x, float y, float h);
66 RRTNode(float x, float y, float h, float t);
67 RRTNode(float x, float y, float h, float t, float s);
83 std::vector<RRTNode *> &children();
84 RRTNode *parent() const;
95 bool add_child(RRTNode *node, float cost);
96 bool add_child(RRTNode *node, float cost, float time);
97 bool rem_child(RRTNode *node);
99 float ccost(float cost);
100 float dcost(float cost);
101 float ocost(float cost);
104 bool remove_parent();
105 bool parent(RRTNode *parent);
109 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
110 float update_ccost();
120 RRTEdge(RRTNode *init, RRTNode *goal);
122 RRTNode *init() const;
123 RRTNode *goal() const;
125 RRTNode *intersect(RRTEdge *e, bool segment);