Reuse symmetric costs in optimize_order instead of running reverse A* searches

xrspatial/pathfinding.py

@@ -1330,23 +1330,34 @@ def _optimize_waypoint_order(surface, waypoints, barriers, x, y,
     INF = float('inf')
     dist = [[INF] * n for _ in range(n)]
 
+    def _pair_cost(a, b):
+        seg = a_star_search(
+            surface, waypoints[a], waypoints[b],
+            barriers=barriers, x=x, y=y,
+            connectivity=connectivity,
+            snap_start=snap, snap_goal=snap,
+            friction=friction, search_radius=search_radius,
+        )
+        seg_vals = _segment_to_numpy(seg.data)
+        goal_py, goal_px = _get_pixel_id(waypoints[b], surface, x, y)
+        goal_cost = seg_vals[goal_py, goal_px]
+        return float(goal_cost) if np.isfinite(goal_cost) else INF
+
     for i in range(n):
-        for j in range(n):
-            if i == j:
-                dist[i][j] = 0.0
-                continue
-            seg = a_star_search(
-                surface, waypoints[i], waypoints[j],
-                barriers=barriers, x=x, y=y,
-                connectivity=connectivity,
-                snap_start=snap, snap_goal=snap,
-                friction=friction, search_radius=search_radius,
-            )
-            seg_vals = _segment_to_numpy(seg.data)
-            goal_py, goal_px = _get_pixel_id(waypoints[j], surface, x, y)
-            goal_cost = seg_vals[goal_py, goal_px]
-            if np.isfinite(goal_cost):
-                dist[i][j] = goal_cost
+        dist[i][i] = 0.0
+        for j in range(i + 1, n):
+            dist[i][j] = _pair_cost(i, j)
+            if snap:
+                # Snapping can move the requested start and goal pixels,
+                # so the reverse direction may read its cost at a
+                # different pixel; compute it explicitly.
+                dist[j][i] = _pair_cost(j, i)
+            else:
+                # The pixel graph is undirected and edge costs use the
+                # mean friction of both endpoints, so the cost matrix is
+                # symmetric: reuse cost(i -> j) as cost(j -> i) instead
+                # of running the reverse A* search.
+                dist[j][i] = dist[i][j]
 
     # Fixed endpoints: first=0, last=n-1
     if n <= 12:

xrspatial/tests/test_pathfinding.py

@@ -1062,6 +1062,83 @@ def test_optimize_order_preserves_endpoints():
     assert tuple(order[-1]) == wp3
 
 
+def test_optimize_order_symmetric_matrix_call_count():
+    """Cost matrix reuses symmetric costs: N(N-1)/2 A* runs, not N(N-1).
+
+    Issue #3661: the pixel graph is undirected and edge costs use the
+    mean friction of both endpoints, so cost(i->j) == cost(j->i) and
+    the reverse searches are redundant when snap is off.
+    """
+    import xrspatial.pathfinding as pf
+
+    data = np.ones((10, 10))
+    agg = _make_raster(data)
+
+    wps = [(9.0, 0.0), (5.0, 5.0), (5.0, 0.0), (9.0, 9.0), (0.0, 9.0)]
+    n = len(wps)
+
+    calls = []
+    orig = pf.a_star_search
+
+    def counting(*args, **kwargs):
+        calls.append(1)
+        return orig(*args, **kwargs)
+
+    from unittest.mock import patch
+    with patch.object(pf, 'a_star_search', side_effect=counting):
+        order = pf._optimize_waypoint_order(
+            agg, wps, [], 'x', 'y', 8, False, None, None)
+
+    assert len(calls) == n * (n - 1) // 2
+    # endpoints stay fixed and all waypoints are kept
+    assert order[0] == wps[0]
+    assert order[-1] == wps[-1]
+    assert sorted(order) == sorted(wps)
+
+
+@pytest.mark.filterwarnings("ignore:Start at a non crossable location:Warning")
+@pytest.mark.filterwarnings("ignore:End at a non crossable location:Warning")
+def test_optimize_order_snap_keeps_both_directions():
+    """With snap=True the reverse searches still run (snapping can move
+    the requested pixels, so symmetry is not guaranteed).
+
+    The middle waypoint sits on a NaN cell so snapping really relocates
+    it: the direction ending at that waypoint reads its cost at the
+    requested (NaN) pixel while the reverse starts from the snapped
+    pixel, which is the asymmetry the double computation protects.
+    """
+    import xrspatial.pathfinding as pf
+
+    data = np.ones((10, 10))
+    data[5, 5] = np.nan  # snap target: waypoint below lands here
+    agg = _make_raster(data)
+
+    # coords: y=[9..0], x=[0..9]; (4.0, 5.0) maps to pixel (5, 5)
+    wps = [(9.0, 0.0), (4.0, 5.0), (0.0, 9.0)]
+    n = len(wps)
+
+    calls = []
+    orig = pf.a_star_search
+
+    def counting(*args, **kwargs):
+        calls.append(1)
+        return orig(*args, **kwargs)
+
+    from unittest.mock import patch
+    with patch.object(pf, 'a_star_search', side_effect=counting):
+        order = pf._optimize_waypoint_order(
+            agg, wps, [], 'x', 'y', 8, True, None, None)
+
+    assert len(calls) == n * (n - 1)
+    assert order[0] == wps[0]
+    assert order[-1] == wps[-1]
+    # Note: the NaN waypoint may be missing from the returned order.
+    # That is pre-existing behavior tracked in issue #3646 (waypoints
+    # silently dropped when the matrix has no finite tour), not part
+    # of the call-count contract under test here.
+    assert set(order) <= set(wps)
+
+
 # --- Cross-backend tests ---
 
 @pytest.mark.skipif(not has_dask_array(), reason="Requires dask.Array")