src/rrtext3.cc

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