src/rrtext3.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 <queue>
   8 #include "rrtext.h"
   9
  10 void RRTExt3::reset()
  11 {
  12         RRTS::reset();
  13         this->orig_path().clear();
  14         this->first_optimized_path().clear();
  15         this->orig_path_cost_ = 9999.9;
  16         this->first_optimized_path_cost_ = 9999.9;
  17 }
  18
  19 void RRTExt3::first_path_optimization()
  20 {
  21         if (this->orig_path().size() == 0) {
  22                 this->orig_path_ = RRTS::path();
  23                 if (this->orig_path().size() == 0)
  24                         return;
  25                 else
  26                         this->orig_path_cost(this->orig_path().back()->cc);
  27         }
  28         class DijkstraNode : public RRTNode {
  29                 public:
  30                         DijkstraNode *s = nullptr;
  31                         RRTNode *n = nullptr;
  32                         unsigned int i = 0;
  33                         double cc = 9999;
  34                         bool v = false;
  35                         bool vi()
  36                         {
  37                                 if (this->v)
  38                                         return true;
  39                                 this->v = true;
  40                                 return false;
  41                         }
  42                         DijkstraNode(const RRTNode& n) {
  43                             this->x(n.x());
  44                             this->y(n.y());
  45                             this->h(n.h());
  46                             this->sp(n.sp());
  47                             this->st(n.st());
  48                         }
  49                         // override
  50                         DijkstraNode *p_ = nullptr;
  51                         DijkstraNode *p() const { return this->p_; }
  52                         void p(DijkstraNode *p) { this->p_ = p; }
  53         };
  54         class DijkstraNodeComparator {
  55                 public:
  56                         int operator() (
  57                                 const DijkstraNode &n1,
  58                                 const DijkstraNode &n2
  59                         )
  60                         {
  61                                 return n1.cc > n2.cc;
  62                         }
  63         };
  64         std::vector<DijkstraNode> dn;
  65         dn.reserve(this->orig_path().size());
  66         unsigned int dncnt = 0;
  67         for (auto n: this->orig_path()) {
  68                 if (
  69                         n->t(RRTNodeType::cusp)
  70                         || n->t(RRTNodeType::connected)
  71                 ) {
  72                         dn.push_back(DijkstraNode(*n));
  73                         dn.back().cc = n->cc;
  74                         dn.back().s = &dn.back();
  75                         dn.back().n = n;
  76                         dn.back().i = dncnt++;
  77                 }
  78         }
  79         dn.push_back(DijkstraNode(*this->orig_path().back()));
  80         dn.back().cc = this->orig_path().back()->cc;
  81         dn.back().s = &dn.back();
  82         dn.back().n = this->orig_path().back();
  83         dn.back().i = dncnt++;
  84         std::priority_queue<
  85                 DijkstraNode,
  86                 std::vector<DijkstraNode>,
  87                 DijkstraNodeComparator
  88         > pq;
  89         dn.front().vi();
  90         pq.push(dn.front());
  91         while (!pq.empty()) {
  92                 DijkstraNode f = pq.top();
  93                 pq.pop();
  94                 for (unsigned int i = f.i + 1; i < dncnt; i++) {
  95                         double cost = f.cc + this->cost_search(f, dn[i]);
  96                         this->steer(f, dn[i]);
  97                         if (this->steered().size() == 0)
  98                                 break; // TODO why not continue?
  99                         if (std::get<0>(this->collide_steered_from(f)))
 100                                 continue;
 101                         if (cost < dn[i].cc) {
 102                                 dn[i].cc = cost;
 103                                 dn[i].p(f.s);
 104                                 if (!dn[i].vi())
 105                                         pq.push(dn[i]);
 106                         }
 107                 }
 108         }
 109         DijkstraNode *ln = nullptr;
 110         for (auto &n: dn) {
 111                 if (n.v)
 112                         ln = &n;
 113         }
 114         if (ln == nullptr || ln->p() == nullptr)
 115                 return;
 116         while (ln->p() != nullptr) {
 117                 RRTNode *t = ln->n;
 118                 RRTNode *f = ln->p()->n;
 119                 this->steer(*f, *t);
 120                 if (std::get<0>(this->collide_steered_from(*f)))
 121                         return;
 122                 this->join_steered(f);
 123                 t->p(&this->nodes().back());
 124                 t->c(this->cost_build(this->nodes().back(), *t));
 125                 ln = ln->p();
 126         }
 127         RRTS::compute_path();
 128 }
 129
 130 void RRTExt3::second_path_optimization()
 131 {
 132         if (this->first_optimized_path().size() == 0) {
 133                 return;
 134         }
 135         class DijkstraNode : public RRTNode {
 136                 public:
 137                         DijkstraNode *s = nullptr;
 138                         RRTNode *n = nullptr;
 139                         unsigned int i = 0;
 140                         double cc = 9999;
 141                         bool v = false;
 142                         bool vi()
 143                         {
 144                                 if (this->v)
 145                                         return true;
 146                                 this->v = true;
 147                                 return false;
 148                         }
 149                         DijkstraNode(const RRTNode& n) {
 150                             this->x(n.x());
 151                             this->y(n.y());
 152                             this->h(n.h());
 153                             this->sp(n.sp());
 154                             this->st(n.st());
 155                         }
 156                         // override
 157                         DijkstraNode *p_ = nullptr;
 158                         DijkstraNode *p() const { return this->p_; }
 159                         void p(DijkstraNode *p) { this->p_ = p; }
 160         };
 161         class DijkstraNodeComparator {
 162                 public:
 163                         int operator() (
 164                                 const DijkstraNode &n1,
 165                                 const DijkstraNode &n2
 166                         )
 167                         {
 168                                 return n1.cc > n2.cc;
 169                         }
 170         };
 171         std::vector<DijkstraNode> dn;
 172         dn.reserve(this->orig_path().size());
 173         unsigned int dncnt = 0;
 174         for (auto n: this->orig_path()) {
 175                 if (
 176                         n->t(RRTNodeType::cusp)
 177                         || n->t(RRTNodeType::connected)
 178                 ) {
 179                         dn.push_back(DijkstraNode(*n));
 180                         dn.back().cc = n->cc;
 181                         dn.back().s = &dn.back();
 182                         dn.back().n = n;
 183                         dn.back().i = dncnt++;
 184                 }
 185         }
 186         dn.push_back(DijkstraNode(*this->orig_path().back()));
 187         dn.back().cc = this->orig_path().back()->cc;
 188         dn.back().s = &dn.back();
 189         dn.back().n = this->orig_path().back();
 190         dn.back().i = dncnt++;
 191         std::priority_queue<
 192                 DijkstraNode,
 193                 std::vector<DijkstraNode>,
 194                 DijkstraNodeComparator
 195         > pq;
 196         // TODO rewrite
 197         dn.back().vi();
 198         pq.push(dn.back());
 199         while (!pq.empty()) {
 200                 DijkstraNode t = pq.top();
 201                 pq.pop();
 202                 for (unsigned int i = t.i - 1; i > 0; i--) {
 203                         double cost = dn[i].cc + this->cost_search(dn[i], t);
 204                         this->steer(dn[i], t);
 205                         if (this->steered().size() == 0)
 206                                 break; // TODO why not continue?
 207                         if (std::get<0>(this->collide_steered_from(dn[i])))
 208                                 continue;
 209                         if (cost < t.cc) {
 210                                 t.cc = cost;
 211                                 t.p(dn[i].s);
 212                                 if (!dn[i].vi())
 213                                         pq.push(dn[i]);
 214                         }
 215                 }
 216         }
 217         DijkstraNode *fn = nullptr;
 218         for (auto &n: dn) {
 219                 if (n.v) {
 220                         fn = &n;
 221                         break;
 222                 }
 223         }
 224         if (fn == nullptr || fn->p() == nullptr)
 225                 return;
 226         DijkstraNode *ln = &dn.back();
 227         while (ln->p() != fn) {
 228                 RRTNode *t = ln->n;
 229                 RRTNode *f = ln->p()->n;
 230                 this->steer(*f, *t);
 231                 if (std::get<0>(this->collide_steered_from(*f)))
 232                         return;
 233                 this->join_steered(f);
 234                 t->p(&this->nodes().back());
 235                 t->c(this->cost_build(this->nodes().back(), *t));
 236                 ln = ln->p();
 237         }
 238         RRTS::compute_path();
 239 }
 240
 241 void RRTExt3::compute_path()
 242 {
 243         RRTS::compute_path();
 244         auto tstart = std::chrono::high_resolution_clock::now();
 245         this->first_path_optimization();
 246         this->first_optimized_path_ = RRTS::path();
 247         if (this->first_optimized_path_.size() > 0) {
 248                 this->first_optimized_path_cost(
 249                         this->first_optimized_path_.back()->cc
 250                 );
 251         } else {
 252                 return;
 253         }
 254         this->second_path_optimization();
 255         auto tend = std::chrono::high_resolution_clock::now();
 256         auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
 257             tend - tstart);
 258         this->log_opt_time_.push_back(tdiff.count());
 259 }
 260
 261 Json::Value RRTExt3::json()
 262 {
 263         Json::Value jvo = RRTS::json();
 264         jvo["orig_path_cost"] = this->orig_path_cost();
 265         {
 266                 unsigned int cu = 0;
 267                 unsigned int co = 0;
 268                 unsigned int pcnt = 0;
 269                 for (auto n: this->orig_path()) {
 270                         jvo["orig_path"][pcnt][0] = n->x();
 271                         jvo["orig_path"][pcnt][1] = n->y();
 272                         jvo["orig_path"][pcnt][2] = n->h();
 273                         if (n->t(RRTNodeType::cusp))
 274                                 cu++;
 275                         if (n->t(RRTNodeType::connected))
 276                                 co++;
 277                         pcnt++;
 278                 }
 279                 jvo["orig_cusps-in-path"] = cu;
 280                 jvo["orig_connecteds-in-path"] = co;
 281         }
 282         return jvo;
 283 }
 284
 285 void RRTExt3::json(Json::Value jvi)
 286 {
 287         return RRTS::json(jvi);
 288 }