refactor(navigator/geofence): move lineSegmentIntersectsPolygon to geofence_utils

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 &center, float radius)
 {

src/lib/geofence/geofence_utils.h

@@ -96,6 +96,19 @@ bool insideCircle(const matrix::Vector2<double> &center, 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).

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 vertices_local[_num_vertices];
-			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));
 
-			if (geofence_utils::segmentsIntersect(start_local, end_local, v1_local, v2_local)) {
+		for (int i = 0; i < _num_vertices; i++) {
-				return true;
+			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

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<dm_item_t>(_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{};
-				mission_fence_point_s vertex_previous{};
 
-				int prev_idx = vertex_idx == 0 ? info.vertex_count - 1 : vertex_idx - 1;
+				if (!_dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx,
-
+							     reinterpret_cast<uint8_t *>(&vertex),
-				dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
+							     sizeof(mission_fence_point_s))) {
-				bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx,
+					load_success = false;
-								       reinterpret_cast<uint8_t *>(&vertex_current),
-								       sizeof(mission_fence_point_s));
-
-				if (!success) {
 					break;
 				}
 
-				success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + prev_idx,
+				vertices_local[vertex_idx] = ref.project(vertex.lat, vertex.lon);
-								  reinterpret_cast<uint8_t *>(&vertex_previous),
+			}
-								  sizeof(mission_fence_point_s));
 
-				if (!success) {
+			if (!load_success) {
-					break;
+				continue;
-				}
+			}
 
-				matrix::Vector2f vertex_current_local = ref.project(vertex_current.lat, vertex_current.lon);
+			if (geofence_utils::lineSegmentIntersectsPolygon(
-				matrix::Vector2f vertex_previous_local = ref.project(vertex_previous.lat, vertex_previous.lon);
+				    start_local, end_local, vertices_local, info.vertex_count)) {
-
+				return true;
-				if (geofence_utils::segmentsIntersect(start_local, end_local, vertex_current_local, vertex_previous_local)) {
-					return true;
-				}
 			}
 
 		} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {