From 515dd9ff4071147ea3113cafd90a150f43781542 Mon Sep 17 00:00:00 2001 From: buzhidao Date: Thu, 16 Jul 2026 08:51:17 +0800 Subject: [PATCH 1/3] feat: add collision radius check helper + unit tests (#55) --- packages/server/src/lappa/sim/__init__.py | 3 +- packages/server/src/lappa/sim/collision.py | 52 +++++++++++++++ packages/server/tests/test_collision.py | 77 ++++++++++++++++++++++ 3 files changed, 131 insertions(+), 1 deletion(-) create mode 100644 packages/server/src/lappa/sim/collision.py create mode 100644 packages/server/tests/test_collision.py diff --git a/packages/server/src/lappa/sim/__init__.py b/packages/server/src/lappa/sim/__init__.py index 68e0fec..4934bea 100644 --- a/packages/server/src/lappa/sim/__init__.py +++ b/packages/server/src/lappa/sim/__init__.py @@ -1,4 +1,5 @@ from lappa.sim.engines import ENGINES, create_engine from lappa.sim.session import SESSION, SimSession +from lappa.sim.collision import point_in_circle, flag_collisions, first_collision -__all__ = ["ENGINES", "create_engine", "SESSION", "SimSession"] +__all__ = ["ENGINES", "create_engine", "SESSION", "SimSession", "point_in_circle", "flag_collisions", "first_collision"] diff --git a/packages/server/src/lappa/sim/collision.py b/packages/server/src/lappa/sim/collision.py new file mode 100644 index 0000000..a6d05cf --- /dev/null +++ b/packages/server/src/lappa/sim/collision.py @@ -0,0 +1,52 @@ +"""Collision radius check helpers for path waypoint validation.""" + +from __future__ import annotations + +CircleObstacle = tuple[float, float, float] # (center_x, center_y, radius) + + +def point_in_circle( + px: float, py: float, cx: float, cy: float, radius: float +) -> bool: + """Return True if point (px, py) is inside the circle (cx, cy, radius).""" + dx = px - cx + dy = py - cy + return dx * dx + dy * dy <= radius * radius + + +def flag_collisions( + path: list[tuple[float, float]], + obstacles: list[CircleObstacle], +) -> list[bool]: + """Return a bool list, True where the path point intersects any obstacle. + + Parameters + ---------- + path : list of (x, y) waypoints. + obstacles : list of (center_x, center_y, radius) circular obstacles. + + Returns + ------- + list[bool] of the same length as *path*, where *True* means the point + lies inside at least one circular obstacle. + """ + result: list[bool] = [] + for px, py in path: + collides = any( + point_in_circle(px, py, cx, cy, r) for cx, cy, r in obstacles + ) + result.append(collides) + return result + + +def first_collision( + path: list[tuple[float, float]], + obstacles: list[CircleObstacle], +) -> int | None: + """Return the index of the first path point inside any circular obstacle, + or None if the entire path is clear.""" + flags = flag_collisions(path, obstacles) + for i, hit in enumerate(flags): + if hit: + return i + return None diff --git a/packages/server/tests/test_collision.py b/packages/server/tests/test_collision.py new file mode 100644 index 0000000..34d4568 --- /dev/null +++ b/packages/server/tests/test_collision.py @@ -0,0 +1,77 @@ +"""Tests for collision radius check helpers.""" + + +from lappa.sim.collision import point_in_circle, flag_collisions, first_collision + + +def test_point_in_circle_inside() -> None: + """Exact center should be inside.""" + assert point_in_circle(0.0, 0.0, 0.0, 0.0, 1.0) is True + + +def test_point_in_circle_on_boundary() -> None: + """Point on radius boundary is considered inside.""" + assert point_in_circle(1.0, 0.0, 0.0, 0.0, 1.0) is True + + +def test_point_in_circle_outside() -> None: + """Point outside circle should be False.""" + assert point_in_circle(3.0, 0.0, 0.0, 0.0, 1.0) is False + + +def test_point_in_circle_diagonal() -> None: + """Diagonal distance check with tolerance.""" + d = 0.5 + # (0.5, 0.5) is distance ~0.707 from origin → inside radius 1 + assert point_in_circle(d, d, 0.0, 0.0, 1.0) is True + assert point_in_circle(0.9, 0.0, 0.0, 0.0, 1.0) is True + assert point_in_circle(1.1, 0.0, 0.0, 0.0, 1.0) is False + + +def test_point_in_circle_offset_center() -> None: + """Circle not at origin.""" + assert point_in_circle(5.0, 5.0, 5.0, 5.0, 2.0) is True + assert point_in_circle(7.0, 5.0, 5.0, 5.0, 2.0) is True # on boundary + assert point_in_circle(7.1, 5.0, 5.0, 5.0, 2.0) is False + + +def test_flag_collisions_all_clear() -> None: + path = [(10.0, 10.0), (20.0, 20.0), (30.0, 30.0)] + obstacles = [(0.0, 0.0, 1.0), (5.0, 5.0, 1.0)] + assert flag_collisions(path, obstacles) == [False, False, False] + + +def test_flag_collisions_some_hit() -> None: + path = [(10.0, 10.0), (0.0, 0.0), (30.0, 30.0)] + obstacles = [(0.0, 0.0, 1.0)] + assert flag_collisions(path, obstacles) == [False, True, False] + + +def test_flag_collisions_overlapping_obstacles() -> None: + """Point inside overlapping obstacle pair.""" + path = [(1.5, 0.0)] + obstacles = [(0.0, 0.0, 2.0), (2.0, 0.0, 0.5)] # both cover (1.5, 0) + assert flag_collisions(path, obstacles) == [True] + + +def test_first_collision_none() -> None: + path = [(10.0, 10.0)] + obstacles = [(0.0, 0.0, 1.0)] + assert first_collision(path, obstacles) is None + + +def test_first_collision_found() -> None: + path = [(10.0, 10.0), (0.5, 0.5), (20.0, 20.0), (0.0, 0.0)] + obstacles = [(0.0, 0.0, 1.0)] + # (0.5, 0.5) is first hit at index 1 + assert first_collision(path, obstacles) == 1 + + +def test_empty_path() -> None: + assert flag_collisions([], [(0.0, 0.0, 1.0)]) == [] + assert first_collision([], [(0.0, 0.0, 1.0)]) is None + + +def test_no_obstacles() -> None: + path = [(0.0, 0.0), (1.0, 1.0)] + assert flag_collisions(path, []) == [False, False] From 4a2304ae07999961e1516bc01ddd3a74b1bea902 Mon Sep 17 00:00:00 2001 From: buzhidao Date: Thu, 16 Jul 2026 08:52:33 +0800 Subject: [PATCH 2/3] feat: add s-curve smooth path fixture + unit test (#56) --- .../fixtures/sample_path_s_curve_smooth.json | 17 ++++++++++++++++ ...test_sample_path_s_curve_smooth_fixture.py | 20 +++++++++++++++++++ 2 files changed, 37 insertions(+) create mode 100644 packages/server/tests/fixtures/sample_path_s_curve_smooth.json create mode 100644 packages/server/tests/test_sample_path_s_curve_smooth_fixture.py diff --git a/packages/server/tests/fixtures/sample_path_s_curve_smooth.json b/packages/server/tests/fixtures/sample_path_s_curve_smooth.json new file mode 100644 index 0000000..03fdc63 --- /dev/null +++ b/packages/server/tests/fixtures/sample_path_s_curve_smooth.json @@ -0,0 +1,17 @@ +{ + "name": "s_curve_smooth", + "points": [ + [0, 1], + [1, 1.2], + [2, 1.8], + [3, 2.2], + [4, 2.0], + [5, 1.5], + [6, 1.0], + [7, 0.8], + [8, 1.0] + ], + "path_length_m": 8.55850692876998, + "expected_net_displacement_m": 8.0, + "note": "Smooth S-curve: gentle inflection at midpoint" +} diff --git a/packages/server/tests/test_sample_path_s_curve_smooth_fixture.py b/packages/server/tests/test_sample_path_s_curve_smooth_fixture.py new file mode 100644 index 0000000..7002184 --- /dev/null +++ b/packages/server/tests/test_sample_path_s_curve_smooth_fixture.py @@ -0,0 +1,20 @@ +"""S-curve smooth path fixture smoke.""" + +from __future__ import annotations + +import json +import math +from pathlib import Path + + +def test_sample_path_s_curve_smooth() -> None: + p = Path(__file__).parent / "fixtures" / "sample_path_s_curve_smooth.json" + data = json.loads(p.read_text(encoding="utf-8")) + pts = data["points"] + assert len(pts) >= 4 + length = 0.0 + for a, b in zip(pts, pts[1:]): + length += math.hypot(b[0] - a[0], b[1] - a[1]) + assert abs(length - float(data["path_length_m"])) < 1e-6 + net = math.hypot(pts[-1][0] - pts[0][0], pts[-1][1] - pts[0][1]) + assert abs(net - float(data["expected_net_displacement_m"])) < 1e-3 From 4580999394de716b3483ada49b153fd11eea021d Mon Sep 17 00:00:00 2001 From: buzhidao Date: Thu, 16 Jul 2026 08:55:05 +0800 Subject: [PATCH 3/3] feat: add lappa path resample CLI with --step-m (#52) --- packages/server/src/lappa/cli.py | 74 ++++++++++++++++++++++++++ packages/server/tests/test_resample.py | 71 ++++++++++++++++++++++++ 2 files changed, 145 insertions(+) create mode 100644 packages/server/tests/test_resample.py diff --git a/packages/server/src/lappa/cli.py b/packages/server/src/lappa/cli.py index a73c576..8e0cfa1 100644 --- a/packages/server/src/lappa/cli.py +++ b/packages/server/src/lappa/cli.py @@ -78,6 +78,49 @@ def _path_stats(points: list[tuple[float, float]]) -> dict[str, float | int]: } +def _resample( + points: list[tuple[float, float]], step_m: float +) -> list[tuple[float, float]]: + """Resample a polyline at fixed step meters. + + Walks along each segment inserting points every *step_m*. + The first and last original points are always included. + """ + if step_m <= 0: + raise ValueError("step_m must be positive") + if len(points) < 2: + return list(points) + + result: list[tuple[float, float]] = [points[0]] + remaining = 0.0 + + def _maybe_append(pt: tuple[float, float]) -> None: + """Append pt if it differs from the last point.""" + last = result[-1] + if abs(pt[0] - last[0]) > 1e-12 or abs(pt[1] - last[1]) > 1e-12: + result.append(pt) + + for a, b in zip(points, points[1:]): + seg_x = b[0] - a[0] + seg_y = b[1] - a[1] + seg_len = math.hypot(seg_x, seg_y) + if seg_len < 1e-12: + continue + + # Walk from the first step boundary along this segment + dist = step_m - remaining if remaining > 0 else step_m + while dist < seg_len + 1e-12: + t = min(dist / seg_len, 1.0) if seg_len > 0 else 0.0 + _maybe_append((a[0] + t * seg_x, a[1] + t * seg_y)) + dist += step_m + remaining = dist - seg_len + + # Ensure last point is included + _maybe_append(points[-1]) + + return result + + @app.command("version") def version_cmd() -> None: rprint({"version": __version__}) @@ -210,6 +253,37 @@ def path_stats_cmd( rprint(_path_stats(_path_points_from_fixture(file))) +@path_app.command("resample") +def path_resample_cmd( + file: Path = typer.Option( + ..., + "--file", + "-f", + exists=True, + file_okay=True, + dir_okay=False, + readable=True, + help="JSON fixture with a points array.", + ), + step_m: float = typer.Option( + 0.5, + "--step-m", + "-s", + min=0.001, + help="Step size in meters for resampling.", + ), +) -> None: + """Resample a polyline fixture at fixed step meters and print new length and points.""" + points = _path_points_from_fixture(file) + resampled = _resample(points, step_m) + stats = _path_stats(resampled) + stats["step_m"] = step_m + rprint(stats) + console.print("[dim]Points:[/dim]") + for px, py in resampled: + console.print(f" [{px:.4f}, {py:.4f}]") + + @workspace_app.command("open") def workspace_open(path: Path) -> None: pkg = workspace_store.resolve_package_ref(path, base_dir=Path.cwd()) diff --git a/packages/server/tests/test_resample.py b/packages/server/tests/test_resample.py new file mode 100644 index 0000000..47e71b1 --- /dev/null +++ b/packages/server/tests/test_resample.py @@ -0,0 +1,71 @@ +"""Tests for polyline resample helper.""" + +from __future__ import annotations + +import json + +from lappa.cli import _resample, _path_stats + + +def test_resample_identity_short() -> None: + """Resample with step larger than total length should keep endpoints.""" + pts = [(0.0, 0.0), (1.0, 0.0)] + result = _resample(pts, 5.0) + assert result == pts + + +def test_resample_step_half() -> None: + """1m line with 0.5m step → 3 points (0, 0.5, 1).""" + pts = [(0.0, 0.0), (1.0, 0.0)] + result = _resample(pts, 0.5) + assert len(result) == 3 + assert abs(result[0][0]) < 1e-12 + assert abs(result[1][0] - 0.5) < 1e-12 + assert abs(result[2][0] - 1.0) < 1e-12 + + +def test_resample_preserves_length() -> None: + """Resampled path length should be close to original.""" + pts = [(0.0, 0.0), (3.0, 4.0)] # 5m line + result = _resample(pts, 0.3) + orig_len = _path_stats(pts)["path_length_m"] + new_len = _path_stats(result)["path_length_m"] + assert abs(new_len - orig_len) < 0.01 + + +def test_resample_multi_segment() -> None: + """L-shape: (0,0)→(3,0)→(3,4) → 7m total.""" + pts = [(0.0, 0.0), (3.0, 0.0), (3.0, 4.0)] + result = _resample(pts, 1.0) + # Expect points at: (0,0), (1,0), (2,0), (3,0), (3,1), (3,2), (3,3), (3,4) + assert len(result) == 8 + + +def test_resample_s_curve() -> None: + """S-curve fixture with 0.5m step.""" + pts = [ + (0.0, 1.0), (1.0, 1.2), (2.0, 1.8), (3.0, 2.2), + (4.0, 2.0), (5.0, 1.5), (6.0, 1.0), (7.0, 0.8), (8.0, 1.0), + ] + result = _resample(pts, 0.5) + assert len(result) > len(pts) + # First and last must match + assert abs(result[0][0] - pts[0][0]) < 1e-12 + assert abs(result[-1][0] - pts[-1][0]) < 1e-12 + + +def test_resample_empty_or_single() -> None: + assert _resample([], 0.5) == [] + assert _resample([(1.0, 2.0)], 0.5) == [(1.0, 2.0)] + + +def test_resample_negative_step() -> None: + import pytest + with pytest.raises(ValueError, match="positive"): + _resample([(0.0, 0.0), (1.0, 0.0)], -0.1) + + +def test_resample_zero_step() -> None: + import pytest + with pytest.raises(ValueError, match="positive"): + _resample([(0.0, 0.0), (1.0, 0.0)], 0.0)