60 lines
1.8 KiB
Python
60 lines
1.8 KiB
Python
from dataclasses import dataclass
|
|
|
|
from edge.tracking import Track
|
|
from edge.models import L_ANKLE, R_ANKLE
|
|
|
|
|
|
def point_in_polygon(x: float, y: float, poly: list[tuple[float, float]]) -> bool:
|
|
inside = False
|
|
n = len(poly)
|
|
j = n - 1
|
|
for i in range(n):
|
|
xi, yi = poly[i]
|
|
xj, yj = poly[j]
|
|
if ((yi > y) != (yj > y)) and (x < (xj - xi) * (y - yi) / ((yj - yi) or 1e-9) + xi):
|
|
inside = not inside
|
|
j = i
|
|
return inside
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Zone:
|
|
zone_id: str
|
|
level: str # "warning" | "restricted"
|
|
polygon: list # list[tuple[float,float]]
|
|
|
|
|
|
@dataclass
|
|
class ZoneConfig:
|
|
debounce_s: float = 0.75
|
|
|
|
|
|
class ZoneIntrusionDetector:
|
|
def __init__(self, zones: list[Zone], cfg: ZoneConfig):
|
|
self.zones = zones
|
|
self.cfg = cfg
|
|
self._inside_since: dict[tuple[int, str], float] = {}
|
|
self._fired: set[tuple[int, str]] = set()
|
|
|
|
def _foot(self, track: Track) -> tuple[float, float]:
|
|
kps = track.latest.keypoints
|
|
a, b = kps[L_ANKLE], kps[R_ANKLE]
|
|
return ((a.x + b.x) / 2, (a.y + b.y) / 2)
|
|
|
|
def active_zone_ids(self, track_id: int) -> set:
|
|
return {z for (tid, z) in self._inside_since if tid == track_id}
|
|
|
|
def update(self, track: Track, now: float) -> list[dict]:
|
|
fx, fy = self._foot(track)
|
|
out = []
|
|
for zone in self.zones:
|
|
key = (track.track_id, zone.zone_id)
|
|
if point_in_polygon(fx, fy, zone.polygon):
|
|
start = self._inside_since.setdefault(key, now)
|
|
if now - start >= self.cfg.debounce_s and key not in self._fired:
|
|
self._fired.add(key)
|
|
out.append({"zone_id": zone.zone_id, "level": zone.level})
|
|
else:
|
|
self._inside_since.pop(key, None)
|
|
self._fired.discard(key)
|
|
return out
|