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                 if (this->orig_path().size() == 0)
   9                         return this->orig_path();
  10                 else
  11                         this->orig_path_cost(cc(*this->orig_path().back()));
  12         }
  13         class DijkstraNode : public RRTNode {
  14                 public:
  15                         DijkstraNode *s = nullptr;
  16                         RRTNode *n = nullptr;
  17                         unsigned int i = 0;
  18                         double cc = 9999;
  19                         bool v = false;
  20                         bool vi()
  21                         {
  22                                 if (this->v)
  23                                         return true;
  24                                 this->v = true;
  25                                 return false;
  26                         }
  27                         using RRTNode::RRTNode;
  28                         // override
  29                         DijkstraNode *p_ = nullptr;
  30                         DijkstraNode *p() const { return this->p_; }
  31                         void p(DijkstraNode *p) { this->p_ = p; }
  32         };
  33         class DijkstraNodeComparator {
  34                 public:
  35                         int operator() (
  36                                 const DijkstraNode &n1,
  37                                 const DijkstraNode &n2
  38                         )
  39                         {
  40                                 return n1.cc > n2.cc;
  41                         }
  42         };
  43         std::vector<DijkstraNode> dn;
  44         dn.reserve(this->orig_path().size());
  45         unsigned int dncnt = 0;
  46         for (auto n: this->orig_path()) {
  47                 if (
  48                         n->t(RRTNodeType::cusp)
  49                         || n->t(RRTNodeType::connected)
  50                 ) {
  51                         dn.push_back(DijkstraNode(*n));
  52                         dn.back().cc = cc(*n);
  53                         dn.back().s = &dn.back();
  54                         dn.back().n = n;
  55                         dn.back().i = dncnt++;
  56                 }
  57         }
  58         dn.push_back(DijkstraNode(*this->orig_path().back()));
  59         dn.back().cc = cc(*this->orig_path().back());
  60         dn.back().s = &dn.back();
  61         dn.back().n = this->orig_path().back();
  62         dn.back().i = dncnt++;
  63         std::priority_queue<
  64                 DijkstraNode,
  65                 std::vector<DijkstraNode>,
  66                 DijkstraNodeComparator
  67         > pq;
  68         dn.front().vi();
  69         pq.push(dn.front());
  70         while (!pq.empty()) {
  71                 DijkstraNode f = pq.top();
  72                 pq.pop();
  73                 for (unsigned int i = f.i + 1; i < dncnt; i++) {
  74                         double cost = f.cc + this->cost_search(f, dn[i]);
  75                         this->steer(f, dn[i]);
  76                         if (this->steered().size() == 0)
  77                                 break; // TODO why not continue?
  78                         if (std::get<0>(this->collide_steered_from(f)))
  79                                 continue;
  80                         if (cost < dn[i].cc) {
  81                                 dn[i].cc = cost;
  82                                 dn[i].p(f.s);
  83                                 if (!dn[i].vi())
  84                                         pq.push(dn[i]);
  85                         }
  86                 }
  87         }
  88         DijkstraNode *ln = nullptr;
  89         for (auto &n: dn) {
  90                 if (n.v)
  91                         ln = &n;
  92         }
  93         if (ln == nullptr || ln->p() == nullptr)
  94                 return this->orig_path();
  95         while (ln->p() != nullptr) {
  96                 RRTNode *t = ln->n;
  97                 RRTNode *f = ln->p()->n;
  98                 this->steer(*f, *t);
  99                 if (std::get<0>(this->collide_steered_from(*f)))
 100                         return this->orig_path();
 101                 this->join_steered(f);
 102                 t->p(&this->nodes().back());
 103                 t->c(this->cost_build(this->nodes().back(), *t));
 104                 ln = ln->p();
 105         }
 106         return RRTS::path();
 107 }
 108
 109 Json::Value RRTExt3::json()
 110 {
 111         Json::Value jvo = RRTS::json();
 112         jvo["orig_path_cost"] = this->orig_path_cost();
 113         {
 114                 unsigned int cu = 0;
 115                 unsigned int co = 0;
 116                 unsigned int pcnt = 0;
 117                 for (auto n: this->path()) {
 118                         jvo["orig_path"][pcnt][0] = n->x();
 119                         jvo["orig_path"][pcnt][1] = n->y();
 120                         jvo["orig_path"][pcnt][2] = n->h();
 121                         if (n->t(RRTNodeType::cusp))
 122                                 cu++;
 123                         if (n->t(RRTNodeType::connected))
 124                                 co++;
 125                         pcnt++;
 126                 }
 127                 jvo["orig_cusps-in-path"] = cu;
 128                 jvo["orig_connecteds-in-path"] = co;
 129         }
 130         return jvo;
 131 }
 132
 133 void RRTExt3::json(Json::Value jvi)
 134 {
 135         return RRTS::json(jvi);
 136 }