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         if (gbc.drivable(fbc)) {
  16                 this->steer(f, g);
  17                 if (std::get<0>(this->collide_steered_from(f)))
  18                         return false;
  19                 this->join_steered(&f);
  20                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {
  21                         g.p(&f);
  22                         g.c(cost);
  23                 }
  24                 return true;
  25         }
  26         return false;
  27 }