colregs-planner 0.1.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (53) hide show
  1. colregs_planner/__init__.py +22 -0
  2. colregs_planner/ais/__init__.py +0 -0
  3. colregs_planner/ais/cleaning.py +166 -0
  4. colregs_planner/ais/windows.py +52 -0
  5. colregs_planner/colregs_graph/__init__.py +0 -0
  6. colregs_planner/colregs_graph/colregs_classifier.py +418 -0
  7. colregs_planner/colregs_graph/colregs_graph.py +115 -0
  8. colregs_planner/motion_models/__init__.py +1 -0
  9. colregs_planner/motion_models/adapters.py +386 -0
  10. colregs_planner/motion_models/ct_model.py +191 -0
  11. colregs_planner/motion_models/milliampere_model.py +344 -0
  12. colregs_planner/motion_models/vikenes_model.py +404 -0
  13. colregs_planner/nominal/__init__.py +19 -0
  14. colregs_planner/nominal/indexer.py +166 -0
  15. colregs_planner/nominal/library.py +144 -0
  16. colregs_planner/nominal/retriever.py +216 -0
  17. colregs_planner/planner_api/__init__.py +40 -0
  18. colregs_planner/planner_api/dp_planner.py +320 -0
  19. colregs_planner/planner_api/manoeuvre_search.py +519 -0
  20. colregs_planner/planner_api/manoeuvres.py +48 -0
  21. colregs_planner/planner_api/schema.py +208 -0
  22. colregs_planner/planner_api/service.py +687 -0
  23. colregs_planner/planner_api/tools.py +218 -0
  24. colregs_planner/policies/__init__.py +1 -0
  25. colregs_planner/policies/constant.py +70 -0
  26. colregs_planner/policies/cto.py +452 -0
  27. colregs_planner/policies/debug_types.py +79 -0
  28. colregs_planner/policies/predictor_dp.py +52 -0
  29. colregs_planner/policies/search_dp.py +93 -0
  30. colregs_planner/policies/vo_analytic.py +777 -0
  31. colregs_planner/policies/vo_common.py +387 -0
  32. colregs_planner/policies/vo_kuwata.py +553 -0
  33. colregs_planner/prediction/__init__.py +1 -0
  34. colregs_planner/prediction/parallel.py +203 -0
  35. colregs_planner/prediction/predictor.py +693 -0
  36. colregs_planner/prediction/responding_filter.py +217 -0
  37. colregs_planner/prediction/vo_rollout.py +621 -0
  38. colregs_planner/scenario_config.py +165 -0
  39. colregs_planner/seachart/__init__.py +0 -0
  40. colregs_planner/seachart/loader.py +292 -0
  41. colregs_planner/vessel/__init__.py +2 -0
  42. colregs_planner/vessel/base.py +85 -0
  43. colregs_planner/vessel/encounter.py +225 -0
  44. colregs_planner/vessel/state.py +170 -0
  45. colregs_planner/visualization/__init__.py +1 -0
  46. colregs_planner/visualization/live_viz.py +2088 -0
  47. colregs_planner/visualization/map_background.py +131 -0
  48. colregs_planner/visualization/multi_panel.py +540 -0
  49. colregs_planner-0.1.0.dist-info/METADATA +471 -0
  50. colregs_planner-0.1.0.dist-info/RECORD +53 -0
  51. colregs_planner-0.1.0.dist-info/WHEEL +5 -0
  52. colregs_planner-0.1.0.dist-info/licenses/LICENSE +21 -0
  53. colregs_planner-0.1.0.dist-info/top_level.txt +1 -0
