Implement different search for interesting nodes

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 634f325c783b7ac8d4a775e92e2ebd35bca378d0..04fa8c4177066ea397f956f4f8af97211bd2b43b 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,6 +1,6 @@
 #include <queue>
 #include "rrtext.h"
-
+#include <iostream>
 std::vector<RRTNode *> RRTExt13::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {
@@ -47,18 +47,38 @@ std::vector<RRTNode *> RRTExt13::first_path_optimization()
                         }
         };
         std::vector<DijkstraNode> dn;
-        dn.reserve(this->orig_path().size());
+        unsigned int ops = this->orig_path().size();
+        auto op = this->orig_path();
+        dn.reserve(ops);
         unsigned int dncnt = 0;
-        for (auto n: this->orig_path()) {
-                if (
-                        n->t(RRTNodeType::cusp)
-                        || n->t(RRTNodeType::connected)
-                ) {
-                        dn.push_back(DijkstraNode(*n));
-                        dn.back().cc = cc(*n);
-                        dn.back().s = &dn.back();
-                        dn.back().n = n;
-                        dn.back().i = dncnt++;
+        dn.push_back(DijkstraNode(*this->orig_path().front()));
+        dn.back().cc = cc(*this->orig_path().front());
+        dn.back().s = &dn.back();
+        dn.back().n = this->orig_path().front();
+        dn.back().i = dncnt++;
+        for (unsigned int i = 0; i < ops - 1; i++) {
+                auto ibc = BicycleCar();
+                ibc.x(op[i]->x());
+                ibc.y(op[i]->y());
+                ibc.h(op[i]->h());
+                for (unsigned int j = i + 1; j < ops; j++) {
+                        auto jbc = BicycleCar();
+                        jbc.x(op[j]->x());
+                        jbc.y(op[j]->y());
+                        jbc.h(op[j]->h());
+                        if (!jbc.drivable(ibc)) {
+                                //std::cerr<<*op[i];
+                                //std::cerr<<" to ";
+                                //std::cerr<<*op[j];
+                                //std::cerr<<std::endl;
+                                dn.push_back(DijkstraNode(*op[j-1]));
+                                dn.back().cc = cc(*op[j-1]);
+                                dn.back().s = &dn.back();
+                                dn.back().n = op[j-1];
+                                dn.back().i = dncnt++;
+                                i = j;
+                                break;
+                        }
                 }
         }
         dn.push_back(DijkstraNode(*this->orig_path().back()));
@@ -154,18 +174,38 @@ std::vector<RRTNode *> RRTExt13::second_path_optimization()
                         }
         };
         std::vector<DijkstraNode> dn;
-        dn.reserve(this->orig_path().size());
+        unsigned int ops = this->orig_path().size();
+        auto op = this->orig_path();
+        dn.reserve(ops);
         unsigned int dncnt = 0;
-        for (auto n: this->orig_path()) {
-                if (
-                        n->t(RRTNodeType::cusp)
-                        || n->t(RRTNodeType::connected)
-                ) {
-                        dn.push_back(DijkstraNode(*n));
-                        dn.back().cc = cc(*n);
-                        dn.back().s = &dn.back();
-                        dn.back().n = n;
-                        dn.back().i = dncnt++;
+        dn.push_back(DijkstraNode(*this->orig_path().front()));
+        dn.back().cc = cc(*this->orig_path().front());
+        dn.back().s = &dn.back();
+        dn.back().n = this->orig_path().front();
+        dn.back().i = dncnt++;
+        for (unsigned int i = ops - 1; i > 1; i--) {
+                auto ibc = BicycleCar();
+                ibc.x(op[i]->x());
+                ibc.y(op[i]->y());
+                ibc.h(op[i]->h());
+                for (unsigned int j = i - 1; j > 0; j--) {
+                        auto jbc = BicycleCar();
+                        jbc.x(op[j]->x());
+                        jbc.y(op[j]->y());
+                        jbc.h(op[j]->h());
+                        if (!jbc.drivable(ibc)) {
+                                //std::cerr<<*op[i];
+                                //std::cerr<<" to ";
+                                //std::cerr<<*op[j];
+                                //std::cerr<<std::endl;
+                                dn.push_back(DijkstraNode(*op[j-1]));
+                                dn.back().cc = cc(*op[j-1]);
+                                dn.back().s = &dn.back();
+                                dn.back().n = op[j-1];
+                                dn.back().i = dncnt++;
+                                i = j;
+                                break;
+                        }
                 }
         }
         dn.push_back(DijkstraNode(*this->orig_path().back()));