Test for angle difference, maximum distance

diff --git a/src/rrts.cc b/src/rrts.cc
index 65b1c62c311c90db3df6afca69a93e349b2a6472..45dea55b8615f7f53775b090fc6d022c3c530ff2 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -229,6 +229,12 @@ bool RRTS::goal_found(RRTNode &f)
         bool in_zone = false;
         double cost = this->cost_build(f, g);
         // TODO find if ``f`` in ``g``'s zone.
+        double h_d = f.h() - g.h();
+        if (h_d < -M_PI/2 || h_d > M_PI/2)
+                return false;
+        double max_dist = g.mtr() * 2 * sin(M_PI/2 / 2); // mtr circle chord
+        if (sqrt(pow(f.x() - g.x(), 2) + pow(f.y() - g.y(), 2)) > max_dist)
+                return false;
         if (in_zone) {
                 found = true;
                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {