Add trees overlap check

diff --git a/base/main.cc b/base/main.cc
index 0a426efba42bf2d0c2604f6ab3e0192238743437..1b176f86ed0b5184d8f98b7c28abb89849492f92 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -177,6 +177,7 @@ int main()
         }
 #elif defined USE_PTHREAD
         bool gf = false;
+        float mc = 9999;
         pthread_t rt; // root thread
         pthread_t gt; // goal thread
         pthread_t ct; // connect thread
@@ -194,8 +195,29 @@ int main()
         p.p_goal_.tstart();
         pthread_create(&rt, NULL, &next_run, (void *) &ra);
         pthread_create(&gt, NULL, &next_run, (void *) &ga);
+        volatile int nofrn = 0;
+        volatile int nofgn = 0;
+        RRTNode *rn;
+        RRTNode *gn;
         while (!gf && p.elapsed() < TMAX) {
-                sleep(0.2);
+                // overlap trees
+                nofrn = p.p_root_.nodes().size() - 1;  // TODO workaround
+                nofgn = p.p_goal_.nodes().size() - 1;
+                for (int i = 0; i < nofrn; i++) {
+                        rn = p.p_root_.nodes()[i];
+                        if (rn->parent() == nullptr)
+                                continue;
+                        for (int j = 0; j < nofgn; j++) {
+                                gn = p.p_goal_.nodes()[j];
+                                if (gn->parent() == nullptr)
+                                        continue;
+                                if (rn->ccost() + gn->ccost() < mc &&
+                                                IS_NEAR(rn, gn)) {
+                                        gf = true;
+                                }
+                        }
+                }
+                // end of overlap trees
                 p.tend();
         }
         pthread_join(rt, NULL);