src/rrtext12.cc

   1 #include "rrtext.h"
   2
   3 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
   4 {
   5         // assume that `t` is circle uniformly sampled
   6         auto bc = BicycleCar();
   7         bc.x(f.x());
   8         bc.y(f.y());
   9         bc.h(f.h());
  10         bc.set_max_steer();
  11         auto max_steer = bc.st();
  12         // map t.h() in -PI...+PI to -max_steer...+max_steer
  13         bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
  14         bc.sp((this->udy_(this->gen_) > 0.5) ? 0.2 : -0.2);
  15         this->steered().clear();
  16         while (true) {
  17                 bc.next();
  18                 auto n = RRTNode(bc);
  19                 if (
  20                         this->steered().size() == 0
  21                         && std::get<0>(this->collide_two_nodes(f, n))
  22                 ) {
  23                     return;
  24                 } else if (
  25                         (
  26                                 this->steered().size() > 0
  27                                 && std::get<0>(this->collide_two_nodes(
  28                                         this->steered().back(),
  29                                         n
  30                                 ))
  31                         )
  32                         || this->steered().size() > 25
  33                 ) {
  34                         break;
  35                 }
  36                 this->steered().push_back(RRTNode(bc));
  37         }
  38         bc.sp(-1*bc.sp());
  39         bc.next();
  40         bc.sp(-1*bc.sp());
  41         this->samples().back().x(bc.x());
  42         this->samples().back().y(bc.y());
  43         this->samples().back().h(bc.h());
  44 }