src/rrtext15.cc

   1 #include "rrtext.hh"
   2
   3 namespace rrts {
   4
   5 Json::Value
   6 RRTExt15::json() const
   7 {
   8         auto jvo = RRTS::json();
   9         unsigned int i = 0;
  10         for (auto c: this->log_path_cost_) {
  11                 jvo["log_path_cost"][i++] = c;
  12         }
  13         return jvo;
  14 }
  15
  16 void
  17 RRTExt15::json(Json::Value jvi)
  18 {
  19         RRTS::json(jvi);
  20 }
  21
  22 bool
  23 RRTExt15::next()
  24 {
  25         bool c = RRTS::next();
  26         if (this->log_path_cost_.size() == 0) {
  27                 this->log_path_cost_.push_back(this->goal_.cc());
  28         } else {
  29                 double lc = this->log_path_cost_.back();
  30                 double gc = this->goal_.cc();
  31                 if (gc > 0.0 && (lc == 0.0 || gc < lc)) {
  32                         this->log_path_cost_.push_back(gc);
  33                 } else {
  34                         this->log_path_cost_.push_back(lc);
  35                 }
  36         }
  37         return c;
  38 }
  39
  40 } // namespace rrts