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/>.
29 RRTNode::RRTNode(float x, float y):
37 RRTNode::RRTNode(float x, float y, float h):
40 h_((h > 2 * M_PI) ? h - 2 * M_PI : h),
45 RRTNode::RRTNode(float x, float y, float h, float t):
48 h_((h > 2 * M_PI) ? h - 2 * M_PI : h),
53 RRTNode::RRTNode(float x, float y, float h, float t, float s):
56 h_((h > 2 * M_PI) ? h - 2 * M_PI : h),
62 float RRTNode::x() const
67 float RRTNode::y() const
72 float RRTNode::h() const
77 float RRTNode::t() const
82 float RRTNode::s() const
87 float RRTNode::ccost() const
92 float RRTNode::dcost() const
97 float RRTNode::ocost() const
102 char RRTNode::tree() const
107 std::vector<RRTNode *> &RRTNode::children()
109 return this->children_;
112 RRTNode *RRTNode::parent() const
114 return this->parent_;
117 bool RRTNode::visited()
119 return this->visited_;
123 void RRTNode::h(float ch)
128 void RRTNode::t(float ct)
133 void RRTNode::s(float cs)
138 bool RRTNode::add_child(RRTNode *node, float cost)
140 return this->add_child(node, cost, 1);
143 bool RRTNode::add_child(RRTNode *node, float cost, float time)
146 node->ccost(this->ccost() + cost);
148 node->t(this->t() + time);
149 this->children_.push_back(node);
153 bool RRTNode::rem_child(RRTNode *node)
156 for (auto ch: this->children_) {
158 this->children_.erase(this->children_.begin() + i);
166 float RRTNode::ccost(float cost)
168 return this->ccost_ = cost;
171 float RRTNode::dcost(float cost)
173 return this->dcost_ = cost;
176 float RRTNode::ocost(float cost)
178 return this->ocost_ = cost;
181 char RRTNode::tree(char t)
183 return this->tree_ = t;
186 bool RRTNode::remove_parent()
188 this->parent_ = nullptr;
192 bool RRTNode::parent(RRTNode *parent)
194 if (parent == nullptr)
196 this->parent_ = parent;
200 bool RRTNode::visit(bool v)
202 return this->visited_ = v;
206 bool RRTNode::comp_ccost(RRTNode *n1, RRTNode *n2)
208 return n1->ccost() < n2->ccost();
211 float RRTNode::update_ccost()
213 std::vector<RRTNode *> s; // DFS stack
214 std::vector<RRTNode *> r; // reset visited_
217 while (s.size() > 0) {
222 for (auto ch: tmp->children()) {
223 ch->ccost(tmp->ccost() + ch->dcost());
233 bool RRTNode::visit()
235 if (this->visited_) {
238 this->visited_ = true;
243 init_(new RRTNode()),
247 RRTEdge::RRTEdge(RRTNode *init, RRTNode *goal):
252 RRTNode *RRTEdge::init() const
257 RRTNode *RRTEdge::goal() const
262 RRTNode *RRTEdge::intersect(RRTEdge *e, bool segment)
264 // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
265 float x1 = this->init()->x();
266 float y1 = this->init()->y();
267 float x2 = this->goal()->x();
268 float y2 = this->goal()->y();
269 float x3 = e->init()->x();
270 float y3 = e->init()->y();
271 float x4 = e->goal()->x();
272 float y4 = e->goal()->y();
273 float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
277 float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
278 (x1 - x2) * (x3 * y4 - y3 * x4);
280 float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
281 (y1 - y2) * (x3 * y4 - y3 * x4);
283 return new RRTNode(px, py, 0);
285 float t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
287 float u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
290 if (t < 0 || t > 1 || u < 0 || u > 1)
292 return new RRTNode(x1 + t * (x2 - x1), y1 + t * (y2 - y1), 0);