Johnny Fernandes
43 lines
1.3 KiB
Python
43 lines
1.3 KiB
Python
"""Shared action post-processing.
|
|
|
|
Every dog mode routes its action through ``modulate_speed_near_sheep``
|
|
so the magnitude is reduced near sheep — direction (intent) is
|
|
preserved.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
|
|
SLOW_NEAR_SHEEP = 2.5 # m — distance below which action norm is scaled down
|
|
MIN_SPEED = 0.30 # action norm at zero distance
|
|
|
|
|
|
def modulate_speed_near_sheep(
|
|
vx: float, vy: float,
|
|
dog_xy: tuple[float, float],
|
|
sheep_positions,
|
|
slow_dist: float = SLOW_NEAR_SHEEP,
|
|
min_scale: float = MIN_SPEED,
|
|
) -> tuple[float, float]:
|
|
"""Linearly ramp action magnitude from ``min_scale`` at distance 0
|
|
to 1.0 at ``slow_dist``. ``sheep_positions`` may be a
|
|
``{name: (x, y)}`` dict or an iterable of ``(x, y)`` tuples.
|
|
"""
|
|
if not sheep_positions:
|
|
return vx, vy
|
|
if hasattr(sheep_positions, "values"):
|
|
positions = sheep_positions.values()
|
|
else:
|
|
positions = sheep_positions
|
|
nearest = float("inf")
|
|
for sx, sy in positions:
|
|
d = math.hypot(sx - dog_xy[0], sy - dog_xy[1])
|
|
if d < nearest:
|
|
nearest = d
|
|
if nearest >= slow_dist or nearest == float("inf"):
|
|
return vx, vy
|
|
scale = min_scale + (1.0 - min_scale) * (nearest / slow_dist)
|
|
return vx * scale, vy * scale
|