src/rrtext11.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include "rrtext.h"
   8
   9 bool RRTExt11::goal_found(RRTNode &f)
  10 {
  11         auto &g = this->goals().front();
  12         auto fbc = BicycleCar();
  13         fbc.x(f.x());
  14         fbc.y(f.y());
  15         fbc.h(f.h());
  16         auto gbc = BicycleCar();
  17         gbc.x(g.x());
  18         gbc.y(g.y());
  19         gbc.h(g.h());
  20         bool drivable = false;
  21         if (this->entry_set)
  22             drivable = gbc.drivable(fbc, this->entry.b, this->entry.e);
  23         else
  24             drivable = gbc.drivable(fbc);
  25         if (drivable) {
  26                 this->steer(f, g);
  27                 if (std::get<0>(this->collide_steered_from(f)))
  28                         return false;
  29                 double cost = this->cost_build(f, g);
  30                 this->join_steered(&f);
  31                 if (g.p() == nullptr) {
  32                         g.p(&f);
  33                         g.c(cost);
  34                         this->path_cost_before_opt_ = g.cc;
  35                 } else if (f.cc + cost < g.cc) {
  36                         g.p(&f);
  37                         g.c(cost);
  38                 }
  39                 return true;
  40         }
  41         return false;
  42 }