From 10bc335b72283711ffdc1a347544b324cf41df52 Mon Sep 17 00:00:00 2001 From: Balduin Date: Thu, 7 May 2026 16:04:06 +0200 Subject: [PATCH] refactor(navigator/geofence): move lineSegmentIntersectsPolygon to geofence_utils --- src/lib/geofence/geofence_utils.cpp | 14 +++++++ src/lib/geofence/geofence_utils.h | 13 +++++++ .../GeofenceAvoidancePlannerTest.cpp | 13 +++---- src/modules/navigator/geofence.cpp | 39 ++++++++----------- 4 files changed, 49 insertions(+), 30 deletions(-) diff --git a/src/lib/geofence/geofence_utils.cpp b/src/lib/geofence/geofence_utils.cpp index 0594341afc..ffa77338dd 100644 --- a/src/lib/geofence/geofence_utils.cpp +++ b/src/lib/geofence/geofence_utils.cpp @@ -86,6 +86,20 @@ bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2, return t > 0.0f && t < 1.0f && u > 0.0f && u < 1.0f; } +bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::Vector2f &end, + const matrix::Vector2f *vertices, int num_vertices) +{ + for (int vertex_idx = 0; vertex_idx < num_vertices; vertex_idx++) { + int prev_idx = vertex_idx == 0 ? num_vertices - 1 : vertex_idx - 1; + + if (segmentsIntersect(start, end, vertices[vertex_idx], vertices[prev_idx])) { + return true; + } + } + + return false; +} + bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end, const matrix::Vector2f ¢er, float radius) { diff --git a/src/lib/geofence/geofence_utils.h b/src/lib/geofence/geofence_utils.h index c0f5b746d2..6a490d5eda 100644 --- a/src/lib/geofence/geofence_utils.h +++ b/src/lib/geofence/geofence_utils.h @@ -96,6 +96,19 @@ bool insideCircle(const matrix::Vector2 ¢er, float radius, bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2, const matrix::Vector2f &v1, const matrix::Vector2f &v2); +/** + * Check if a line segment intersects any edge of a polygon (excluding endpoints). + * Works in local Cartesian coordinates (meters). + * + * @param start segment start in local frame + * @param end segment end in local frame + * @param vertices polygon vertices in local frame + * @param num_vertices number of vertices + * @return true if the segment intersects any polygon edge + */ +bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::Vector2f &end, + const matrix::Vector2f *vertices, int num_vertices); + /** * Check if a line segment intersects a circle. * Works in local Cartesian coordinates (meters). diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp index fd3765691d..6a165b1837 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceAvoidancePlannerTest.cpp @@ -49,17 +49,14 @@ 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; + return geofence_utils::lineSegmentIntersectsPolygon(start_local, end_local, + vertices_local, _num_vertices); } PolygonInfo getPolygonInfoByIndex(int index) override diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 487125aad1..f1a5e15204 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -738,35 +738,30 @@ 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); - - 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)) { + return true; } } else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {