Add second path optimization method

diff --git a/api/rrtext.h b/api/rrtext.h
index d17697f726bbc5e8dc6cec39465141ac34b47366..18d1f46a096af689847421b8fd2a6b2b2c612fdd 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -227,6 +227,7 @@ class RRTExt3 : public virtual RRTS {
                 double first_optimized_path_cost_ = 9999;
         public:
                 std::vector<RRTNode *> first_path_optimization();
+                std::vector<RRTNode *> second_path_optimization();
                 std::vector<RRTNode *> path();
                 Json::Value json();
                 void json(Json::Value jvi);

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 2ca6e74b083f03410788597b29da596e182f48d6..20d5b4465a8416c8817fa30298718d21fee97e54 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -106,6 +106,111 @@ std::vector<RRTNode *> RRTExt3::first_path_optimization()
         return RRTS::path();
 }
 
+std::vector<RRTNode *> RRTExt3::second_path_optimization()
+{
+        if (this->first_optimized_path().size() == 0) {
+                return this->orig_path();
+        }
+        class DijkstraNode : public RRTNode {
+                public:
+                        DijkstraNode *s = nullptr;
+                        RRTNode *n = nullptr;
+                        unsigned int i = 0;
+                        double cc = 9999;
+                        bool v = false;
+                        bool vi()
+                        {
+                                if (this->v)
+                                        return true;
+                                this->v = true;
+                                return false;
+                        }
+                        using RRTNode::RRTNode;
+                        // override
+                        DijkstraNode *p_ = nullptr;
+                        DijkstraNode *p() const { return this->p_; }
+                        void p(DijkstraNode *p) { this->p_ = p; }
+        };
+        class DijkstraNodeComparator {
+                public:
+                        int operator() (
+                                const DijkstraNode &n1,
+                                const DijkstraNode &n2
+                        )
+                        {
+                                return n1.cc > n2.cc;
+                        }
+        };
+        std::vector<DijkstraNode> dn;
+        dn.reserve(this->orig_path().size());
+        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().back()));
+        dn.back().cc = cc(*this->orig_path().back());
+        dn.back().s = &dn.back();
+        dn.back().n = this->orig_path().back();
+        dn.back().i = dncnt++;
+        std::priority_queue<
+                DijkstraNode,
+                std::vector<DijkstraNode>,
+                DijkstraNodeComparator
+        > pq;
+        // TODO rewrite
+        dn.back().vi();
+        pq.push(dn.back());
+        while (!pq.empty()) {
+                DijkstraNode t = pq.top();
+                pq.pop();
+                for (unsigned int i = t.i - 1; i > 0; i--) {
+                        double cost = dn[i].cc + this->cost_search(dn[i], t);
+                        this->steer(dn[i], t);
+                        if (this->steered().size() == 0)
+                                break; // TODO why not continue?
+                        if (std::get<0>(this->collide_steered_from(dn[i])))
+                                continue;
+                        if (cost < t.cc) {
+                                t.cc = cost;
+                                t.p(dn[i].s);
+                                if (!dn[i].vi())
+                                        pq.push(dn[i]);
+                        }
+                }
+        }
+        DijkstraNode *fn = nullptr;
+        for (auto &n: dn) {
+                if (n.v) {
+                        fn = &n;
+                        break;
+                }
+        }
+        if (fn == nullptr || fn->p() == nullptr)
+                return this->first_optimized_path();
+        DijkstraNode *ln = &dn.back();
+        while (ln->p() != fn) {
+                RRTNode *t = ln->n;
+                RRTNode *f = ln->p()->n;
+                this->steer(*f, *t);
+                if (std::get<0>(this->collide_steered_from(*f)))
+                        return this->first_optimized_path();
+                this->join_steered(f);
+                t->p(&this->nodes().back());
+                t->c(this->cost_build(this->nodes().back(), *t));
+                ln = ln->p();
+        }
+        return RRTS::path();
+}
+
 std::vector<RRTNode *> RRTExt3::path()
 {
         this->first_optimized_path_ = this->first_path_optimization();
@@ -113,7 +218,9 @@ std::vector<RRTNode *> RRTExt3::path()
                 this->first_optimized_path_cost(
                         cc(*this->first_optimized_path_.back())
                 );
-        return this->first_optimized_path_;
+        else
+                return this->orig_path();
+        return this->second_path_optimization();
 }
 
 Json::Value RRTExt3::json()