diff --git a/notebooks/user_guide.ipynb b/notebooks/user_guide.ipynb index c2d77b5d..c3f452ad 100644 --- a/notebooks/user_guide.ipynb +++ b/notebooks/user_guide.ipynb @@ -4495,6 +4495,93 @@ "plt.show()" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "#### Intrusion and Avoidance numbers\n", + "\n", + "The dimensionless Intrusion number $\\mathcal{I}n$ and Avoidance number $\\mathcal{A}v$ characterise a crowd by how much pedestrians encroach on each other's personal space and how imminent collisions are ([Cordes et al., *PNAS Nexus*, 2024](https://doi.org/10.1093/pnasnexus/pgae120)). Both are computed per pedestrian and frame.\n", + "\n", + "The **Intrusion number** adds up the encroachments of all neighbours $j$ within $3\\,r_{\\text{soc}}$ of pedestrian $i$ (Eq. 1 of the paper):\n", + "\n", + "$$\n", + "\\mathcal{I}n_i = \\sum_{j \\in \\mathcal{N}_i}\n", + "\\left(\\frac{r_{\\text{soc}} - \\ell_{\\text{min}}}{r_{ij} - \\ell_{\\text{min}}}\\right)^2\n", + "$$\n", + "\n", + "with the social radius $r_{\\text{soc}} = 0.8\\,$m, the pedestrian diameter $\\ell_{\\text{min}} = 0.2\\,$m and the centre-to-centre distance $r_{ij}$. `IntrusionMethod.SUM` (the default) is this definition: encroachments add up, as mechanical forces do. `IntrusionMethod.MAX` is the variant of Eq. S6 in the paper's SI Appendix B: the intrusion is dominated by the closest neighbour and does not grow with the number of neighbours. The authors report that the delineation of regimes is robust under this change.\n", + "\n", + "The **Avoidance number** is based on the time-to-collision $\\tau_{ij}$, the time until $i$ and $j$ first touch if both keep their current velocity, with $\\tau_{ij} = \\infty$ if they never touch (Eq. 2 of the paper):\n", + "\n", + "$$\n", + "\\mathcal{A}v_i = \\sum_{j \\in \\mathcal{N}'_i} \\frac{\\tau_0}{\\tau_{ij}}\n", + "$$\n", + "\n", + "with the reference time $\\tau_0 = 3\\,$s. The equation is written as a sum, but the paper restricts the neighbour set $\\mathcal{N}'_i$ to the single neighbour with the shortest $\\tau_{ij}$, the most imminent risk. The sum therefore has one term, and because $\\tau_0/\\tau_{ij}$ decreases with $\\tau_{ij}$ it equals the maximum of $\\tau_0/\\tau_{ij}$ over all neighbours in the frame. That maximum is what `compute_avoidance` returns; a pedestrian on no collision course gets $\\mathcal{A}v_i = 0$.\n", + "\n", + "Recorded head trajectories produce apparent overlaps of the discs. Following the paper, both functions cap each per-pair term, at 400 for $\\mathcal{I}n_{ij}$ and 60 for $\\mathcal{A}v_{ij}$ (`cap=None` disables it). The paper also smooths the trajectories with a 4th-order Butterworth filter at 0.5 Hz and computes velocities over about 1 s; the functions do neither, so we choose `frame_step` for a 1 s window below.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from pedpy import IntrusionMethod, compute_avoidance, compute_intrusion\n", + "\n", + "intrusion = compute_intrusion(traj_data=traj)\n", + "intrusion.head()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "frame_step = int(traj.frame_rate / 2) # velocity window 2 * frame_step / frame_rate = 1 s, as in the paper\n", + "avoidance = compute_avoidance(traj_data=traj, frame_step=frame_step, tau_0=3.0)\n", + "avoidance.head()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "##### Temporal evolution\n", + "\n", + "We can visualize how these numbers evolve over time by averaging over the pedestrians in each frame. We follow the paper's rules: the intrusion number is averaged over all pedestrians, isolated ones counting as 0, while the avoidance number is averaged only over pedestrians on a collision course, $\\mathcal{A}v_i > 0$. The paper samples these averages every 0.5 s over the stationary state; here we show every frame.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "\n", + "mean_in = intrusion.groupby(\"frame\")[\"intrusion\"].mean()\n", + "mean_av = avoidance[avoidance[\"avoidance\"] > 0].groupby(\"frame\")[\"avoidance\"].mean()\n", + "\n", + "fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 4))\n", + "\n", + "ax1.plot(mean_in.index / traj.frame_rate, mean_in.values)\n", + "ax1.set_xlabel(\"Time / s\")\n", + "ax1.set_ylabel(r\"$\\langle \\mathcal{I}n \\rangle$\")\n", + "ax1.set_title(\"Mean Intrusion Number\")\n", + "\n", + "ax2.plot(mean_av.index / traj.frame_rate, mean_av.values)\n", + "ax2.set_xlabel(\"Time / s\")\n", + "ax2.set_ylabel(r\"$\\langle \\mathcal{A}v \\rangle$\")\n", + "ax2.set_title(\"Mean Avoidance Number\")\n", + "\n", + "fig.tight_layout()\n", + "plt.show()" + ] + }, { "cell_type": "markdown", "metadata": { diff --git a/pedpy/__init__.py b/pedpy/__init__.py index 6dd82288..8950914d 100644 --- a/pedpy/__init__.py +++ b/pedpy/__init__.py @@ -12,6 +12,7 @@ from .column_identifier import ( ACC_COL, + AVOIDANCE_COL, A_X_COL, A_Y_COL, COUNT_COL, @@ -25,6 +26,7 @@ FRAME_COL, ID_COL, INTERSECTION_COL, + INTRUSION_COL, LAST_FRAME_COL, MEAN_SPEED_COL, MID_FRAME_COL, @@ -96,6 +98,7 @@ Cutoff, SpeedCalculation, compute_frame_range_in_area, + compute_individual_distances, compute_individual_voronoi_polygons, compute_intersecting_polygons, compute_neighbor_distance, @@ -117,7 +120,12 @@ compute_speed_profile, get_grid_cells, ) -from .methods.spatial_analysis import compute_pair_distribution_function +from .methods.spatial_analysis import ( + IntrusionMethod, + compute_avoidance, + compute_intrusion, + compute_pair_distribution_function, +) from .methods.speed_calculator import ( compute_individual_speed, compute_line_speed, @@ -187,6 +195,7 @@ "compute_frame_range_in_area", "compute_individual_voronoi_polygons", "compute_intersecting_polygons", + "compute_individual_distances", "compute_neighbors", "compute_neighbor_distance", "compute_time_distance_line", @@ -216,6 +225,9 @@ "compute_mean_acceleration_per_frame", "compute_voronoi_acceleration", "correct_invalid_trajectories", + "compute_intrusion", + "IntrusionMethod", + "compute_avoidance", "detect_anomalies_in_trajectories", "PEDPY_BLUE", "PEDPY_GREEN", @@ -271,6 +283,8 @@ "MID_POSITION_COL", "END_POSITION_COL", "WINDOW_SIZE_COL", + "INTRUSION_COL", + "AVOIDANCE_COL", "__version__", "AccelerationError", "GeometryError", diff --git a/pedpy/column_identifier.py b/pedpy/column_identifier.py index bde0ee2f..2f4add48 100644 --- a/pedpy/column_identifier.py +++ b/pedpy/column_identifier.py @@ -44,3 +44,6 @@ SPEED_SP2_COL: Final = "s_sp-1" FLOW_SP1_COL: Final = "j_sp+1" FLOW_SP2_COL: Final = "j_sp-1" + +INTRUSION_COL: Final = "intrusion" +AVOIDANCE_COL: Final = "avoidance" diff --git a/pedpy/methods/method_utils.py b/pedpy/methods/method_utils.py index 76bb0464..85870774 100644 --- a/pedpy/methods/method_utils.py +++ b/pedpy/methods/method_utils.py @@ -1309,3 +1309,35 @@ def is_individual_speed_valid( return DataValidationStatus.ENTRY_MISSING return DataValidationStatus.DATA_CORRECT + + +def compute_individual_distances(*, traj_data: TrajectoryData) -> pd.DataFrame: + """Compute pairwise distances between all pedestrians per frame. + + Args: + traj_data: trajectory data + + Returns: + DataFrame with columns 'id', 'frame', 'neighbor_id', and 'distance' + """ + neighbor_point_col = f"{POINT_COL}_{NEIGHBOR_ID_COL}" + points = traj_data.data[[ID_COL, FRAME_COL, POINT_COL]] + neighbor_points = points.rename( + columns={ + ID_COL: NEIGHBOR_ID_COL, + POINT_COL: neighbor_point_col, + } + ) + matrix = points.merge( + neighbor_points, + how="inner", + on=FRAME_COL, + ) + matrix = matrix[matrix[ID_COL] != matrix[NEIGHBOR_ID_COL]] + matrix[DISTANCE_COL] = np.linalg.norm( + shapely.get_coordinates(matrix[POINT_COL]) - shapely.get_coordinates(matrix[neighbor_point_col]), + axis=1, + ) + + matrix = matrix.drop(columns=[POINT_COL, neighbor_point_col]) + return matrix.reset_index(drop=True) diff --git a/pedpy/methods/spatial_analysis.py b/pedpy/methods/spatial_analysis.py index 4fa5a060..1e088271 100644 --- a/pedpy/methods/spatial_analysis.py +++ b/pedpy/methods/spatial_analysis.py @@ -1,19 +1,35 @@ """Module containing functions to compute spatial analysis methods. -For example: the pair distribution function. +For example: the pair distribution function, and the intrusion and +avoidance numbers of Cordes et al., PNAS Nexus 2024 +(https://doi.org/10.1093/pnasnexus/pgae120). """ import warnings -from typing import Tuple +from enum import Enum +from typing import Optional, Tuple import numpy as np import numpy.typing as npt import pandas as pd +import shapely from scipy.spatial.distance import cdist -from pedpy.column_identifier import FRAME_COL, ID_COL, X_COL, Y_COL +from pedpy.column_identifier import ( + AVOIDANCE_COL, + FRAME_COL, + ID_COL, + INTRUSION_COL, + X_COL, + Y_COL, +) from pedpy.data.trajectory_data import TrajectoryData -from pedpy.methods.method_utils import _check_trajectory_data +from pedpy.methods.method_utils import ( + SpeedCalculation, + _check_trajectory_data, + compute_individual_distances, +) +from pedpy.methods.speed_calculator import compute_individual_speed def compute_pair_distribution_function( @@ -133,3 +149,230 @@ def _calculate_pair_distances( distances_list.extend(distances_upper_triangle) return np.array(distances_list) + + +class IntrusionMethod(Enum): # pylint: disable=too-few-public-methods + """Identifier of method to compute the intrusion.""" + + MAX = "max" + """Use the max value in neighborhood as intrusion.""" + SUM = "sum" + """Use the sum of all values in neighborhood as intrusion.""" + + +def compute_intrusion( + *, + traj_data: TrajectoryData, + r_soc: float = 0.8, + l_min: float = 0.2, + method: IntrusionMethod = IntrusionMethod.SUM, + cap: Optional[float] = 400.0, +) -> pd.DataFrame: + r"""Compute the intrusion number for each pedestrian per frame. + + The intrusion variable :math:`\mathcal{I}n_i` quantifies how much + other agents encroach on pedestrian *i*'s personal space + (Cordes et al. 2024, Eq. 1): + + .. math:: + + \mathcal{I}n_i = \sum_{j \in \mathcal{N}_i} + \left(\frac{r_\text{soc} - \ell_\text{min}} + {r_{ij} - \ell_\text{min}}\right)^{k_I}, + + with :math:`k_I = 2`. The neighbor set :math:`\mathcal{N}_i` contains + all agents *j* with :math:`r_{ij} \leq 3\,r_\text{soc}`. + + Differences to the paper's processing that the caller has to + handle: + + - The paper's simulated models (Jakob-KKU/Pedestrian_Models) replace + Eq. 1 within 0.01 m of contact by a smooth polynomial so that the + model force stays finite. The data analysis has no such term and + none is applied here; the cap takes that role. + - The paper smooths the trajectories with a 4th-order Butterworth + filter at 0.5 Hz before computing :math:`r_{ij}`. No smoothing is + done here; smooth the trajectories first if they contain head sway. + - The paper's crowd average of :math:`\mathcal{I}n` includes isolated + pedestrians with :math:`\mathcal{I}n_i = 0`, sampled every 0.5 s + over the stationary state. + + Args: + traj_data (TrajectoryData): trajectory data to analyze + r_soc (float): social radius in m (default 0.8) + l_min (float): pedestrian diameter in m (default 0.2) + method (IntrusionMethod): aggregation over the neighbors. + ``SUM`` is the definition of the paper (Eq. 1): the + encroachments of all neighbors add up, as mechanical forces + do. ``MAX`` is the variant of Eq. S6 in the paper's SI + Appendix B: the intrusion is dominated by the closest + neighbor and does not grow with the number of neighbors. + The authors report that the delineation of regimes is + robust under this change. + cap (float, optional): upper bound applied to every per-pair term + before aggregation (default 400, the paper's value, which + limits artifacts from apparent overlaps of the discs in + noisy trajectories). Pairs at or below ``l_min`` distance, + where Eq. 1 diverges, take the cap. ``None`` disables the + cap; such pairs are then infinite. + + Returns: + DataFrame with columns 'id', 'frame', and 'intrusion', one row + per pedestrian and frame. Agents with no neighbor within the + cutoff get 0. + """ + intrusion = compute_individual_distances(traj_data=traj_data) + intrusion = intrusion.loc[intrusion.distance <= 3 * r_soc].copy() + with np.errstate(divide="ignore"): + intrusion.loc[:, INTRUSION_COL] = np.where( + intrusion.distance > l_min, + ((r_soc - l_min) / (intrusion.distance - l_min)) ** 2, + np.inf, + ) + if cap is not None: + intrusion.loc[:, INTRUSION_COL] = np.minimum(intrusion[INTRUSION_COL], cap) + intrusion = ( + intrusion.groupby(by=[ID_COL, FRAME_COL]) + .agg( + intrusion=(INTRUSION_COL, method.value), + ) + .reset_index() + ) + + return _fill_missing_agents(traj_data=traj_data, result=intrusion, column=INTRUSION_COL) + + +def compute_avoidance( + *, + traj_data: TrajectoryData, + frame_step: int, + l_min: float = 0.2, + tau_0: float, + cap: Optional[float] = 60.0, +) -> pd.DataFrame: + r"""Compute the avoidance number for each pedestrian per frame. + + The avoidance variable :math:`\mathcal{A}v_i` quantifies the + imminence of collisions that pedestrian *i* faces, based on + the time-to-collision (TTC) with neighbors + (Cordes et al. 2024, Eq. 2): + + .. math:: + + \mathcal{A}v_i = \sum_{j \in \mathcal{N}'_i} + \left(\frac{\tau_0}{\tau_{ij}}\right)^{k_A}, + + with :math:`k_A = 1`. Unlike Eq. 1, the neighbor set + :math:`\mathcal{N}'_i` contains only the neighbor with the shortest + time-to-collision :math:`\tau_{ij}`, the most imminent risk. The sum + therefore has a single term, and because :math:`\tau_0 / \tau_{ij}` + decreases with :math:`\tau_{ij}`, :math:`\mathcal{A}v_i` equals the + largest :math:`\tau_0 / \tau_{ij}` over all neighbors *j* in the same + frame. That maximum is what this function computes. The TTC is the + time until the discs of diameter ``l_min`` around *i* and *j* first + touch if both keep their current velocity; pairs that are not on a + collision course have :math:`\tau_{ij} = \infty` and contribute 0. + + Pairs whose discs already touch or overlap (:math:`r_{ij} \leq \ell`) + have no collision ahead of them, so :math:`\tau_{ij} = \infty` and + they contribute 0. This is the convention of the authors' data + analysis (Jakob-KKU/Analyze_Trajectories); their simulation code + instead treats contact as :math:`\tau_{ij} = 0`, a safeguard for + hard discs that cannot overlap. With the analysis convention and + the cap below, this function and an independent re-implementation + of the authors' analysis code give identical numbers and reproduce + the six Waiting points of the paper's Fig. 1B on the paper's + supplementary trajectories within the reading accuracy of the + figure. Every pair in a frame is a candidate; the paper sets no + distance cutoff for :math:`\mathcal{N}'_i`. + + Differences to the paper's processing that the caller has to + handle: + + - Velocities come from :func:`compute_individual_speed` with + :attr:`SpeedCalculation.BORDER_SINGLE_SIDED`, a centered window of + ``2 * frame_step / frame_rate`` seconds, one-sided at the borders of + a trajectory. The paper smooths the trajectories with a 4th-order + Butterworth filter at 0.5 Hz and computes velocities over about + 1 s. Neither is done here; :math:`\mathcal{A}v` is sensitive to + both. ``frame_step = frame_rate / 2`` gives the paper's window. + - The paper's crowd average of :math:`\mathcal{A}v` excludes + pedestrians with :math:`\mathcal{A}v_i = 0` ("we only consider data + points with a finite TTC in the average"), sampled every 0.5 s over + the stationary state. + + Args: + traj_data (TrajectoryData): trajectory data to analyze + frame_step (int): number of frames on each side of a frame used + for the velocity, see above + l_min (float): pedestrian diameter :math:`\ell` in m at which + two pedestrians touch (default 0.2, as the paper uses for + recorded trajectories; its simulations use + :math:`\ell_\text{soc} = 0.4` m instead) + tau_0 (float): reference timescale in s (paper uses 3.0) + cap (float, optional): upper bound applied to every per-pair term + before taking the maximum (default 60, the paper's value, + which limits artifacts from apparent overlaps of the discs + in noisy trajectories). ``None`` disables the cap. + + Returns: + DataFrame with columns 'id', 'frame', and 'avoidance', one row + per pedestrian and frame. Agents on no collision course, or + alone in their frame, get 0. + """ + velocity = compute_individual_speed( + traj_data=traj_data, + frame_step=frame_step, + compute_velocity=True, + speed_calculation=SpeedCalculation.BORDER_SINGLE_SIDED, + ) + + data = traj_data.data.merge(velocity, on=[ID_COL, FRAME_COL]) + data["velocity"] = shapely.points(data.v_x, data.v_y) + + matrix = data.merge(data, how="inner", on=FRAME_COL, suffixes=("", "_neighbor")) + matrix = matrix[matrix[ID_COL] != matrix[f"{ID_COL}_neighbor"]] + + pos = shapely.get_coordinates(matrix.point) + pos_neighbor = shapely.get_coordinates(matrix.point_neighbor) + distance = np.linalg.norm(pos - pos_neighbor, axis=1) + + delta_v = shapely.get_coordinates(matrix.velocity) - shapely.get_coordinates(matrix.velocity_neighbor) + delta_v_norm = np.linalg.norm(delta_v, axis=1) + + # solve the quadratic only for separated pairs with a relative velocity + computable = (distance > l_min) & (delta_v_norm > 0) + + ttc = np.full(matrix.shape[0], np.inf) + + if np.any(computable): + d_c = distance[computable] + dv_c = delta_v[computable] + dv_norm_c = delta_v_norm[computable] + + e_v = (pos[computable] - pos_neighbor[computable]) / d_c[:, np.newaxis] + v_rel_hat = dv_c / dv_norm_c[:, np.newaxis] + cos_alpha = np.sum(e_v * v_rel_hat, axis=1) + + capital_a = (cos_alpha**2 - 1) * d_c**2 + l_min**2 + sqrt_a_safe = np.sqrt(np.maximum(capital_a, 0.0)) + + valid = (capital_a >= 0) & (-cos_alpha * d_c - sqrt_a_safe >= 0) + + idx = np.where(computable)[0][valid] + ttc[idx] = (-cos_alpha[valid] * d_c[valid] - sqrt_a_safe[valid]) / dv_norm_c[valid] + + matrix = matrix.copy() + with np.errstate(divide="ignore"): + matrix[AVOIDANCE_COL] = tau_0 / ttc + if cap is not None: + matrix[AVOIDANCE_COL] = np.minimum(matrix[AVOIDANCE_COL], cap) + + avoidance = matrix.groupby(by=[ID_COL, FRAME_COL], as_index=False).agg(avoidance=(AVOIDANCE_COL, "max")) + return _fill_missing_agents(traj_data=traj_data, result=avoidance, column=AVOIDANCE_COL) + + +def _fill_missing_agents(*, traj_data: TrajectoryData, result: pd.DataFrame, column: str) -> pd.DataFrame: + """Return one row per pedestrian and frame, 0 where no pair contributed.""" + agents = traj_data.data[[ID_COL, FRAME_COL]] + return agents.merge(result, on=[ID_COL, FRAME_COL], how="left").fillna({column: 0.0}) diff --git a/tests/unit_tests/methods/test_spatial_analysis.py b/tests/unit_tests/methods/test_spatial_analysis.py new file mode 100644 index 00000000..eecaddb8 --- /dev/null +++ b/tests/unit_tests/methods/test_spatial_analysis.py @@ -0,0 +1,216 @@ +import math + +import pandas as pd +import pytest + +from pedpy.column_identifier import AVOIDANCE_COL, FRAME_COL, ID_COL, INTRUSION_COL +from pedpy.data.trajectory_data import TrajectoryData +from pedpy.methods.spatial_analysis import ( + IntrusionMethod, + compute_avoidance, + compute_intrusion, +) + + +def _make_traj(positions, frame_rate=10): + """Build a single-frame TrajectoryData from a list of (x, y) positions.""" + rows = [] + for pid, (x, y) in enumerate(positions): + rows.append({"id": pid, "frame": 0, "x": x, "y": y}) + return TrajectoryData(data=pd.DataFrame(rows), frame_rate=frame_rate) + + +# --------------------------------------------------------------------------- +# Intrusion +# --------------------------------------------------------------------------- + + +class TestComputeIntrusion: + """Tests for compute_intrusion (Cordes et al. 2024, Eq. 1).""" + + def test_two_agents_known_distance(self): + """Two agents at known distance, check In_i by hand.""" + r_soc = 0.8 + l_min = 0.2 + d = 0.5 # distance between the two agents + # Expected per-pair intrusion: ((0.8-0.2)/(0.5-0.2))^2 = (0.6/0.3)^2 = 4.0 + traj = _make_traj([(0, 0), (d, 0)]) + result = compute_intrusion(traj_data=traj, r_soc=r_soc, l_min=l_min) + + for pid in [0, 1]: + val = result.loc[result[ID_COL] == pid, INTRUSION_COL].iloc[0] + assert val == pytest.approx(4.0) + + def test_neighbor_cutoff_excludes_distant_agents(self): + """Agents beyond 3*r_soc should not contribute to intrusion.""" + r_soc = 0.8 + l_min = 0.2 + far = 3 * r_soc + 0.1 # just beyond cutoff + traj = _make_traj([(0, 0), (far, 0)]) + result = compute_intrusion(traj_data=traj, r_soc=r_soc, l_min=l_min) + + # Both agents are present with zero intrusion + assert len(result) == 2 + assert (result[INTRUSION_COL] == 0).all() + + def test_three_agents_sum(self): + """Three agents: agent 0 has two neighbors, check sum.""" + r_soc = 0.8 + l_min = 0.2 + d1 = 0.5 + d2 = 0.6 + traj = _make_traj([(0, 0), (d1, 0), (0, d2)]) + + result = compute_intrusion(traj_data=traj, r_soc=r_soc, l_min=l_min, method=IntrusionMethod.SUM) + + in_01 = ((r_soc - l_min) / (d1 - l_min)) ** 2 + in_02 = ((r_soc - l_min) / (d2 - l_min)) ** 2 + expected_0 = in_01 + in_02 + + val = result.loc[result[ID_COL] == 0, INTRUSION_COL].iloc[0] + assert val == pytest.approx(expected_0) + + def test_method_max(self): + """IntrusionMethod.MAX returns the max over neighbors.""" + r_soc = 0.8 + l_min = 0.2 + d1 = 0.5 + d2 = 0.6 + traj = _make_traj([(0, 0), (d1, 0), (0, d2)]) + + result = compute_intrusion(traj_data=traj, r_soc=r_soc, l_min=l_min, method=IntrusionMethod.MAX) + + in_01 = ((r_soc - l_min) / (d1 - l_min)) ** 2 + in_02 = ((r_soc - l_min) / (d2 - l_min)) ** 2 + expected_0 = max(in_01, in_02) + + val = result.loc[result[ID_COL] == 0, INTRUSION_COL].iloc[0] + assert val == pytest.approx(expected_0) + + def test_near_contact_pair_is_capped(self): + """A pair just outside l_min takes the paper's cap of 400.""" + traj = _make_traj([(0, 0), (0.21, 0)]) + result = compute_intrusion(traj_data=traj) + assert (result[INTRUSION_COL] == 400.0).all() + + raw = compute_intrusion(traj_data=traj, cap=None) + assert raw[INTRUSION_COL].iloc[0] == pytest.approx((0.6 / 0.01) ** 2) + + def test_overlapping_pair_takes_the_cap(self): + """Discs at or inside l_min diverge in Eq. 1 and take the cap.""" + for d in (0.2, 0.1, 0.0): + traj = _make_traj([(0, 0), (d, 0)]) + assert (compute_intrusion(traj_data=traj)[INTRUSION_COL] == 400.0).all() + assert math.isinf(compute_intrusion(traj_data=traj, cap=None)[INTRUSION_COL].iloc[0]) + + def test_isolated_agent_has_zero_intrusion(self): + """An agent alone in its frame is present with In = 0.""" + traj = _make_traj([(0, 0)]) + result = compute_intrusion(traj_data=traj) + assert len(result) == 1 + assert result[INTRUSION_COL].iloc[0] == 0.0 + + +# --------------------------------------------------------------------------- +# Avoidance +# --------------------------------------------------------------------------- + + +def _make_moving_traj(agent_data, frame_rate=10): + """Build multi-frame TrajectoryData for agents with constant velocity. + + agent_data: list of (x0, y0, vx, vy) per agent. + Creates two frames separated by dt = 1/frame_rate so velocities resolve. + """ + rows = [] + dt = 1.0 / frame_rate + for pid, (x0, y0, vx, vy) in enumerate(agent_data): + for frame in range(3): + rows.append( + { + "id": pid, + "frame": frame, + "x": x0 + vx * frame * dt, + "y": y0 + vy * frame * dt, + } + ) + return TrajectoryData(data=pd.DataFrame(rows), frame_rate=frame_rate) + + +class TestComputeAvoidance: + """Tests for compute_avoidance (Cordes et al. 2024, Eq. 2).""" + + def test_head_on_collision(self): + """Two agents approaching head-on: TTC is analytically known.""" + # Agent 0 at x=-2, moving right at v=1 + # Agent 1 at x=+2, moving left at v=-1 + # At frame 1 (dt=0.1): positions are -1.9 and +1.9, distance=3.8 + # Relative speed = 2, TTC = (d - R) / |delta_v| = (3.8 - 0.2) / 2 = 1.8 + l_min = 0.2 + tau_0 = 3.0 + d_at_frame1 = 3.8 + expected_ttc = (d_at_frame1 - l_min) / 2.0 + expected_av = tau_0 / expected_ttc + + traj = _make_moving_traj([(-2, 0, 1, 0), (2, 0, -1, 0)], frame_rate=10) + result = compute_avoidance(traj_data=traj, frame_step=1, l_min=l_min, tau_0=tau_0) + + # Check frame 1 (middle frame where velocity is computed) + row = result[(result[ID_COL] == 0) & (result[FRAME_COL] == 1)] + assert len(row) == 1 + assert row[AVOIDANCE_COL].iloc[0] == pytest.approx(expected_av, rel=1e-9) + + def test_diverging_agents_zero_avoidance(self): + """Two agents moving apart should have Av = 0 (TTC = inf).""" + tau_0 = 3.0 + traj = _make_moving_traj([(-2, 0, -1, 0), (2, 0, 1, 0)], frame_rate=10) + result = compute_avoidance(traj_data=traj, frame_step=1, l_min=0.2, tau_0=tau_0) + + row = result[(result[ID_COL] == 0) & (result[FRAME_COL] == 1)] + assert len(row) == 1 + assert row[AVOIDANCE_COL].iloc[0] == pytest.approx(0.0, abs=1e-10) + + def test_oblique_near_miss(self): + """An approach that passes inside l_min collides, outside it does not.""" + tau_0 = 3.0 + l_min = 0.2 + # Agent 0 rests at the origin; agent 1 passes at lateral offset y + # moving at 1 m/s along x and is at x = -1.9 at frame 1. + y = 0.19 + traj = _make_moving_traj([(0, 0, 0, 0), (-2.0, y, 1, 0)], frame_rate=10) + result = compute_avoidance(traj_data=traj, frame_step=1, l_min=l_min, tau_0=tau_0) + expected_ttc = 1.9 - math.sqrt(l_min**2 - y**2) + row = result[(result[ID_COL] == 0) & (result[FRAME_COL] == 1)] + assert row[AVOIDANCE_COL].iloc[0] == pytest.approx(tau_0 / expected_ttc, rel=1e-9) + + traj = _make_moving_traj([(0, 0, 0, 0), (-2.0, 0.21, 1, 0)], frame_rate=10) + result = compute_avoidance(traj_data=traj, frame_step=1, l_min=l_min, tau_0=tau_0) + row = result[(result[ID_COL] == 0) & (result[FRAME_COL] == 1)] + assert row[AVOIDANCE_COL].iloc[0] == 0.0 + + def test_touching_and_overlapping_pairs_have_no_collision(self): + """Discs already in contact have no collision ahead: TTC inf, Av 0.""" + for x1 in (0.1, 0.05): # distance 0.2 and 0.1 at frame 1 + traj = _make_moving_traj([(-0.2, 0, 1, 0), (x1 + 0.1, 0, -1, 0)], frame_rate=10) + for cap in (60.0, None): + result = compute_avoidance(traj_data=traj, frame_step=1, tau_0=3.0, cap=cap) + row = result[(result[ID_COL] == 0) & (result[FRAME_COL] == 1)] + assert row[AVOIDANCE_COL].iloc[0] == 0.0 + + def test_near_contact_approach_is_capped(self): + """A pair 1 mm outside l_min closing at 0.2 m/s has Av 600, capped at 60.""" + traj = _make_moving_traj([(-0.2105, 0, 0.1, 0), (0.0105, 0, -0.1, 0)], frame_rate=10) + result = compute_avoidance(traj_data=traj, frame_step=1, tau_0=3.0) + row = result[(result[ID_COL] == 0) & (result[FRAME_COL] == 1)] + assert row[AVOIDANCE_COL].iloc[0] == 60.0 + + raw = compute_avoidance(traj_data=traj, frame_step=1, tau_0=3.0, cap=None) + row = raw[(raw[ID_COL] == 0) & (raw[FRAME_COL] == 1)] + assert row[AVOIDANCE_COL].iloc[0] == pytest.approx(600.0, rel=1e-6) + + def test_isolated_agent_has_zero_avoidance(self): + """An agent alone in its frame is present with Av = 0.""" + traj = _make_moving_traj([(0, 0, 1, 0)], frame_rate=10) + result = compute_avoidance(traj_data=traj, frame_step=1, tau_0=3.0) + assert len(result) == 3 + assert (result[AVOIDANCE_COL] == 0.0).all()