@@ -0,0 +1,22 @@
1
+ """colregs_planner: COLREGs-aware multi-vessel trajectory prediction and
2
+ own-ship manoeuvre planning.
3
+
4
+ from colregs_planner import ColregsDPPlanner, Route # DP reference for an own ship
5
+ from colregs_planner import PredictionService # predictions for a planner
6
+ """
7
+
8
+ from importlib.metadata import PackageNotFoundError, version
9
+
10
+ from colregs_planner.planner_api import (
11
+ ColregsDPPlanner,
12
+ DPReference,
13
+ PredictionService,
14
+ Route,
15
+ )
16
+
17
+ try:
18
+ __version__ = version("colregs-planner")
19
+ except PackageNotFoundError: # running from a source tree that is not installed
20
+ __version__ = "0.0.0"
21
+
22
+ __all__ = ["ColregsDPPlanner", "DPReference", "PredictionService", "Route", "__version__"]
File without changes
@@ -0,0 +1,166 @@
1
+ """colregs_planner/ais/cleaning.py
2
+
3
+ Raw Kystverket AIS parquet -> cleaned, uniformly-resampled per-voyage tracks.
4
+ Shared between scripts/process_raw_ais.py (the encounter-extraction pipeline)
5
+ and colregs_planner/nominal/indexer.py (the nominal track-library builder) so both stay
6
+ in sync without one importing the other as a script.
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ import math
12
+ from pathlib import Path
13
+
14
+ import numpy as np
15
+ import pandas as pd
16
+
17
+ _COG_UNAVAILABLE = 360.0
18
+ _HDG_UNAVAILABLE = 511
19
+ _ROT_UNAVAILABLE = -128
20
+ # Default area (lat_min, lat_max, lon_min, lon_max): the Oslofjord study area.
21
+ DEFAULT_BOUNDS: tuple[float, float, float, float] = (59.0, 59.95, 10.2, 10.85)
22
+
23
+
24
+ # ---------------------------------------------------------------------------
25
+ # Load & clean one daily parquet file
26
+ # ---------------------------------------------------------------------------
27
+
28
+ def load_and_clean(
29
+ path: Path,
30
+ bounds: tuple[float, float, float, float] = DEFAULT_BOUNDS,
31
+ ) -> pd.DataFrame:
32
+ """Load one daily parquet file, keeping valid rows inside *bounds*
33
+ (lat_min, lat_max, lon_min, lon_max) [deg]."""
34
+ lat_min, lat_max, lon_min, lon_max = bounds
35
+ df = pd.read_parquet(path)
36
+ df["sog"] = pd.to_numeric(df["speed_over_ground"], errors="coerce")
37
+ df["cog"] = pd.to_numeric(df["course_over_ground"], errors="coerce")
38
+ df.drop(columns=["speed_over_ground", "course_over_ground", "geometry"],
39
+ errors="ignore", inplace=True)
40
+ df.rename(columns={"date_time_utc": "timestamp"}, inplace=True)
41
+ df.loc[df["cog"] >= _COG_UNAVAILABLE, "cog"] = float("nan")
42
+ df.loc[df["true_heading"] == _HDG_UNAVAILABLE, "true_heading"] = float("nan")
43
+ df.loc[df["rate_of_turn"] == _ROT_UNAVAILABLE, "rate_of_turn"] = float("nan")
44
+ if df["timestamp"].dt.tz is not None:
45
+ df["timestamp"] = df["timestamp"].dt.tz_localize(None)
46
+ mmsi = df["mmsi"].astype(int)
47
+ df = df[(mmsi >= 100_000_000) & (mmsi <= 999_999_999)].copy()
48
+ df = df[df["latitude"].between(lat_min, lat_max) &
49
+ df["longitude"].between(lon_min, lon_max)].copy()
50
+ df = df[df["sog"].between(0.0, 50.0)].copy()
51
+ df.sort_values("timestamp", inplace=True)
52
+ df.drop_duplicates(subset=["mmsi", "timestamp"], keep="first", inplace=True)
53
+ return df.reset_index(drop=True)
54
+
55
+
56
+ # ---------------------------------------------------------------------------
57
+ # Circular COG interpolation
58
+ # ---------------------------------------------------------------------------
59
+
60
+ def _interp_circular_deg(t_new: np.ndarray, t_old: np.ndarray,
61
+ angles_deg: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
62
+ cos_v = np.cos(np.radians(angles_deg))
63
+ sin_v = np.sin(np.radians(angles_deg))
64
+ ci = np.interp(t_new, t_old, cos_v)
65
+ si = np.interp(t_new, t_old, sin_v)
66
+ norms = np.hypot(ci, si)
67
+ norms[norms < 1e-9] = 1.0
68
+ return ci / norms, si / norms
69
+
70
+
71
+ def _fill_nan_nearest(vals: np.ndarray) -> np.ndarray:
72
+ """Forward/backward fill NaN using nearest valid value."""
73
+ nans = np.isnan(vals)
74
+ if not nans.any() or nans.all():
75
+ return vals
76
+ idx = np.arange(len(vals))
77
+ valid_idx = idx[~nans]
78
+ fill_pos = np.searchsorted(valid_idx, idx[nans])
79
+ fill_pos = np.clip(fill_pos, 0, len(valid_idx) - 1)
80
+ vals = vals.copy()
81
+ vals[nans] = vals[valid_idx[fill_pos]]
82
+ return vals
83
+
84
+
85
+ # ---------------------------------------------------------------------------
86
+ # Trajectory segmentation and resampling
87
+ # ---------------------------------------------------------------------------
88
+
89
+ def resample_track(
90
+ track: pd.DataFrame,
91
+ dt: float,
92
+ gap_threshold: float,
93
+ min_segment_obs: int,
94
+ ) -> list[pd.DataFrame]:
95
+ track = track.sort_values("timestamp").reset_index(drop=True)
96
+ ts_unix = (track["timestamp"] - pd.Timestamp("1970-01-01")).dt.total_seconds().values
97
+ diffs = np.diff(ts_unix)
98
+ split_at = list(np.where(diffs > gap_threshold)[0] + 1)
99
+ seg_starts = [0] + split_at
100
+ seg_ends = split_at + [len(track)]
101
+ mmsi_val = int(track["mmsi"].iloc[0])
102
+ result: list[pd.DataFrame] = []
103
+
104
+ for s0, s1 in zip(seg_starts, seg_ends):
105
+ seg = track.iloc[s0:s1].reset_index(drop=True)
106
+ if len(seg) < min_segment_obs:
107
+ continue
108
+ ts_old = ts_unix[s0:s1]
109
+ span = ts_old[-1] - ts_old[0]
110
+ if span < dt:
111
+ continue
112
+ t_grid = np.arange(ts_old[0], ts_old[-1] + 1e-9, dt)
113
+ out: dict = {}
114
+ out["timestamp"] = pd.to_datetime(t_grid, unit="s")
115
+ out["mmsi"] = mmsi_val
116
+
117
+ for col in ["latitude", "longitude", "sog"]:
118
+ vals = seg[col].to_numpy(dtype=float, na_value=float("nan"))
119
+ vals = _fill_nan_nearest(vals)
120
+ out[col] = np.interp(t_grid, ts_old, vals)
121
+
122
+ # COG circular interpolation
123
+ cog_vals = seg["cog"].to_numpy(dtype=float, na_value=float("nan"))
124
+ if np.isnan(cog_vals).all():
125
+ cog_vals = np.zeros(len(seg))
126
+ else:
127
+ cog_vals = _fill_nan_nearest(cog_vals)
128
+ ci, si = _interp_circular_deg(t_grid, ts_old, cog_vals)
129
+ out["cos_hdg"] = ci
130
+ out["sin_hdg"] = si
131
+ out["cog"] = np.degrees(np.arctan2(si, ci)) % 360
132
+
133
+ hdg_rad = np.arctan2(si, ci)
134
+ dh = np.diff(hdg_rad)
135
+ dh = (dh + math.pi) % (2 * math.pi) - math.pi
136
+ out["heading_rate"] = np.concatenate([[0.0], dh / dt])
137
+
138
+ if "true_heading" in seg.columns:
139
+ th = seg["true_heading"].to_numpy(dtype=float, na_value=float("nan"))
140
+ if not np.isnan(th).all():
141
+ th = _fill_nan_nearest(th)
142
+ th_ci, th_si = _interp_circular_deg(t_grid, ts_old, th)
143
+ out["true_heading"] = np.degrees(np.arctan2(th_si, th_ci)) % 360
144
+ else:
145
+ out["true_heading"] = np.full(len(t_grid), float("nan"))
146
+
147
+ for col in ["status", "maneuvre", "msg_type"]:
148
+ if col in seg.columns:
149
+ out[col] = np.interp(
150
+ t_grid, ts_old,
151
+ seg[col].to_numpy(dtype=float, na_value=0.0)
152
+ )
153
+ if "rate_of_turn" in seg.columns:
154
+ vals = seg["rate_of_turn"].to_numpy(dtype=float, na_value=float("nan"))
155
+ vals = _fill_nan_nearest(vals) if not np.isnan(vals).all() else np.zeros_like(vals)
156
+ out["rate_of_turn"] = np.interp(t_grid, ts_old, vals)
157
+ for col in ["data_source", "ais_class"]:
158
+ if col in seg.columns:
159
+ out[col] = seg[col].iloc[0]
160
+
161
+ original_ts = set(np.round(ts_old / dt).astype(int))
162
+ grid_r = np.round(t_grid / dt).astype(int)
163
+ out["interpolated"] = ~np.isin(grid_r, list(original_ts))
164
+ result.append(pd.DataFrame(out))
165
+
166
+ return result
@@ -0,0 +1,52 @@
1
+ """colregs_planner/ais/windows.py
2
+
3
+ Helpers for the encounter windows written by scripts/process_raw_ais.py.
4
+ """
5
+
6
+ from __future__ import annotations
7
+
8
+ import math
9
+
10
+ from colregs_planner.vessel.state import VesselState
11
+
12
+ _KNOTS_TO_MS = 0.5144
13
+
14
+
15
+ def window_to_frames(w: dict) -> list[dict[str, VesselState]]:
16
+ """Convert a window to a per-timestep list of {str(mmsi): VesselState}."""
17
+ mmsi_list = sorted(w["vessels"].keys())
18
+ n_steps = len(next(iter(w["vessels"].values())))
19
+ frames: list[dict[str, VesselState]] = []
20
+ for t in range(n_steps):
21
+ frame: dict[str, VesselState] = {}
22
+ for mmsi in mmsi_list:
23
+ df = w["vessels"][mmsi]
24
+ if t >= len(df):
25
+ continue
26
+ row = df.iloc[t]
27
+ frame[str(mmsi)] = VesselState(
28
+ north=float(row["north"]),
29
+ east=float(row["east"]),
30
+ psi=math.atan2(float(row["sin_hdg"]), float(row["cos_hdg"])),
31
+ speed=float(row["sog"]) * _KNOTS_TO_MS,
32
+ r=float(row.get("heading_rate", 0.0)),
33
+ )
34
+ if frame:
35
+ frames.append(frame)
36
+ return frames
37
+
38
+
39
+ def traj_limits(
40
+ frames: list[dict[str, VesselState]], padding: float = 300.0
41
+ ) -> tuple[tuple[float, float], tuple[float, float]]:
42
+ """Square (east, north) plot limits around every position in frames."""
43
+ all_n = [s.north for f in frames for s in f.values()]
44
+ all_e = [s.east for f in frames for s in f.values()]
45
+ n_min, n_max = min(all_n) - padding, max(all_n) + padding
46
+ e_min, e_max = min(all_e) - padding, max(all_e) + padding
47
+ nr, er = n_max - n_min, e_max - e_min
48
+ if nr > er:
49
+ d = (nr - er) / 2; e_min -= d; e_max += d
50
+ else:
51
+ d = (er - nr) / 2; n_min -= d; n_max += d
52
+ return (e_min, e_max), (n_min, n_max)
File without changes
@@ -0,0 +1,418 @@
1
+ """
2
+ colregs_classifier.py
3
+
4
+ Observation-driven COLREGs scenario classifier.
5
+
6
+ Produces a ColregsGraph from a dict of observed VesselStates, using the
7
+ Temporally Aware Geometric COLREG Scenario Classification (TAG-CSC) approach
8
+ of Gleeson et al. (2024), adapted for the symmetric multi-vessel graph
9
+ representation used in this project.
10
+
11
+ Classification pipeline (per unordered pair {a, b}):
12
+ 1. DCPA gate — pairs with DCPA > dcpa_max get no edge
13
+ 2. Geometric lookup — (psi, beta) region table → EncounterType
14
+ 3. Temporal vote — forward-project to CPA, take priority-weighted mode
15
+ 4. Hysteresis — maintain active scenario until vessels are d_u apart
16
+ 5. Role assignment — map EncounterType to (role_a, role_b) via ENCOUNTER_ROLES
17
+
18
+ Coordinate convention: NED, heading in radians CW from North.
19
+
20
+ Usage
21
+ -----
22
+ classifier = ColregsClassifier()
23
+ while True:
24
+ states: dict[int, VesselState] = get_observations()
25
+ graph: ColregsGraph = classifier.update(states)
26
+ """
27
+
28
+ from __future__ import annotations
29
+
30
+ import math
31
+ from bisect import bisect_right
32
+ from dataclasses import dataclass, field
33
+ from typing import Optional
34
+
35
+ from colregs_planner.vessel.state import VesselState
36
+ from colregs_planner.vessel.encounter import EncounterType
37
+ from colregs_planner.colregs_graph.colregs_graph import ColregsGraph, ColregsEdge, ENCOUNTER_ROLES
38
+
39
+
40
+ # ---------------------------------------------------------------------------
41
+ # Geometric region table (Gleeson et al. 2024, Table 1)
42
+ #
43
+ # Regions are defined in terms of:
44
+ # psi = theta_TS - theta_OS (relative course, degrees)
45
+ # beta = bearing_to_TS - theta_OS (relative bearing, degrees)
46
+ #
47
+ # Each entry is a list of (psi_min, psi_max, beta_min, beta_max) rectangles.
48
+ # Membership: psi_min <= psi < psi_max AND beta_min <= beta < beta_max.
49
+ # The scenario label is always from vessel_a's perspective (a = OS, b = TS).
50
+ # ---------------------------------------------------------------------------
51
+
52
+ _REGIONS: dict[EncounterType, list[tuple[float, float, float, float]]] = {
53
+ EncounterType.HEAD_ON: [
54
+ ( 135, 180, -45, 45),
55
+ (-180, -135, -45, 45),
56
+ ],
57
+ EncounterType.CROSSING_STARBOARD: [ # vessel_a gives way
58
+ (-180, -64, 45, 180),
59
+ ( -64, 0, 67.5, 112.5),
60
+ (-135, -64, -30, 45),
61
+ ],
62
+ EncounterType.CROSSING_PORT: [ # vessel_a stands on
63
+ ( 66, 180, -180, -45),
64
+ ( 0, 66, -112.5, -67.5),
65
+ ( 66, 135, -45, 30),
66
+ ],
67
+ EncounterType.OVERTAKING_AHEAD: [ # vessel_a is overtaking (gives way)
68
+ (-65, 66, -67.5, 67.5),
69
+ ],
70
+ EncounterType.OVERTAKING_ASTERN: [ # vessel_a is being overtaken (stands on)
71
+ (-65, 66, -180, -112.5),
72
+ (-65, 66, 112.5, 180),
73
+ ],
74
+ }
75
+
76
+ # Priority for tie-breaking in temporal vote (lower = higher priority).
77
+ # Give-way scenarios are higher priority: failing to give way is more dangerous.
78
+ # (Gleeson et al. 2024, Table 2)
79
+ _PRIORITY: dict[EncounterType, int] = {
80
+ EncounterType.CROSSING_STARBOARD: 1,
81
+ EncounterType.OVERTAKING_AHEAD: 2,
82
+ EncounterType.HEAD_ON: 3,
83
+ EncounterType.CROSSING_PORT: 4,
84
+ EncounterType.OVERTAKING_ASTERN: 5,
85
+ }
86
+
87
+
88
+ # ---------------------------------------------------------------------------
89
+ # Region lookup table
90
+ #
91
+ # Every rectangle edge in _REGIONS is a breakpoint, so the (psi, beta) plane
92
+ # splits into a grid of cells that each lie entirely inside or outside every
93
+ # region. bisect_right on the sorted breakpoints gives the cell index exactly
94
+ # (half-open [min, max) matches the region membership rule), and the table
95
+ # holds the first-matching scenario per cell, preserving _REGIONS order as the
96
+ # tie-break where rectangles overlap. Index 0 is the cell below all edges.
97
+ # ---------------------------------------------------------------------------
98
+
99
+ _PSI_EDGES: list[float] = sorted({e for rs in _REGIONS.values() for r in rs for e in r[:2]})
100
+ _BETA_EDGES: list[float] = sorted({e for rs in _REGIONS.values() for r in rs for e in r[2:]})
101
+
102
+
103
+ def _region_lookup(psi_deg: float, beta_deg: float) -> Optional[EncounterType]:
104
+ """First scenario in _REGIONS whose rectangles contain (psi, beta), or None."""
105
+ for scenario, rects in _REGIONS.items():
106
+ for psi_min, psi_max, beta_min, beta_max in rects:
107
+ if psi_min <= psi_deg < psi_max and beta_min <= beta_deg < beta_max:
108
+ return scenario
109
+ return None
110
+
111
+
112
+ def _build_table() -> list[list[Optional[EncounterType]]]:
113
+ # Each cell is sampled at its lower-left corner, which lies inside the
114
+ # cell; the out-of-range cells (index 0) map to None.
115
+ return [
116
+ [_region_lookup(p, b) if i and j else None
117
+ for j, b in enumerate([-math.inf] + _BETA_EDGES)]
118
+ for i, p in enumerate([-math.inf] + _PSI_EDGES)
119
+ ]
120
+
121
+
122
+ _TABLE = _build_table()
123
+
124
+
125
+ def _geometric_classify(psi_deg: float, beta_deg: float) -> Optional[EncounterType]:
126
+ """Return EncounterType from vessel_a's perspective, or None."""
127
+ return _TABLE[bisect_right(_PSI_EDGES, psi_deg)][bisect_right(_BETA_EDGES, beta_deg)]
128
+
129
+
130
+ # ---------------------------------------------------------------------------
131
+ # Geometry helpers (NED, radians CW from North)
132
+ # ---------------------------------------------------------------------------
133
+
134
+ _TWO_PI = 2 * math.pi
135
+ _RAD2DEG = 180.0 / math.pi
136
+
137
+
138
+ def _wrap_deg(angle: float) -> float:
139
+ """Wrap angle [rad] to [-pi, pi) and convert to degrees."""
140
+ return ((angle + math.pi) % _TWO_PI - math.pi) * _RAD2DEG
141
+
142
+
143
+ def _compute_dcpa_tcpa(a: VesselState, b: VesselState) -> tuple[float, float]:
144
+ """
145
+ Closest Point of Approach distance [m] and time [s].
146
+
147
+ tcpa < 0 means vessels are already diverging.
148
+ """
149
+ dn = b.north - a.north
150
+ de = b.east - a.east
151
+ dvn = float(b.vn - a.vn)
152
+ dve = float(b.ve - a.ve)
153
+
154
+ speed_sq = dvn * dvn + dve * dve
155
+ if speed_sq < 1e-6:
156
+ return math.hypot(dn, de), math.inf
157
+
158
+ tcpa = -(dn * dvn + de * dve) / speed_sq
159
+ dcpa = math.hypot(dn + tcpa * dvn, de + tcpa * dve)
160
+ return dcpa, tcpa
161
+
162
+
163
+ # ---------------------------------------------------------------------------
164
+ # Per-pair hysteresis state
165
+ # ---------------------------------------------------------------------------
166
+
167
+ @dataclass
168
+ class _PairState:
169
+ active_scenario: Optional[EncounterType] = None
170
+ hysteresis_score: float = 0.0 # in [0, 1]; scenario switches when it hits 0
171
+ clear_count: int = 0 # consecutive diverging steps; clears when >= n_clear
172
+
173
+
174
+ # ---------------------------------------------------------------------------
175
+ # Classifier
176
+ # ---------------------------------------------------------------------------
177
+
178
+ @dataclass
179
+ class ColregsClassifier:
180
+ """
181
+ Stateful COLREGs classifier. Call update() once per timestep.
182
+
183
+ Parameters
184
+ ----------
185
+ dcpa_max : float
186
+ DCPA gate threshold [m]. Pairs with DCPA > dcpa_max get no edge.
187
+ d_a : float
188
+ Maximum range at which a new scenario can be detected [m].
189
+ d_u : float
190
+ Range below which an active scenario is discarded once vessels
191
+ are separating [m].
192
+ n_t : int
193
+ Number of time steps for temporal projection (Gleeson §2.1.2).
194
+ w_reinforce : float
195
+ Hysteresis weight added per timestep when scenario is stable.
196
+ w_forget : float
197
+ Hysteresis weight subtracted per timestep when scenario changes.
198
+ Default 0.0 gives latching behaviour (scenario never forgotten
199
+ until distance check clears it).
200
+ n_clear : int
201
+ Number of consecutive diverging+beyond-d_u steps required before
202
+ an active scenario is cleared. Prevents noisy velocity from
203
+ flickering the tcpa sign and resetting the scenario prematurely.
204
+
205
+ A confirmed overtaking encounter is held until the overtaking vessel is
206
+ past the overtaken vessel's beam (COLREGs Rule 13(d): a later change of
207
+ bearing does not end the duty to keep clear, or turn the encounter into a
208
+ crossing, until she is finally past and clear). Until then it is neither
209
+ cleared nor reclassified, unless the vessels are more than d_a apart.
210
+ Without this, an overtaking vessel that turns away hard makes the TCPA
211
+ go negative and loses the encounter mid-overtake.
212
+ """
213
+
214
+ dcpa_max: float = 1500.0
215
+ d_a: float = 10000.0
216
+ d_u: float = 150.0
217
+ n_t: int = 11
218
+ w_reinforce: float = 1.0
219
+ w_forget: float = 0.01
220
+ n_clear: int = 5
221
+
222
+ _pair_states: dict[tuple[int, int], _PairState] = field(
223
+ default_factory=dict, init=False, repr=False
224
+ )
225
+
226
+ # ------------------------------------------------------------------
227
+ # Public API
228
+ # ------------------------------------------------------------------
229
+
230
+ def update(self, states: dict[int, VesselState]) -> ColregsGraph:
231
+ """
232
+ Classify all vessel pairs and return a ColregsGraph.
233
+
234
+ Parameters
235
+ ----------
236
+ states : dict[int, VesselState]
237
+ Current observed vessel states keyed by vessel index.
238
+ """
239
+ graph = ColregsGraph()
240
+ ids = sorted(states.keys())
241
+
242
+ for i, id_a in enumerate(ids):
243
+ for id_b in ids[i + 1:]:
244
+ key = (id_a, id_b) # always id_a < id_b (sorted above)
245
+ if key not in self._pair_states:
246
+ self._pair_states[key] = _PairState()
247
+
248
+ edge = self._classify_pair(
249
+ id_a, states[id_a],
250
+ id_b, states[id_b],
251
+ self._pair_states[key],
252
+ )
253
+ if edge is not None:
254
+ graph.add_edge(edge)
255
+
256
+ return graph
257
+
258
+ def reset(self) -> None:
259
+ """Clear all hysteresis state (e.g. between episodes)."""
260
+ self._pair_states.clear()
261
+
262
+ # ------------------------------------------------------------------
263
+ # Internal: single pair
264
+ # ------------------------------------------------------------------
265
+
266
+ def _classify_pair(
267
+ self,
268
+ id_a: int, a: VesselState,
269
+ id_b: int, b: VesselState,
270
+ ps: _PairState,
271
+ ) -> Optional[ColregsEdge]:
272
+
273
+ # Relative position / velocity of b w.r.t. a, and CPA (tcpa < 0 means
274
+ # the vessels are already diverging).
275
+ dn = b.north - a.north
276
+ de = b.east - a.east
277
+ dvn = b.speed * math.cos(b.psi) - a.speed * math.cos(a.psi)
278
+ dve = b.speed * math.sin(b.psi) - a.speed * math.sin(a.psi)
279
+ dist = math.hypot(dn, de)
280
+ speed_sq = dvn * dvn + dve * dve
281
+ if speed_sq < 1e-6:
282
+ dcpa, tcpa = dist, math.inf
283
+ else:
284
+ tcpa = -(dn * dvn + de * dve) / speed_sq
285
+ dcpa = math.hypot(dn + tcpa * dvn, de + tcpa * dve)
286
+ approaching = tcpa > 0
287
+ overtaking = self._overtaking_in_progress(a, b, dn, de, dist, ps)
288
+
289
+ # --- Hysteresis: clear active scenario if vessels are separating,
290
+ # beyond the discard distance, AND the backward-projected CPA also
291
+ # indicates they have truly parted (dcpa > d_u). The dcpa check
292
+ # prevents premature clearing when ego's manoeuvre makes the
293
+ # current-velocity TCPA go negative mid-encounter while the vessels
294
+ # haven't yet physically separated beyond d_u.
295
+ if ps.active_scenario is not None:
296
+ if dist > self.d_u and not approaching and dcpa > self.d_u and not overtaking:
297
+ ps.clear_count += 1
298
+ if ps.clear_count >= self.n_clear:
299
+ ps.active_scenario = None
300
+ ps.hysteresis_score = 0.0
301
+ ps.clear_count = 0
302
+ else:
303
+ ps.clear_count = 0
304
+
305
+ # --- Overtaking: hold the encounter as it is until the overtaking
306
+ # vessel is past (Rule 13(d)); bearing changes do not reclassify it.
307
+ if overtaking:
308
+ return self._active_edge(id_a, id_b, ps)
309
+
310
+ # --- DCPA gate: suppress new encounter detection when projected CPA is
311
+ # very large, but always preserve the already-active edge so the
312
+ # starboard constraint is not transiently dropped.
313
+ if dcpa > self.dcpa_max:
314
+ return self._active_edge(id_a, id_b, ps)
315
+
316
+ # --- Range / approach gate: only classify pairs within d_a and closing
317
+ if dist > self.d_a or not approaching:
318
+ return self._active_edge(id_a, id_b, ps)
319
+
320
+ # --- Temporally aware geometric classification
321
+ scenario = self._temporal_classify(a.psi, b.psi, dn, de, dvn, dve, tcpa)
322
+ if scenario is None:
323
+ return self._active_edge(id_a, id_b, ps)
324
+
325
+ # --- Hysteresis update
326
+ self._update_hysteresis(ps, scenario)
327
+ return self._active_edge(id_a, id_b, ps)
328
+
329
+ def _overtaking_in_progress(
330
+ self, a: VesselState, b: VesselState, dn: float, de: float, dist: float,
331
+ ps: _PairState,
332
+ ) -> bool:
333
+ """True while a confirmed overtaking encounter is not yet past: the
334
+ overtaking vessel is still abaft the overtaken vessel's beam, and the
335
+ vessels are within d_a. (dn, de) is b's position relative to a."""
336
+ if dist > self.d_a or ps.hysteresis_score < self.w_reinforce:
337
+ return False
338
+ if ps.active_scenario == EncounterType.OVERTAKING_AHEAD: # a overtakes b
339
+ along = -(dn * math.cos(b.psi) + de * math.sin(b.psi))
340
+ elif ps.active_scenario == EncounterType.OVERTAKING_ASTERN: # b overtakes a
341
+ along = dn * math.cos(a.psi) + de * math.sin(a.psi)
342
+ else:
343
+ return False
344
+ return along < 0.0
345
+
346
+ def _active_edge(
347
+ self, id_a: int, id_b: int, ps: _PairState
348
+ ) -> Optional[ColregsEdge]:
349
+ if ps.active_scenario is None:
350
+ return None
351
+ role_a, role_b = ENCOUNTER_ROLES[ps.active_scenario]
352
+ return ColregsEdge(
353
+ vessel_a=id_a, vessel_b=id_b,
354
+ role_a=role_a, role_b=role_b,
355
+ encounter_type=ps.active_scenario,
356
+ )
357
+
358
+ # ------------------------------------------------------------------
359
+ # Temporal classification (Gleeson §2.1.2)
360
+ # ------------------------------------------------------------------
361
+
362
+ def _temporal_classify(
363
+ self,
364
+ psi_a: float, psi_b: float,
365
+ dn: float, de: float,
366
+ dvn: float, dve: float,
367
+ tcpa: float,
368
+ ) -> Optional[EncounterType]:
369
+ """
370
+ Linearly project both vessels to CPA, classify at n_t equally spaced
371
+ steps, and return the priority-weighted mode scenario label.
372
+
373
+ (dn, de) and (dvn, dve) are b's position and velocity relative to a.
374
+ Under constant-velocity projection the relative course psi is fixed,
375
+ so only the relative bearing beta(t) of dn + t*dvn, de + t*dve varies.
376
+ The DCPA is also invariant along the projection (same relative line),
377
+ so the caller's DCPA gate covers every projected step.
378
+ """
379
+ row = _TABLE[bisect_right(_PSI_EDGES, _wrap_deg(psi_b - psi_a))]
380
+ beta0 = _wrap_deg(math.atan2(de, dn) - psi_a)
381
+
382
+ if not math.isfinite(tcpa) or tcpa <= 0:
383
+ return row[bisect_right(_BETA_EDGES, beta0)]
384
+
385
+ n = self.n_t - 1
386
+ votes: dict[EncounterType, int] = {}
387
+
388
+ for k in range(self.n_t):
389
+ t = tcpa * k / n
390
+ beta = _wrap_deg(math.atan2(de + t * dve, dn + t * dvn) - psi_a)
391
+ label = row[bisect_right(_BETA_EDGES, beta)]
392
+ if label is not None:
393
+ votes[label] = votes.get(label, 0) + 1
394
+
395
+ if not votes:
396
+ return row[bisect_right(_BETA_EDGES, beta0)]
397
+
398
+ max_votes = max(votes.values())
399
+ candidates = [s for s, v in votes.items() if v == max_votes]
400
+ return min(candidates, key=lambda s: _PRIORITY[s])
401
+
402
+ # ------------------------------------------------------------------
403
+ # Hysteresis (Gleeson §2.1.4)
404
+ # ------------------------------------------------------------------
405
+
406
+ def _update_hysteresis(self, ps: _PairState, new_scenario: EncounterType) -> None:
407
+ if ps.active_scenario is None:
408
+ ps.active_scenario = new_scenario
409
+ ps.hysteresis_score = 0.0
410
+ return
411
+
412
+ if new_scenario == ps.active_scenario:
413
+ ps.hysteresis_score = min(1.0, ps.hysteresis_score + self.w_reinforce)
414
+ else:
415
+ ps.hysteresis_score = max(0.0, ps.hysteresis_score - self.w_forget)
416
+ if ps.hysteresis_score <= 0.0:
417
+ ps.active_scenario = new_scenario
418
+ ps.hysteresis_score = 0.0