Probe bounded parallel lanes after capped penalty comparison

crates/oa-core/src/routing.rs

@@ -759,6 +759,64 @@ impl<'a> RoutingSpace<'a> {
             ys: sorted_coordinates(obstacles.iter().flat_map(|r| [r.top, r.bottom]).collect()),
         }
     }
+
+    /// Experimental bounded lanes around settled other-edge segments. Preserve
+    /// every obstacle coordinate; only new lane coordinates count toward the cap.
+    fn with_lanes(&self, start: Position, end: Position, previous: &[Segment]) -> Self {
+        const EXTRA_LIMIT: usize = 64;
+        let add = |base: &[f32], candidates: Vec<f32>, a: f32, b: f32| {
+            let lo = a.min(b);
+            let hi = a.max(b);
+            let midpoint = f64::from(a) / 2.0 + f64::from(b) / 2.0;
+            let mut extra = sorted_coordinates(
+                candidates
+                    .into_iter()
+                    .filter(|v| {
+                        v.is_finite() && base.binary_search_by(|base| base.total_cmp(v)).is_err()
+                    })
+                    .collect(),
+            );
+            let distance = |v: f32| {
+                if v < lo {
+                    f64::from(lo) - f64::from(v)
+                } else if v > hi {
+                    f64::from(v) - f64::from(hi)
+                } else {
+                    0.0
+                }
+            };
+            extra.sort_by(|a, b| {
+                distance(*a)
+                    .total_cmp(&distance(*b))
+                    .then_with(|| {
+                        (f64::from(*a) - midpoint)
+                            .abs()
+                            .total_cmp(&(f64::from(*b) - midpoint).abs())
+                    })
+                    .then_with(|| a.total_cmp(b))
+            });
+            extra.truncate(EXTRA_LIMIT);
+            sorted_coordinates(base.iter().copied().chain(extra).collect())
+        };
+        let mut xs = Vec::new();
+        let mut ys = Vec::new();
+        for segment in previous {
+            if segment.a == segment.b {
+                continue;
+            }
+            if segment.a.x == segment.b.x {
+                xs.extend([segment.a.x - CLEARANCE, segment.a.x + CLEARANCE]);
+            }
+            if segment.a.y == segment.b.y {
+                ys.extend([segment.a.y - CLEARANCE, segment.a.y + CLEARANCE]);
+            }
+        }
+        Self {
+            obstacles: self.obstacles,
+            xs: add(&self.xs, xs, start.x, end.x),
+            ys: add(&self.ys, ys, start.y, end.y),
+        }
+    }
 }
 
 #[allow(clippy::too_many_arguments)]
