src/rrtext11.cc

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