src/rrtext16.cc

   1 #include "reeds_shepp.h"
   2 #include "rrtext.hh"
   3
   4 namespace rrts {
   5
   6 static int
   7 cb_steer(double q[4], void *w)
   8 {
   9         std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
  10         st->push_back(RRTNode());
  11         st->back().x(q[0]);
  12         st->back().y(q[1]);
  13         st->back().h(q[2]);
  14         st->back().sp(q[3]);
  15         return 0;
  16 }
  17
  18 void
  19 RRTExt16::steer(RRTNode const& f, RRTNode const& t)
  20 {
  21         this->steered_.clear();
  22         double q0[] = {f.x(), f.y(), f.h()};
  23         double q1[] = {t.x(), t.y(), t.h()};
  24         ReedsSheppStateSpace rsss(this->bc_.mtr());
  25         rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);
  26 }
  27
  28 } // namespace rrts