src/rrtext15.cc

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