3 bool RRTExt11::goal_found(RRTNode &f)
5 auto &g = this->goals().front();
6 auto fbc = BicycleCar();
10 auto gbc = BicycleCar();
14 bool drivable = false;
16 drivable = gbc.drivable(fbc, this->entry.b, this->entry.e);
18 drivable = gbc.drivable(fbc);
21 if (std::get<0>(this->collide_steered_from(f)))
23 double cost = this->cost_build(f, g);
24 this->join_steered(&f);
25 if (g.p() == nullptr) {
28 this->path_cost_before_opt_ = g.cc;
29 } else if (f.cc + cost < g.cc) {