87 lines
2.7 KiB
Python
87 lines
2.7 KiB
Python
#!/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
|