src/rrtext11.cc

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