Use drivable in goal zone

diff --git a/src/rrts.cc b/src/rrts.cc
index 6255b08a0aed3dbeb3a883e66528a667b81a69c8..b62a965fb2135a9ebe3bd475210e219a9ec1790e 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -224,84 +224,20 @@ void RRTS::join_steered(RRTNode *f)
 
 bool RRTS::goal_found(RRTNode &f)
 {
-        bool found = false;
         auto &g = this->goals().front();
-        bool in_zone = false;
         double cost = this->cost_build(f, g);
-        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;
-        double a = atan2(f.y() - g.y(), f.x() - g.x()) - g.h();
-        while (a < 0)
-                a += 2 * M_PI;
-        if (0 <= a && a < M_PI/2) { // left front g
-                BicycleCar zone_border(g);
-                zone_border.rotate(g.ccl().x(), g.ccl().y(), h_d);
-                a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
-                a -= g.h();
-                while (a < 0)
-                        a += 2 * M_PI;
-                double ub = zone_border.h() - g.h();
-                while (ub < 0)
-                        ub += 2 * M_PI;
-                if (0 <= a && a <= ub)
-                        in_zone = true;
-        } else if (M_PI/2 <= a && a < M_PI) { // left rear g
-                BicycleCar zone_border(g);
-                zone_border.rotate(g.ccl().x(), g.ccl().y(), h_d);
-                a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
-                if (sqrt(
-                        pow(f.y() - zone_border.y(), 2)
-                        + pow(f.x() - zone_border.x(), 2)
-                ) > 0)
-                        a -= M_PI;
-                a -= zone_border.h();
-                while (a < 0)
-                        a += 2 * M_PI;
-                double ub = g.h() - zone_border.h();
-                double lb = zone_border.h();
-                if (lb <= a && a <= ub)
-                        in_zone = true;
-        } else if (M_PI <= a && a < 3 * M_PI/2) { // right rear g
-                BicycleCar zone_border(g);
-                zone_border.rotate(g.ccr().x(), g.ccr().y(), h_d);
-                a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
-                if (sqrt(
-                        pow(f.y() - zone_border.y(), 2)
-                        + pow(f.x() - zone_border.x(), 2)
-                ) > 0)
-                        a -= M_PI;
-                a -= g.h();
-                double ub = zone_border.h() - g.h();
-                double lb = g.h();
-                if (lb <= a && a <= ub)
-                        in_zone = true;
-        } else if (3 * M_PI/2 <= a && a < 2 * M_PI) { // right front g
-                BicycleCar zone_border(g);
-                zone_border.rotate(g.ccr().x(), g.ccr().y(), h_d);
-                a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
-                a -= zone_border.h();
-                while (a < 0)
-                        a += 2 * M_PI;
-                double ub = g.h() - zone_border.h();
-                while (ub < 0)
-                        ub += 2 * M_PI;
-                if (0 <= a && a <= ub)
-                        in_zone = true;
-        } else {
-                // Not happenning, as ``a`` < 2 * M_PI.
-        }
-        if (in_zone) {
-                found = true;
+        if (g.drivable(f)) {
+                this->steer(f, g);
+                if (std::get<0>(this->collide_steered_from(f)))
+                        return false;
+                this->join_steered(&f);
                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {
                         g.p(&f);
                         g.c(cost);
                 }
+                return true;
         }
-        return found;
+        return false;
 }
 
 // RRT* procedures