Code reorganization. Display recording fixes. Search Flight Planner Fixes. Bug Fixes

src/safety/geofence.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+import math
+from utils.helpers import distance_2d
+
+def point_in_polygon(px, py, polygon):
+    n = len(polygon)
+    inside = False
+    j = n - 1
+    for i in range(n):
+        xi, yi = polygon[i]
+        xj, yj = polygon[j]
+        if ((yi > py) != (yj > py)) and (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
+            inside = not inside
+        j = i
+    return inside
+
+def nearest_point_on_polygon(px, py, polygon):
+    min_dist = float('inf')
+    nearest = (px, py)
+    n = len(polygon)
+    for i in range(n):
+        x1, y1 = polygon[i]
+        x2, y2 = polygon[(i + 1) % n]
+        dx, dy = x2 - x1, y2 - y1
+        length_sq = dx * dx + dy * dy
+        if length_sq == 0:
+            proj = (x1, y1)
+        else:
+            t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
+            proj = (x1 + t * dx, y1 + t * dy)
+        dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
+        if dist < min_dist:
+            min_dist = dist
+            nearest = proj
+    return nearest
+
+def distance_to_polygon_edge(px, py, polygon):
+    min_dist = float('inf')
+    n = len(polygon)
+    for i in range(n):
+        x1, y1 = polygon[i]
+        x2, y2 = polygon[(i + 1) % n]
+        dx, dy = x2 - x1, y2 - y1
+        length_sq = dx * dx + dy * dy
+        if length_sq == 0:
+            dist = math.sqrt((px - x1) ** 2 + (py - y1) ** 2)
+        else:
+            t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
+            proj_x = x1 + t * dx
+            proj_y = y1 + t * dy
+            dist = math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)
+        min_dist = min(min_dist, dist)
+    return min_dist
+
+def clip_to_geofence(waypoints, polygon, warn_dist=3.0, stop_on_leave=False):
+    safe_polygon = []
+    if warn_dist > 0:
+        cx = sum(p[0] for p in polygon) / len(polygon)
+        cy = sum(p[1] for p in polygon) / len(polygon)
+        for x, y in polygon:
+            dx = x - cx
+            dy = y - cy
+            length = math.sqrt(dx*dx + dy*dy)
+            if length > 0:
+                shrink = warn_dist / length
+                safe_polygon.append((x - dx * shrink, y - dy * shrink))
+            else:
+                safe_polygon.append((x, y))
+    else:
+        safe_polygon = polygon
+
+    clipped = []
+    consecutive_outside = 0
+    for wx, wy in waypoints:
+        if point_in_polygon(wx, wy, safe_polygon):
+            clipped.append((wx, wy))
+            consecutive_outside = 0
+        else:
+            consecutive_outside += 1
+            if stop_on_leave and consecutive_outside >= 2:
+                break
+            nearest = nearest_point_on_polygon(wx, wy, safe_polygon)
+            if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
+                clipped.append(nearest)
+    return clipped