feat: add lappa path resample CLI with --step-m (#52)

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())

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"]

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

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"
+}

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]

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)

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