Change overlap trees procedure

To be compatible with 2d indexing structure.

diff --git a/base/main.cc b/base/main.cc
index 87e0c154f017c7f7480dc05c4c859d097963bb4a..1442d4a0da3183bed719ce8bcd948876019c46c9 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -197,32 +197,25 @@ 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 = nullptr;
-        RRTNode *gn = nullptr;
         while (!gf && p.elapsed() < TMAX) {
                 // 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;
-                                        p.goal_found(true);
-                                        ron = rn;
-                                        gon = gn;
-                                        mc = rn->ccost() + gn->ccost();
-                                }
+                for (int i = 0; i < IXSIZE; i++) {
+                for (int j = 0; j < IYSIZE; j++) {
+                        if (p.p_root_.ixy_[i][j].changed() &&
+                                        p.p_goal_.ixy_[i][j].changed()) {
+for (auto rn: p.p_root_.ixy_[i][j].nodes()) {
+for (auto gn: p.p_goal_.ixy_[i][j].nodes()) {
+        if (rn->ccost() + gn->ccost() < mc &&
+                        IS_NEAR(rn, gn)) {
+                gf = true;
+                p.goal_found(true);
+                ron = rn;
+                gon = gn;
+                mc = rn->ccost() + gn->ccost();
+        }
+}}
                         }
-                }
+                }}
                 // end of overlap trees
                 p.tend();
         }