src/rrtext1.cc

   1 #include "rrtext.h"
   2 #include "reeds_shepp.h"
   3
   4 double RRTExt1::cost_build(RRTNode &f, RRTNode &t)
   5 {
   6         double q0[] = {f.x(), f.y(), f.h()};
   7         double q1[] = {t.x(), t.y(), t.h()};
   8         ReedsSheppStateSpace rsss(this->bc.mtr());
   9         return rsss.distance(q0, q1);
  10 }
  11
  12 double RRTExt1::cost_search(RRTNode &f, RRTNode &t)
  13 {
  14         double cost = 0;
  15         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
  16         double heur = std::min(
  17                 std::abs(t.h() - f.h()),
  18                 2 * M_PI - std::abs(t.h() - f.h())
  19         );
  20         heur *= this->bc.mtr();
  21         cost = std::max(cost, heur);
  22         return cost;
  23 }