src/rrtext3.cc

   1 #include <queue>
   2 #include "rrtext.h"
   3
   4 std::vector<RRTNode *> RRTExt3::path()
   5 {
   6         if (this->orig_path().size() == 0) {
   7                 this->orig_path_ = RRTS::path();
   8         }
   9         class DijkstraNode : public RRTNode {
  10                 public:
  11                         DijkstraNode *s = nullptr;
  12                         RRTNode *n = nullptr;
  13                         unsigned int i = 0;
  14                         double cc = 9999;
  15                         bool v = false;
  16                         bool vi()
  17                         {
  18                                 if (this->v)
  19                                         return true;
  20                                 this->v = true;
  21                                 return false;
  22                         }
  23                         using RRTNode::RRTNode;
  24                         // override
  25                         DijkstraNode *p_ = nullptr;
  26                         DijkstraNode *p() const { return this->p_; }
  27                         void p(DijkstraNode *p) { this->p_ = p; }
  28         };
  29         class DijkstraNodeComparator {
  30                 public:
  31                         int operator() (
  32                                 const DijkstraNode &n1,
  33                                 const DijkstraNode &n2
  34                         )
  35                         {
  36                                 return n1.cc > n2.cc;
  37                         }
  38         };
  39         std::vector<DijkstraNode> dn;
  40         dn.reserve(this->orig_path().size());
  41         unsigned int dncnt = 0;
  42         for (auto n: this->orig_path()) {
  43                 if (
  44                         n->t(RRTNodeType::cusp)
  45                         || n->t(RRTNodeType::connected)
  46                 ) {
  47                         dn.push_back(DijkstraNode(*n));
  48                         dn.back().cc = cc(*n);
  49                         dn.back().s = &dn.back();
  50                         dn.back().n = n;
  51                         dn.back().i = dncnt++;
  52                 }
  53         }
  54         dn.push_back(DijkstraNode(*this->orig_path().back()));
  55         dn.back().cc = cc(*this->orig_path().back());
  56         dn.back().s = &dn.back();
  57         dn.back().n = this->orig_path().back();
  58         dn.back().i = dncnt++;
  59         std::priority_queue<
  60                 DijkstraNode,
  61                 std::vector<DijkstraNode>,
  62                 DijkstraNodeComparator
  63         > pq;
  64         dn.front().vi();
  65         pq.push(dn.front());
  66         while (!pq.empty()) {
  67                 DijkstraNode f = pq.top();
  68                 pq.pop();
  69                 for (unsigned int i = f.i + 1; i < dncnt; i++) {
  70                         double cost = f.cc + this->cost_search(f, dn[i]);
  71                         this->steer(f, dn[i]);
  72                         if (this->steered().size() == 0)
  73                                 break; // TODO why not continue?
  74                         if (std::get<0>(this->collide_steered_from(f)))
  75                                 continue;
  76                         if (cost < dn[i].cc) {
  77                                 dn[i].cc = cost;
  78                                 dn[i].p(f.s);
  79                                 if (!dn[i].vi())
  80                                         pq.push(dn[i]);
  81                         }
  82                 }
  83         }
  84         DijkstraNode *ln = nullptr;
  85         for (auto &n: dn) {
  86                 if (n.v)
  87                         ln = &n;
  88         }
  89         if (ln == nullptr || ln->p() == nullptr)
  90                 return this->orig_path();
  91         std::vector<RRTNode *> path;
  92         return path;
  93 }