src/rrtext10.cc

   1 #include "rrtext.hh"
   2 #include "reeds_shepp.h"
   3
   4 namespace rrts {
   5
   6 double
   7 RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
   8 {
   9         double q0[] = {f.x(), f.y(), f.h()};
  10         double q1[] = {t.x(), t.y(), t.h()};
  11         ReedsSheppStateSpace rsss(this->bc_.mtr());
  12         return rsss.distance(q0, q1);
  13 }
  14
  15 double
  16 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
  17 {
  18         return f.edist(t) + std::abs(t.h() - f.h());
  19 }
  20
  21 } // namespace rrts