2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
9 bool RRTExt11::goal_found(RRTNode &f)
11 auto &g = this->goals().front();
12 auto fbc = BicycleCar();
16 auto gbc = BicycleCar();
20 bool drivable = false;
22 drivable = gbc.drivable(fbc, this->entry.b, this->entry.e);
24 drivable = gbc.drivable(fbc);
27 if (std::get<0>(this->collide_steered_from(f)))
29 double cost = this->cost_build(f, g);
30 this->join_steered(&f);
31 if (g.p() == nullptr) {
34 this->path_cost_before_opt_ = g.cc;
35 } else if (f.cc + cost < g.cc) {