@@ -849,13 +907,17 @@ fn route_edge(
                 // Only the final leg has a known continuation. Intermediate
                 // authored pins must not inherit the target's terminal direction.
                 let outgoing = (index == pins.len()).then_some(b.point);
-                let path =
-                    fast_path(start, end, &space, &occupied, incoming, outgoing).or_else(|| {
+                // Both searches see identical lanes. Earlier legs/current
+                // source stubs affect cost as before, but do not generate lanes.
+                let leg_space = space.with_lanes(start, end, previous);
+                let path = fast_path(start, end, &leg_space, &occupied, incoming, outgoing)
+                    .or_else(|| {
                         if !search {
                             return None;
                         }
-                        let (path, hit_limit) =
-                            grid_path(start, end, &space, &occupied, incoming, outgoing, budget);
+                        let (path, hit_limit) = grid_path(
+                            start, end, &leg_space, &occupied, incoming, outgoing, budget,
+                        );
                         exhausted |= hit_limit;
                         path
                     });
@@ -1611,6 +1673,97 @@ impl BoundsBuilder {
 mod terminal_join_tests {
     use super::*;
 
+    #[test]
+    fn lanes_preserve_base_coordinates_and_bound_unique_extras_deterministically() {
+        let base: Vec<_> = (0..80).map(|i| 10_000.0 + i as f32).collect();
+        let space = RoutingSpace {
+            obstacles: &[],
+            xs: base.clone(),
+            ys: base.clone(),
+        };
+        let mut previous = Vec::new();
+        for i in 0..100 {
+            let coordinate = i as f32 * 40.0;
+            previous.push(Segment {
+                a: pos(coordinate, 0.0),
+                b: pos(coordinate, 100.0),
+                count: 1,
+            });
+            previous.push(Segment {
+                a: pos(0.0, coordinate),
+                b: pos(100.0, coordinate),
+                count: 1,
+            });
+        }
+        let lanes = space.with_lanes(pos(0.0, 0.0), pos(100.0, 100.0), &previous);
+        previous.reverse();
+        previous.extend(previous.clone());
+        let reordered = space.with_lanes(pos(0.0, 0.0), pos(100.0, 100.0), &previous);
+        assert_eq!(lanes.xs, reordered.xs);
+        assert_eq!(lanes.ys, reordered.ys);
+        for coordinates in [&lanes.xs, &lanes.ys] {
+            assert_eq!(coordinates.len(), 80 + 64);
+            assert!(base.iter().all(|v| coordinates.contains(v)));
+            assert!(coordinates.windows(2).all(|v| v[0] < v[1]));
+            assert!(coordinates.contains(&12.0));
+            assert!(coordinates.contains(&92.0));
+            assert!(!coordinates.contains(&3972.0));
+        }
+    }
+
+    #[test]
+    fn lanes_rank_interval_then_midpoint_and_skip_nonfinite_values() {
+        let space = RoutingSpace::new(&[]);
+        let previous: Vec<_> = (0..100)
+            .map(|i| Segment {
+                a: pos(i as f32 * 40.0, 0.0),
+                b: pos(i as f32 * 40.0, 100.0),
+                count: 1,
+            })
+            .chain(std::iter::once(Segment {
+                a: pos(f32::INFINITY, 0.0),
+                b: pos(f32::INFINITY, 100.0),
+                count: 1,
+            }))
+            .collect();
+        let lanes = space.with_lanes(pos(0.0, 0.0), pos(4000.0, 0.0), &previous);
+        assert_eq!(lanes.xs.len(), 64);
+        assert!(lanes.xs.contains(&1988.0));
+        assert!(lanes.xs.contains(&2012.0));
+        assert!(!lanes.xs.contains(&12.0));
+        assert!(lanes.xs.iter().all(|v| v.is_finite()));
+        assert!(lanes.ys.is_empty());
+    }
+
+    #[test]
+    fn shared_lane_space_offers_clear_parallel_path_to_fast_and_grid() {
+        let start = pos(0.0, 0.0);
+        let end = pos(200.0, 0.0);
+        let previous = vec![Segment {
+            a: start,
+            b: end,
+            count: 1,
+        }];
+        let base = RoutingSpace::new(&[]);
+        let lanes = base.with_lanes(start, end, &previous);
+        assert_eq!(lanes.ys, vec![-12.0, 12.0]);
+        let fast = fast_path(start, end, &lanes, &previous, None, None).unwrap();
+        assert_eq!(fast.first(), Some(&start));
+        assert_eq!(fast.last(), Some(&end));
+        // Under experimental P=3, direct cost280 loses to length224 + turns48.
+        // No terminal direction is fixed in this private leg-level witness.
+        // Keep the witness valid when parent compares the baseline coefficient.
+        let expected = path_cost(&[start, end], &previous).min(272.0);
+        assert!((path_cost(&fast, &previous) - expected).abs() < EPSILON);
+        let mut budget = VIEW_SEARCH_BUDGET;
+        let (grid, exhausted) = grid_path(start, end, &lanes, &previous, None, None, &mut budget);
+        assert!(!exhausted);
+        let grid = grid.unwrap();
+        assert_eq!(grid.first(), Some(&start));
+        assert_eq!(grid.last(), Some(&end));
+        assert!((path_cost(&grid, &previous) - expected).abs() < EPSILON);
+    }
+
     fn complete_cost(
         path: &[Position],
         before: Position,