haptic_skin.geo

 1"""Geographic helpers for navigation: distances, bearings, directional buckets."""
 2
 3from __future__ import annotations
 4
 5import math
 6from typing import Tuple
 7
 8EARTH_RADIUS_M = 6_371_000.0
 9LatLon = Tuple[float, float]
10
11
12def haversine_m(a: LatLon, b: LatLon) -> float:
13    """Great-circle distance between two (lat, lon) points, in metres."""
14    lat1, lon1 = math.radians(a[0]), math.radians(a[1])
15    lat2, lon2 = math.radians(b[0]), math.radians(b[1])
16    dlat, dlon = lat2 - lat1, lon2 - lon1
17    h = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
18    return 2 * EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(h)))
19
20
21def bearing_deg(a: LatLon, b: LatLon) -> float:
22    """Initial compass bearing from a to b, degrees in [0, 360)."""
23    lat1, lat2 = math.radians(a[0]), math.radians(b[0])
24    dlon = math.radians(b[1] - a[1])
25    x = math.sin(dlon) * math.cos(lat2)
26    y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
27    return (math.degrees(math.atan2(x, y)) + 360.0) % 360.0
28
29
30def relative_bearing(target_abs: float, heading: float) -> float:
31    """Signed angle (deg) of ``target_abs`` relative to ``heading``, in (-180, 180].
32
33    0 = straight ahead, +90 = to the right, -90 = to the left, 180 = behind.
34    """
35    diff = (target_abs - heading + 180.0) % 360.0 - 180.0
36    return diff if diff != -180.0 else 180.0
37
38
39def bucket_from_relative(relative: float) -> int:
40    """Map a relative bearing to one of 8 motor indices (0 = front, clockwise)."""
41    return round(relative / 45.0) % 8