src/rrtext6.cc

   1 #include "rrtext.hh"
   2 #include "reeds_shepp.h"
   3
   4 namespace rrts {
   5
   6 double
   7 RRTExt6::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 RRTExt6::cost_search(RRTNode const& f, RRTNode const& t) const
  17 {
  18         return this->cost_build(f, t);
  19 }
  20
  21 } // namespace rrts