src/rrtext12.cc

   1 #include "rrtext.h"
   2 #include <iostream>
   3
   4 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
   5 {
   6         // assume that `t` is circle uniformly sampled
   7         auto bc = BicycleCar();
   8         bc.x(f.x());
   9         bc.y(f.y());
  10         bc.h(f.h());
  11         bc.set_max_steer();
  12         auto max_steer = bc.st();
  13         // map t.h() in -PI...+PI to -max_steer...+max_steer
  14         bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
  15         bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
  16         this->steered().clear();
  17         bc.next();
  18         this->steered().push_back(RRTNode(bc));
  19 }