From 45855db8d74aa873c1725090352f1348afac35c0 Mon Sep 17 00:00:00 2001 From: Balduin Date: Thu, 7 May 2026 16:04:06 +0200 Subject: [PATCH] Big refactor making line-polygon intersection robust Rewrite everything in terms of primitive orient2d - https://perso.uclouvain.be/jean-francois.remacle/LMECA2170/robnotes.pdf Rather than trying to do it carefully in float, convert everything to fixed-point (int32 centimeters) so geometry questions can be answered exactly - http://www.science.smith.edu/~jorourke/books/compgeom.html The fixed point polygon vertices are stored in PlannerPolygons, which also exposes all necessary geometry operations. Three API layers: - Functions taking int32 coordinates directly - Functions taking vertex indices and calling the above - isLineFromPointToNodeIntersectingAnyInside, which takes the _float meter_ vehicle local position and gives the visible graph nodes Next steps: - Profile, performance should be better or similar - Geofence::checkIfLineViolatesAnyFence rebuilds a PlannerPolygons object every call, could optimise - Only the midpoint is check. A contrived nonconvex polygon with multiple vertex hits could slip through - Ignore or make test & fix - Related: midpoint could land outside due to integer division - Find if there is still a way to simplify / clarify the handling of the exclusion/inclusion difference, and write more tests for all the edge cases. - rename some things, for sure isLineFromPointToNodeIntersectingAnyInside, and comment where logic is not clear - lineSegmentIntersectsCircle is still float, could change to fixed - But do we even want to check circle intersection, or should we use the k-gon approximation consistently? - Input data validation: Complain if - Polygon non-simple (self intersecting) - Geofence features smaller than margin or distances smaller than 2*margin, so the shrinking gives topology changes - Next iteration could handle this even better by allowing some violations (of only the expanded polygons) but at high cost - Anything else --- src/lib/geofence/GeofenceUtilsTest.cpp | 262 ++++++++++++++++-- src/lib/geofence/geofence_utils.cpp | 170 ++++++++++-- src/lib/geofence/geofence_utils.h | 205 ++++++++++++-- .../GeofenceAvoidancePlannerTest.cpp | 14 +- .../geofence_avoidance_planner.cpp | 123 +++++--- .../geofence_avoidance_planner.h | 26 +- src/modules/navigator/geofence.cpp | 47 ++-- 7 files changed, 702 insertions(+), 145 deletions(-) diff --git a/src/lib/geofence/GeofenceUtilsTest.cpp b/src/lib/geofence/GeofenceUtilsTest.cpp index 5b634bfd9e..0c7267541c 100644 --- a/src/lib/geofence/GeofenceUtilsTest.cpp +++ b/src/lib/geofence/GeofenceUtilsTest.cpp @@ -37,29 +37,259 @@ using namespace matrix; + +// Layer-1 primitives are tested directly in int32 (no float, no cm semantics). + +TEST(GeofenceUtilsTest, Orient2d) +{ + // CCW turn -> +1, CW turn -> -1, collinear -> 0. + EXPECT_EQ(1, geofence_utils::orient2d(0, 0, 1, 0, 0, 1)); + EXPECT_EQ(-1, geofence_utils::orient2d(0, 0, 1, 0, 0, -1)); + EXPECT_EQ(0, geofence_utils::orient2d(0, 0, 2, 2, 1, 1)); + EXPECT_EQ(0, geofence_utils::orient2d(0, 0, 2, 2, 3, 3)); +} + +using SS = geofence_utils::SegSegResult; + TEST(GeofenceUtilsTest, SegmentsSharedEndpointNoIntersection) { - // Two segments share endpoint (1,1) but go in different directions — no crossing - Vector2f p1(0.f, 0.f); - Vector2f p2(2.f, 2.f); - - Vector2f v1(1.f, 1.f); - Vector2f v2(2.f, 2.f); - - EXPECT_FALSE(geofence_utils::segmentsIntersect(p1, p2, v1, v2)); + // Collinear, second segment is a sub-interval of the first. + EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 2, 2, 1, 1, 2, 2)); } TEST(GeofenceUtilsTest, SegmentsCross) { - // vertical line from origin straight up - Vector2f p1(0.f, 0.f); - Vector2f p2(0.f, 1.f); - - Vector2f v1(-0.0001f, 0.0001f); - Vector2f v2(1.f, 0.0001f); - - EXPECT_TRUE(geofence_utils::segmentsIntersect(p1, p2, v1, v2)); + // Vertical ab vs horizontal cd just above the x-axis; strict interior crossing. + EXPECT_EQ(SS::Cross, geofence_utils::segmentsIntersect(0, 0, 0, 100, -1, 1, 100, 1)); } + +TEST(GeofenceUtilsTest, SegmentsTouching) +{ + // Endpoint c of cd lies strictly on the open ab. + EXPECT_EQ(SS::CInsideAB, geofence_utils::segmentsIntersect(0, 0, 0, 200, 0, 100, 100, 100)); + // Argument order swapped: now endpoint a of the first segment is on cd. + EXPECT_EQ(SS::AInsideCD, geofence_utils::segmentsIntersect(0, 100, 100, 100, 0, 0, 0, 200)); + + // Slanted variant. + EXPECT_EQ(SS::CInsideAB, geofence_utils::segmentsIntersect(-100, 0, 100, 200, 0, 100, 100, 100)); + EXPECT_EQ(SS::AInsideCD, geofence_utils::segmentsIntersect(0, 100, 100, 100, -100, 0, 100, 200)); +} + +TEST(GeofenceUtilsTest, SegmentsParallel) +{ + // Disjoint, non-collinear. + EXPECT_EQ(SS::Disjoint, geofence_utils::segmentsIntersect(0, 0, 300, 0, 1000, 1000, 4000, 2000)); + + // A segment with itself is collinear, not a proper crossing. + EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 300, 0, 0, 0, 300, 0)); + EXPECT_EQ(SS::Collinear, + geofence_utils::segmentsIntersect(1000, 1000, 4000, 2000, 1000, 1000, 4000, 2000)); +} + +TEST(GeofenceUtilsTest, SegmentsCollinearDisjoint) +{ + // Two segments on the same line but with non-overlapping intervals. + EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 100, 0, 200, 0, 300, 0)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonExclusionOutside) +{ + static constexpr int N = 4; + + // Unit square (exclusion zone: inside disallowed) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line in a completely random place + Vector2f l1(4.f, 5.f); + Vector2f l2(5.f, 4.f); + + // Outside an exclusion zone is allowed -> no intersection + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false)); + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonExclusionThroughEdge) +{ + static constexpr int N = 4; + + // Unit square (exclusion zone) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line obviously passing through an edge + Vector2f l1(0.5f, 0.5f); + Vector2f l2(0.5f, 1.5f); + + // Crossing an edge always counts as intersection regardless of zone type + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false)); + + // Line through several edges + Vector2f l3(0.5f, -0.5f); + Vector2f l4(0.5f, 1.5f); + + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonExclusionInside) +{ + static constexpr int N = 4; + + // Unit square (exclusion zone) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line going exactly between opposite vertices + Vector2f l1(0.f, 0.f); + Vector2f l2(1.f, 1.f); + + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false)); + + // Line exactly touching sides + Vector2f l3(0.5f, 0.0f); + Vector2f l4(0.5f, 1.0f); + + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false)); + + // Line completely in the interior of an exclusion zone -> disallowed + Vector2f l5(0.2f, 0.2f); + Vector2f l6(0.6f, 0.5f); + + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l5, l6, vertices, N, false)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l5, l6, vertices_rev, N, false)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonExclusionTouching) +{ + static constexpr int N = 4; + + // Unit square (exclusion zone) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line exactly equal to one of the edges + Vector2f l1(0.f, 0.f); + Vector2f l2(1.f, 0.f); + + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false)); + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false)); + + // Same line but longer at one end - fails + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, 2.f * l2, vertices, N, false)); + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, 2.f * l2, vertices_rev, N, false)); + + // A line just skimming the (1, 1) vertex but staying outside + Vector2f l3(2.f, 0.f); + Vector2f l4(0.f, 2.f); + + // Should be no intersection + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false)); + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false)); + + // But if we move the line a couple of cm towards the square + l4(1) = 1.98f; + + // We have a (strict) intersection + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonInclusionOutside) +{ + static constexpr int N = 4; + + // Unit square (inclusion zone: outside disallowed) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line in a completely random place outside the polygon + Vector2f l1(4.f, 5.f); + Vector2f l2(5.f, 4.f); + + // Outside an inclusion zone is disallowed -> intersection + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, true)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, true)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonInclusionThroughEdge) +{ + static constexpr int N = 4; + + // Unit square (inclusion zone) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line crossing an edge from inside to outside + Vector2f l1(0.5f, 0.5f); + Vector2f l2(0.5f, 1.5f); + + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, true)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, true)); + + // Line through several edges + Vector2f l3(0.5f, -0.5f); + Vector2f l4(0.5f, 1.5f); + + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, true)); + EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, true)); +} + +TEST(GeofenceUtilsTest, SegmentPolygonInclusionInside) +{ + static constexpr int N = 4; + + // Unit square (inclusion zone) + Vector2f p1(0.f, 0.f); + Vector2f p2(1.f, 0.f); + Vector2f p3(1.f, 1.f); + Vector2f p4(0.f, 1.f); + + Vector2f vertices[N] = {p1, p2, p3, p4}; + Vector2f vertices_rev[N] = {p4, p3, p2, p1}; + + // Line completely in the interior of an inclusion zone -> allowed + Vector2f l1(0.2f, 0.2f); + Vector2f l2(0.6f, 0.5f); + + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, true)); + EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, true)); +} + + TEST(GeofenceUtilsTest, SegmentAndCircleIntersect) { // vertical line from origin straight up diff --git a/src/lib/geofence/geofence_utils.cpp b/src/lib/geofence/geofence_utils.cpp index 0594341afc..9743ba3d7c 100644 --- a/src/lib/geofence/geofence_utils.cpp +++ b/src/lib/geofence/geofence_utils.cpp @@ -37,17 +37,13 @@ namespace geofence_utils { -bool insidePolygon(const matrix::Vector2f *vertices, int num_vertices, - const matrix::Vector2f &point) +namespace { - bool c = false; - for (int i = 0, j = num_vertices - 1; i < num_vertices; j = i++) { - insidePolygonUpdateState(c, vertices[i], vertices[j], point); - } +// Float-meter to int32-cm: the only conversion at the float/cm boundary. +int32_t metersToCm(float m) { return static_cast(std::llroundf(m * 100.f)); } - return c; -} +} // namespace bool insideCircle(const matrix::Vector2 ¢er, float radius, const matrix::Vector2 &point) @@ -56,34 +52,152 @@ bool insideCircle(const matrix::Vector2 ¢er, float radius, return dist < radius; } -bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2, - const matrix::Vector2f &v1, const matrix::Vector2f &v2) +int PlannerPolygons::addNode(const matrix::Vector2f &p) { - // line intersection algorithm from wikipedia - // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection - // Basic idea: a 2D vector formula for each line is created, consisting of a start point e.g. p1 - // and a direction vector pointing from p1 to p2. A running variable t [0,1] defines the position - // on the line betwen p1 and p2. So in this case the formula for the line is P = p1 + t * (p2-p1). - // The same is done for the second line and then both lines are set to be equal (to find the intersection). - // We get two equations with two unknowns (t and u) which can be solved. If the denominator is zero, the lines are parallel - // or coinciding, which means we can stop. If the solution for t and u is between 0 and 1, the line segments intersect, otherwise they don't. + if (_num_nodes >= kMaxNodes) { return -1; } - float x1 = p1(0), y1 = p1(1); - float x2 = p2(0), y2 = p2(1); - float x3 = v1(0), y3 = v1(1); - float x4 = v2(0), y4 = v2(1); + setNode(_num_nodes, p); + return _num_nodes++; +} - float denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); +void PlannerPolygons::setNode(int idx, const matrix::Vector2f &p) +{ + _x_cm[idx] = metersToCm(p(0)); + _y_cm[idx] = metersToCm(p(1)); +} - if (fabsf(denominator) < FLT_EPSILON) { +bool PlannerPolygons::addPolygon(const matrix::Vector2f *vertices_in, int num_vertices, bool is_inclusion_zone) +{ + if (num_vertices < 3 + || _num_polygons >= kMaxPolygons + || _num_nodes + num_vertices > kMaxNodes) { return false; } - float t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / denominator; - float u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / denominator; + // Canonical orientation: walking vertices in stored order, INSIDE is on + // the left of each edge. CCW for exclusion (bounded interior is forbidden), + // CW for inclusion (unbounded exterior is forbidden). Reverse iff input + // doesn't already match. + const bool reverse = (isPolygonCCW(vertices_in, num_vertices) == is_inclusion_zone); - // lines intersect if both running variables are between 0 and 1 - return t > 0.0f && t < 1.0f && u > 0.0f && u < 1.0f; + PolygonInfo &poly = _polygons[_num_polygons]; + poly.start_index = _num_nodes; + poly.num_vertices = num_vertices; + poly.inside_is_bounded = !is_inclusion_zone; + + for (int i = 0; i < num_vertices; i++) { + setNode(poly.start_index + i, vertices_in[reverse ? (num_vertices - 1 - i) : i]); + } + + _num_nodes += num_vertices; + ++_num_polygons; + return true; +} + +// Standard convex/reflex vertex test: at a convex vertex Inside is the small +// wedge between the two incident edges (left of BOTH); at a reflex vertex it +// is the large arc (left of EITHER). +bool PlannerPolygons::pointInsideInteriorCone(const PolygonInfo &poly, + int32_t px, int32_t py, int v) const +{ + const int prev = poly.start_index + ((v == 0) ? poly.num_vertices - 1 : v - 1); + const int curr = poly.start_index + v; + const int next = poly.start_index + ((v + 1) % poly.num_vertices); + + const int o_in = orient2d(_x_cm[prev], _y_cm[prev], _x_cm[curr], _y_cm[curr], px, py); + const int o_out = orient2d(_x_cm[curr], _y_cm[curr], _x_cm[next], _y_cm[next], px, py); + const int is_convex = orient2d(_x_cm[prev], _y_cm[prev], _x_cm[curr], _y_cm[curr], _x_cm[next], _y_cm[next]); + + return is_convex > 0 // If convex == 0 (exact 180deg vertex) the below cases are equivalent + ? (o_in > 0 && o_out > 0) // Inside = intersection of both half-planes + : (o_in > 0 || o_out > 0); // Inside = union of both half-planes +} + +bool PlannerPolygons::intersectsInsideOf(const PolygonInfo &poly, + int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const +{ + const int32_t mid_x = (s_x + e_x) / 2; + const int32_t mid_y = (s_y + e_y) / 2; + + // Single pass over polygon edges, doing two jobs at once: + // (1) classify the test segment vs each polygon edge -- early-return on + // a proper crossing, wedge-check polygon vertices that sit strictly + // on the open test segment; + // (2) accumulate Sunday's winding-number contribution for the midpoint, + // used (with poly.inside_is_bounded) to classify the no-crossing + // case as Inside or Outside. + int wn = 0; + bool mid_on_boundary = false; + + for (int i = 0; i < poly.num_vertices; i++) { + const int prev_idx = poly.start_index + ((i == 0) ? poly.num_vertices - 1 : i - 1); + const int curr_idx = poly.start_index + i; + const int32_t ax = _x_cm[prev_idx], ay = _y_cm[prev_idx]; + const int32_t bx = _x_cm[curr_idx], by = _y_cm[curr_idx]; + + // (1) test segment vs polygon edge (a, b) + switch (segmentsIntersect(ax, ay, bx, by, s_x, s_y, e_x, e_y)) { + case SegSegResult::Cross: + return true; + + case SegSegResult::BInsideCD: + // polygon vertex `b` strictly on the open test segment -- a graze + // unless the wedge classifies the two endpoints differently + if (pointInsideInteriorCone(poly, s_x, s_y, i) != + pointInsideInteriorCone(poly, e_x, e_y, i)) { + return true; + } + + break; + + default: + break; + } + + // (2) midpoint winding contribution (Sunday) + const int side = orient2d(ax, ay, bx, by, mid_x, mid_y); + + if (side == 0 && collinearBetween(ax, ay, bx, by, mid_x, mid_y)) { + mid_on_boundary = true; + + } else if (ay <= mid_y) { + if (by > mid_y && side > 0) { wn++; } + + } else if (by <= mid_y && side < 0) { wn--; } + } + + // Midpoint on the boundary: segment runs along it; non-violating in either zone. + if (mid_on_boundary) { + return false; + } + + // wn != 0 means the midpoint is in the bounded region. The only place + // orientation polarity surfaces. + const bool bounded = (wn != 0); + return bounded == poly.inside_is_bounded; +} + +bool PlannerPolygons::intersectsAnyInside(int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const +{ + for (int p = 0; p < _num_polygons; p++) { + if (intersectsInsideOf(_polygons[p], s_x, s_y, e_x, e_y)) { + return true; + } + } + + return false; +} + +bool PlannerPolygons::isLineBetweenNodesIntersectingAnyInside(int a, int b) const +{ + if (a == b) { return false; } + + return intersectsAnyInside(_x_cm[a], _y_cm[a], _x_cm[b], _y_cm[b]); +} + +bool PlannerPolygons::isLineFromPointToNodeIntersectingAnyInside(const matrix::Vector2f &p, int node_idx) const +{ + return intersectsAnyInside(metersToCm(p(0)), metersToCm(p(1)), _x_cm[node_idx], _y_cm[node_idx]); } bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end, diff --git a/src/lib/geofence/geofence_utils.h b/src/lib/geofence/geofence_utils.h index c0f5b746d2..0482be5874 100644 --- a/src/lib/geofence/geofence_utils.h +++ b/src/lib/geofence/geofence_utils.h @@ -38,6 +38,10 @@ #pragma once +#include +#include +#include + #include namespace geofence_utils @@ -84,30 +88,90 @@ bool insideCircle(const matrix::Vector2 ¢er, float radius, const matrix::Vector2 &point); /** - * Check if two line segments intersect (excluding endpoints). - * Works in local Cartesian coordinates (meters). - * - * @param p1 first segment start in local frame - * @param p2 first segment end in local frame - * @param v1 second segment start in local frame - * @param v2 second segment end in local frame - * @return true if the segments intersect + * Sign of the signed area of triangle abc -- the fundamental 2D orientation + * predicate. +1 if c is left of a->b (CCW turn), -1 if right (CW turn), + * 0 if collinear. Reference: O'Rourke, "Computational Geometry in C", 1.5. */ -bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2, - const matrix::Vector2f &v1, const matrix::Vector2f &v2); +inline int orient2d(int32_t ax, int32_t ay, + int32_t bx, int32_t by, + int32_t cx, int32_t cy) +{ + const int64_t det = (static_cast(bx) - ax) * (static_cast(cy) - ay) + - (static_cast(by) - ay) * (static_cast(cx) - ax); + + if (det > 0) { return 1; } + + if (det < 0) { return -1; } + + return 0; +} /** - * Check if a line segment intersects a circle. - * Works in local Cartesian coordinates (meters). - * - * @param start segment start in local frame - * @param end segment end in local frame - * @param center circle center in local frame - * @param radius circle radius in meters - * @return true if the segment intersects the circle + * For a, b, c known to be collinear (orient2d == 0), is c on the closed + * segment ab? */ -bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end, - const matrix::Vector2f ¢er, float radius); +inline bool collinearBetween(int32_t ax, int32_t ay, int32_t bx, int32_t by, int32_t cx, int32_t cy) +{ + if (std::abs(ax - bx) >= std::abs(ay - by)) { + return (ax <= cx && cx <= bx) || (bx <= cx && cx <= ax); + + } else { + return (ay <= cy && cy <= by) || (by <= cy && cy <= ay); + } +} + +/** + * Classification of the relative position of two segments ab and cd. + * + * Cross proper interior crossing (each segment strictly straddles + * the other's supporting line) + * AInsideCD a strictly between c and d (open interior of segment cd) + * BInsideCD b strictly between c and d + * CInsideAB c strictly between a and b + * DInsideAB d strictly between a and b + * Collinear ab and cd lie on the same supporting line (overlap or not) + * Disjoint none of the above + * + * Reference: O'Rourke, "Computational Geometry in C" (2nd ed.), SegSegInt, + * section 1.5. + */ +enum class SegSegResult { + Disjoint, + Cross, + AInsideCD, BInsideCD, + CInsideAB, DInsideAB, + Collinear, +}; + +inline SegSegResult segmentsIntersect(int32_t ax, int32_t ay, int32_t bx, int32_t by, + int32_t cx, int32_t cy, int32_t dx, int32_t dy) +{ + const int o1 = orient2d(ax, ay, bx, by, cx, cy); + const int o2 = orient2d(ax, ay, bx, by, dx, dy); + const int o3 = orient2d(cx, cy, dx, dy, ax, ay); + const int o4 = orient2d(cx, cy, dx, dy, bx, by); + + if (o1 && o2 && o3 && o4 && o1 != o2 && o3 != o4) { return SegSegResult::Cross; } + + if (!o1 && !o2 && !o3 && !o4) { return SegSegResult::Collinear; } + + // Endpoint strictly on the open interior of the other segment: orient2d + // is zero (point on the supporting line), the point lies between the + // other two via collinearBetween, and is not coincident with either. + if (o3 == 0 && collinearBetween(cx, cy, dx, dy, ax, ay) + && !(ax == cx && ay == cy) && !(ax == dx && ay == dy)) { return SegSegResult::AInsideCD; } + + if (o4 == 0 && collinearBetween(cx, cy, dx, dy, bx, by) + && !(bx == cx && by == cy) && !(bx == dx && by == dy)) { return SegSegResult::BInsideCD; } + + if (o1 == 0 && collinearBetween(ax, ay, bx, by, cx, cy) + && !(cx == ax && cy == ay) && !(cx == bx && cy == by)) { return SegSegResult::CInsideAB; } + + if (o2 == 0 && collinearBetween(ax, ay, bx, by, dx, dy) + && !(dx == ax && dy == ay) && !(dx == bx && dy == by)) { return SegSegResult::DInsideAB; } + + return SegSegResult::Disjoint; +} /** * Check if a polygon's vertices are ordered counter-clockwise using the shoelace formula. @@ -128,17 +192,106 @@ bool isPolygonCCW(const matrix::Vector2f *vertices, int num_vertices); * * Returns false for degenerate polygons: fewer than 3 vertices, zero-length edges, * or antiparallel adjacent edges (polygon doubles back). - * - * @param vertices_in input vertices in local frame - * @param num_vertices number of vertices - * @param margin offset distance in meters - * @param expand true to expand, false to shrink - * @param vertices_out output array (must hold at least num_vertices elements) */ bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices, float margin, bool expand, matrix::Vector2f *vertices_out); +/** + * Read-only fence cache. Owns a flat int32-cm node buffer that holds every + * position the planner cares about (polygon vertices, circle-approximation + * vertices, destination, ...) and a polygon metadata table that slices into + * it. Polygons are stored in canonical orientation: walking vertices in + * stored order, the polygon's INSIDE region is always to the left of each + * edge (CCW for exclusion zones, CW for inclusion zones). + * + * Three layers of intersection query: + * (1) Free int32 predicates (orient2d, segmentsIntersect, collinearBetween) + * at the top of this header -- raw scalar coordinates, no cm semantics. + * (2) `isLineBetweenNodesIntersectingAnyInside(a, b)` -- node-to-node + * visibility for the planner's V^2 hot path; returns false if a == b. + * (3) `isLineFromPointToNodeIntersectingAnyInside(p, idx)` -- the only + * float bridge, intended for the vehicle's current position. + */ +class PlannerPolygons +{ +public: + static constexpr int kMaxNodes = 100; + static constexpr int kMaxPolygons = 16; + + PlannerPolygons() = default; + + void reset() { _num_nodes = 0; _num_polygons = 0; } + + /// Append a single point to the node buffer. Returns its index, or -1 if full. + int addNode(const matrix::Vector2f &p); + + /// Replace the node at `idx` (used to update the destination slot in place). + void setNode(int idx, const matrix::Vector2f &p); + + /// Append a polygon: canonicalize orientation, quantize to cm, append + /// vertices as nodes. Returns false on budget overflow or + /// fewer-than-3-vertex input. + bool addPolygon(const matrix::Vector2f *vertices_in, int num_vertices, bool is_inclusion_zone); + + int numNodes() const { return _num_nodes; } + + matrix::Vector2f node(int idx) const + { + return matrix::Vector2f{_x_cm[idx] / 100.f, _y_cm[idx] / 100.f}; + } + + bool isLineBetweenNodesIntersectingAnyInside(int a, int b) const; + bool isLineFromPointToNodeIntersectingAnyInside(const matrix::Vector2f &p, int node_idx) const; + +private: + struct PolygonInfo { + int start_index; + int num_vertices; + bool inside_is_bounded; + }; + + int32_t _x_cm[kMaxNodes]; + int32_t _y_cm[kMaxNodes]; + int _num_nodes{0}; + + PolygonInfo _polygons[kMaxPolygons]; + int _num_polygons{0}; + + bool intersectsAnyInside(int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const; + bool intersectsInsideOf(const PolygonInfo &poly, + int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const; + bool pointInsideInteriorCone(const PolygonInfo &poly, int32_t px, int32_t py, int v) const; +}; + +/// Convenience for unit tests and one-shot callers: build a transient +/// PlannerPolygons with the given polygon and two extra nodes for the segment +/// endpoints, then run the node-to-node query. +inline bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::Vector2f &end, + const matrix::Vector2f *vertices, int num_vertices, bool is_inclusion_zone) +{ + PlannerPolygons polys; + + if (!polys.addPolygon(vertices, num_vertices, is_inclusion_zone)) { + return false; + } + + return polys.isLineBetweenNodesIntersectingAnyInside(polys.addNode(start), polys.addNode(end)); +} + +/** + * Check if a line segment intersects a circle. + * Works in local Cartesian coordinates (meters). + * + * @param start segment start in local frame + * @param end segment end in local frame + * @param center circle center in local frame + * @param radius circle radius in meters + * @return true if the segment intersects the circle + */ +bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end, + const matrix::Vector2f ¢er, float radius); + /** * Map the upper triangular matrix WITHOUT the diagonal into a flat array. Caller must ensure i != j. * diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp index fd3765691d..4c0ca0c426 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp @@ -49,17 +49,15 @@ public: { MapProjection ref{reference(0), reference(1)}; - for (int i = 0; i < _num_vertices; i++) { - matrix::Vector2f v1_local = ref.project(_vertices[i](0), _vertices[i](1)); - matrix::Vector2f v2_local = ref.project(_vertices[(i + 1) % _num_vertices](0), - _vertices[(i + 1) % _num_vertices](1)); + matrix::Vector2f vertices_local[_num_vertices]; - if (geofence_utils::segmentsIntersect(start_local, end_local, v1_local, v2_local)) { - return true; - } + for (int i = 0; i < _num_vertices; i++) { + vertices_local[i] = ref.project(_vertices[i](0), _vertices[i](1)); } - return false; + const bool is_inclusion_zone = (_fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); + return geofence_utils::lineSegmentIntersectsPolygon(start_local, end_local, + vertices_local, _num_vertices, is_inclusion_zone); } PolygonInfo getPolygonInfoByIndex(int index) override diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp index dd1b6559b8..483b1d82ee 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp @@ -107,7 +107,7 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa return false; } - update_distances_between_vertices(geofence); + update_distances_between_vertices(); // invalidate destination to force a refresh on the next update_destination() _destination_healthy = false; @@ -123,7 +123,8 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( { const int num_polygons = geofence.getNumPolygons(); - int node_index = 0; + _polygons.reset(); + _num_circles = 0; for (int poly_index = 0; poly_index < num_polygons; poly_index++) { @@ -131,14 +132,24 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + const bool is_inclusion = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); + matrix::Vector2f local_in[info.vertex_count]; - matrix::Vector2f local_out[info.vertex_count]; for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) { local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, _reference); } - const bool should_expand = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION); + // Cached polygon for line-violation queries: stores the ACTUAL + // fence boundary (no margin). Graph-node positions below get + // margin-offset separately so paths between them legitimately + // run through the safety band without being flagged as violations. + if (!_polygons.addPolygon(local_in, info.vertex_count, is_inclusion)) { + return false; + } + + matrix::Vector2f local_out[info.vertex_count]; + const bool should_expand = !is_inclusion; if (!geofence_utils::expandOrShrinkPolygon(local_in, info.vertex_count, margin, should_expand, local_out)) { @@ -146,14 +157,21 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( } for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) { - _positions[node_index] = local_out[vertex_idx]; - node_index++; + if (_polygons.addNode(local_out[vertex_idx]) < 0) { return false; } } } else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { - const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, _reference); - const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices; + if (_num_circles >= kMaxCircles) { + return false; + } + const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, _reference); + _circles[_num_circles++] = {center, info.circle_radius}; + + // graph nodes for the circle: polygon approximation at the + // margin-adjusted radius. The line-violation check uses the + // actual circle (radius above), not this approximation. + const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices; float adjusted_radius; if (info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { @@ -169,36 +187,69 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination( for (int i = 0; i < kCircleApproxVertices; i++) { const float angle = i * delta_angle; - _positions[node_index] = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)}; - node_index++; + const matrix::Vector2f p = center + matrix::Vector2f{ + adjusted_radius * cosf(angle), adjusted_radius * sinf(angle) + }; + + if (_polygons.addNode(p) < 0) { return false; } } } } - // node_index equals the polygon vertex count; the destination is reserved one slot beyond - _num_vertices = node_index; - _num_nodes = node_index + 1; // +1 for the destination + // destination occupies one extra slot beyond the polygon/circle vertices + _num_vertices = _polygons.numNodes(); + _dest_idx = _polygons.addNode({0.f, 0.f}); + if (_dest_idx < 0) { return false; } + + _num_nodes = _polygons.numNodes(); return true; } -void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterface &geofence) +bool GeofenceAvoidancePlanner::lineViolatesAnyCachedFenceBetweenNodes(int a, int b) const +{ + if (_polygons.isLineBetweenNodesIntersectingAnyInside(a, b)) { return true; } + + const matrix::Vector2f start = _polygons.node(a); + const matrix::Vector2f end = _polygons.node(b); + + for (int i = 0; i < _num_circles; i++) { + if (geofence_utils::lineSegmentIntersectsCircle(start, end, _circles[i].center, _circles[i].radius)) { + return true; + } + } + + return false; +} + +bool GeofenceAvoidancePlanner::lineViolatesAnyCachedFenceFromPoint(const matrix::Vector2f &p, int node_idx) const +{ + if (_polygons.isLineFromPointToNodeIntersectingAnyInside(p, node_idx)) { return true; } + + const matrix::Vector2f end = _polygons.node(node_idx); + + for (int i = 0; i < _num_circles; i++) { + if (geofence_utils::lineSegmentIntersectsCircle(p, end, _circles[i].center, _circles[i].radius)) { + return true; + } + } + + return false; +} + +void GeofenceAvoidancePlanner::update_distances_between_vertices() { perf_begin(_setup_distances_perf); - // loop through all possible vertex to vertex combinations and store the distance between them for (int i = 0; i < _num_vertices; i++) { for (int j = i + 1; j < _num_vertices; j++) { const size_t idx = geofence_utils::symmetricPairIndex(i, j, _num_nodes); - const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[i], - _positions[j], _reference); - - if (clear) { - _distances[idx] = (_positions[i] - _positions[j]).norm(); + if (lineViolatesAnyCachedFenceBetweenNodes(i, j)) { + _distances[idx] = INFINITY; } else { - _distances[idx] = INFINITY; + _distances[idx] = (_polygons.node(i) - _polygons.node(j)).norm(); } } } @@ -217,28 +268,21 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin matrix::Vector2f dest_local; ref.project(destination(0), destination(1), dest_local(0), dest_local(1)); - const int dest_idx = _num_nodes - 1; - - if (_destination_healthy && (dest_local - _positions[dest_idx]).norm() < FLT_EPSILON) { - // no change in destination, skip the update + if (_destination_healthy && (dest_local - _polygons.node(_dest_idx)).norm() < FLT_EPSILON) { return; } perf_begin(_update_destination_perf); - _positions[dest_idx] = dest_local; + _polygons.setNode(_dest_idx, dest_local); - // distances from each vertex to destination for (int graph_idx = 0; graph_idx < _num_vertices; graph_idx++) { - const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, dest_idx, _num_nodes); + const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, _dest_idx, _num_nodes); - const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[graph_idx], - _positions[dest_idx], _reference); - - if (clear) { - _distances[dist_idx] = (_positions[graph_idx] - _positions[dest_idx]).norm(); + if (lineViolatesAnyCachedFenceBetweenNodes(graph_idx, _dest_idx)) { + _distances[dist_idx] = INFINITY; } else { - _distances[dist_idx] = INFINITY; + _distances[dist_idx] = (_polygons.node(graph_idx) - _polygons.node(_dest_idx)).norm(); } } @@ -275,9 +319,9 @@ int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vec for (int node_idx = 0; node_idx < _num_nodes; node_idx++) { // check if node is reachable from this position - if (!geofence.checkIfLineViolatesAnyFence(_start_local, _positions[node_idx], _reference)) { + if (!lineViolatesAnyCachedFenceFromPoint(_start_local, node_idx)) { - if (node_idx == _num_nodes - 1) { + if (node_idx == _dest_idx) { // destination is directly reachable from start: no detour planning needed. // If the vehicle is already at the start, no waypoints at all are needed. // Otherwise we still need the anchor (point 0) so the vehicle flies back into the fence first. @@ -286,7 +330,7 @@ int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vec return start_is_current_position ? 0 : 1; } - const float cost = (_start_local - _positions[node_idx]).norm() + _best_distance[node_idx]; + const float cost = (_start_local - _polygons.node(node_idx)).norm() + _best_distance[node_idx]; if (cost < best_cost) { best_cost = cost; @@ -307,7 +351,7 @@ int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vec int next = _next_node_buffer[current_index]; _num_path_points++; - if (next == _num_nodes - 1 || next < 0) { + if (next == _dest_idx || next < 0) { break; } @@ -356,6 +400,7 @@ matrix::Vector2d GeofenceAvoidancePlanner::get_point_at_index(int index) const matrix::Vector2d global; MapProjection ref{_reference(0), _reference(1)}; - ref.reproject(_positions[next](0), _positions[next](1), global(0), global(1)); + const matrix::Vector2f p = _polygons.node(next); + ref.reproject(p(0), p(1), global(0), global(1)); return global; } diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h index 8a66d0025e..c99f00465a 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h @@ -42,9 +42,11 @@ #include #include #include +#include #include "geofence_interface.h" static constexpr int kMaxNodes = 100; +static constexpr int kMaxCircles = 16; static constexpr int kCircleApproxVertices = 8; struct PlannedPath { @@ -148,11 +150,26 @@ private: matrix::Vector2f _start_local; - matrix::Vector2f _positions[kMaxNodes]; int _num_nodes{0}; int _num_vertices{0}; + int _dest_idx{-1}; matrix::Vector2 _reference; // lat/lon anchor of the local frame + // Cached, read-only fence representation. _polygons owns the master + // node buffer (polygon vertices + circle approximation vertices + + // destination); _circles holds the analytical circle metadata for the + // line-vs-circle violation check. Built once per `update_vertices` + // (= geofence geometry change); reused by every line-violates-fence + // query during distance setup, destination updates, and start-anchor + // selection. + struct CachedCircle { + matrix::Vector2f center; + float radius; + }; + geofence_utils::PlannerPolygons _polygons; + CachedCircle _circles[kMaxCircles]; + int _num_circles{0}; + bool _polygons_healthy{false}; bool _destination_healthy{false}; bool _start_is_current_position{true}; @@ -163,9 +180,14 @@ private: perf_counter_t _plan_path_perf{perf_alloc(PC_ELAPSED, "rtl_planner: plan path")}; bool update_graph_nodes_without_start_and_destination(GeofenceInterface &geofence, float margin); - void update_distances_between_vertices(GeofenceInterface &geofence); + void update_distances_between_vertices(); void planPath(); + // Read-only query against the cached fence: union of polygon (index-based) + // and circle (analytic, float) checks. No geofence dataman roundtrips. + bool lineViolatesAnyCachedFenceBetweenNodes(int a, int b) const; + bool lineViolatesAnyCachedFenceFromPoint(const matrix::Vector2f &p, int node_idx) const; + bool lat_lon_within_bounds(const matrix::Vector2 &lat_lon); }; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 487125aad1..8ba78daca3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -86,9 +86,7 @@ Geofence::Geofence(Navigator *navigator) : Geofence::~Geofence() { - if (_polygons) { - delete[](_polygons); - } + delete[] _polygons; } void Geofence::run() @@ -252,7 +250,7 @@ void Geofence::_updateFence() memcpy(new_polygons, _polygons, sizeof(PolygonInfo) * _num_polygons); } - delete[](_polygons); + delete[] _polygons; _polygons = new_polygons; } else { @@ -275,7 +273,7 @@ void Geofence::_updateFence() } else { polygon.vertex_count = mission_fence_point.vertex_count; - current_seq += mission_fence_point.vertex_count; + current_seq += polygon.vertex_count; } // check if requiremetns for Home location are met @@ -738,35 +736,32 @@ bool Geofence::checkIfLineViolatesAnyFence(const matrix::Vector2f &start_local, if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + matrix::Vector2f vertices_local[info.vertex_count]; + dm_item_t fence_dataman_id{static_cast(_stats.dataman_id)}; + bool load_success = true; + for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) { - mission_fence_point_s vertex_current{}; - mission_fence_point_s vertex_previous{}; + mission_fence_point_s vertex{}; - int prev_idx = vertex_idx == 0 ? info.vertex_count - 1 : vertex_idx - 1; - - dm_item_t fence_dataman_id{static_cast(_stats.dataman_id)}; - bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx, - reinterpret_cast(&vertex_current), - sizeof(mission_fence_point_s)); - - if (!success) { + if (!_dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx, + reinterpret_cast(&vertex), + sizeof(mission_fence_point_s))) { + load_success = false; break; } - success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + prev_idx, - reinterpret_cast(&vertex_previous), - sizeof(mission_fence_point_s)); + vertices_local[vertex_idx] = ref.project(vertex.lat, vertex.lon); + } - if (!success) { - break; - } + if (!load_success) { + continue; + } - matrix::Vector2f vertex_current_local = ref.project(vertex_current.lat, vertex_current.lon); - matrix::Vector2f vertex_previous_local = ref.project(vertex_previous.lat, vertex_previous.lon); + const bool is_inclusion_zone = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION); - if (geofence_utils::segmentsIntersect(start_local, end_local, vertex_current_local, vertex_previous_local)) { - return true; - } + if (geofence_utils::lineSegmentIntersectsPolygon(start_local, end_local, + vertices_local, info.vertex_count, is_inclusion_zone)) { + return true; } } else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {