Add tree overlap procedure

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 5cac04899b9cfab15a0a64ada723eb9cfb50d075..9e8043943e927502e0e17b5c56a7e7592192aa90 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -598,22 +598,16 @@ T3::T3(RRTNode *init, RRTNode *goal):
 
 bool T3::next()
 {
+        RRTNode *ron = nullptr;
+        RRTNode *gon = nullptr;
         bool ret = false;
         ret = this->p_root_.next();
         ret &= this->p_goal_.next();
-        for (auto rn: this->p_root_.nodes()) {
-                if (rn->parent() == nullptr)
-                        continue;
-                for (auto gn: this->p_goal_.nodes()) {
-                        if (gn->parent() == nullptr)
-                                continue;
-                        if (IS_NEAR(rn, gn)) {
-                                if (this->connecttrees(rn, gn))
-                                        this->goal_found(true);
-                                this->tlog(this->findt());
-                                return true;
-                        }
-                }
+        if (this->overlaptrees(&ron, &gon)) {
+                if (this->connecttrees(ron, gon))
+                        this->goal_found(true);
+                this->tlog(this->findt());
+                ret &= true;
         }
         return ret;
 }
@@ -647,3 +641,21 @@ bool T3::connecttrees(RRTNode *rn, RRTNode *gn)
         rn->add_child(this->goal(), this->p_root_.cost(rn, this->goal()));
         return true;
 }
+
+bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
+{
+        for (auto rn: this->p_root_.nodes()) {
+                if (rn->parent() == nullptr)
+                        continue;
+                for (auto gn: this->p_goal_.nodes()) {
+                        if (gn->parent() == nullptr)
+                                continue;
+                        if (IS_NEAR(rn, gn)) {
+                                *ron = rn;
+                                *gon = gn;
+                                return true;
+                        }
+                }
+        }
+        return false;
+}

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index dfc133d538da8e8b6e73de56af16e66b6ebea502..3af8cfd0756bfcb472cd900048fd1ec5d6d64109 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -152,6 +152,7 @@ class T3: public RRTBase {
                                 std::vector<CircleObstacle> *cobstacles,
                                 std::vector<SegmentObstacle> *sobstacles);
                 bool connecttrees(RRTNode *rn, RRTNode *gn);
+                bool overlaptrees(RRTNode **ron, RRTNode **gon);
 };
 
 #endif