Update ext8 to work in 3 dimensions

diff --git a/src/rrtext8.cc b/src/rrtext8.cc
index 9544db6666d3ff51cbf43064fddce0ec1e488d51..861ea87bed1b8d85df4bb3caa172f16e05e8326b 100644 (file)
--- a/src/rrtext8.cc
+++ b/src/rrtext8.cc
@@ -22,11 +22,15 @@ void RRTExt8::store_node(RRTNode *n, KdNode *&r, int l)
 {
         if (r == nullptr)
                 r = new KdNode(n);
-        else if (l % 2 == 0 && n->x() < r->node()->x())
+        else if (l % 3 == 0 && n->x() < r->node()->x())
                 store_node(n, r->left(), l + 1);
-        else if (l % 2 == 0)
+        else if (l % 3 == 0)
                 store_node(n, r->right(), l + 1);
-        else if (l % 2 == 1 && n->y() < r->node()->y())
+        else if (l % 3 == 1 && n->y() < r->node()->y())
+                store_node(n, r->left(), l + 1);
+        else if (l % 3 == 1)
+                store_node(n, r->right(), l + 1);
+        else if (l % 3 == 0 && n->h() < r->node()->h())
                 store_node(n, r->left(), l + 1);
         else
                 store_node(n, r->right(), l + 1);
@@ -40,22 +44,30 @@ void RRTExt8::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
                 n = r->node();
                 d = this->cost_search(*r->node(), t);
         }
-        if (l % 2 == 0 && t.x() < r->node()->x()) {
+        if (l % 3 == 0 && t.x() < r->node()->x()) {
                 nn(n, t, r->left(), l + 1, d);
                 if (r->node()->x() - t.x() < d)
                         nn(n, t, r->right(), l + 1, d);
-        } else if (l % 2 == 0) {
+        } else if (l % 3 == 0) {
                 nn(n, t, r->right(), l + 1, d);
                 if (t.x() - r->node()->x() < d)
                         nn(n, t, r->left(), l + 1, d);
-        } else if (l % 2 == 1 && t.y() < r->node()->y()) {
+        } else if (l % 3 == 1 && t.y() < r->node()->y()) {
                 nn(n, t, r->left(), l + 1, d);
                 if (r->node()->y() - t.y() < d)
                         nn(n, t, r->right(), l + 1, d);
-        } else {
+        } else if (l % 3 == 1) {
                 nn(n, t, r->right(), l + 1, d);
                 if (t.y() - r->node()->y() < d)
                         nn(n, t, r->left(), l + 1, d);
+        } else if (l % 3 == 2 && t.h() < r->node()->h()) {
+                nn(n, t, r->left(), l + 1, d);
+                if (r->node()->h() - t.h() < d)
+                        nn(n, t, r->right(), l + 1, d);
+        } else {
+                nn(n, t, r->right(), l + 1, d);
+                if (t.h() - r->node()->h() < d)
+                        nn(n, t, r->left(), l + 1, d);
         }
 }