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<RRTNode *> path;
  40         for (auto n: this->orig_path()) {
  41                 if (n->t(RRTNodeType::cusp) || n->t(RRTNodeType::connected))
  42                         path.push_back(n);
  43         }
  44         path.push_back(this->orig_path().back());
  45         std::priority_queue<
  46                 DijkstraNode,
  47                 std::vector<DijkstraNode>,
  48                 DijkstraNodeComparator
  49         > pq;
  50         while (!pq.empty()) {
  51                 DijkstraNode f = pq.top();
  52                 pq.pop();
  53         }
  54         return path;
  55 }