Add in zone computation

Compute if ``f`` position from ``zone_border`` is between
``zone_border`` heading and ``g`` heading.

diff --git a/src/rrts.cc b/src/rrts.cc
index b2966e506de604276f73e002f97783a0b729ead6..852ca399bac637980ee386efacdd34a24f1c6803 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -244,6 +244,24 @@ bool RRTS::goal_found(RRTNode &f)
         BicycleCar zone_border(g);
         zone_border.rotate(x_mtr, y_mtr, h_d);
         // zone_border.h() == f.h() now
+        double a_lb = 0;
+        double a_ub = g.h() - zone_border.h();
+        double a =
+                atan2(f.y() - zone_border.y(), f.x() - zone_border.x())
+                - zone_border.h()
+        ;
+        if (h_d > 0) {
+                a_ub = zone_border.h() - g.h();
+                a =
+                        atan2(f.y() - zone_border.y(), f.x() - zone_border.x())
+                        - g.h()
+                ;
+        }
+        while (a_lb < 0) a_lb += 2 * M_PI;
+        while (a < 0) a += 2 * M_PI;
+        while (a_ub < 0) a_ub += 2 * M_PI;
+        if (a_lb <= a && a <= a_ub)
+                in_zone = true;
         if (in_zone) {
                 found = true;
                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {