use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};
use crate::coords::Point;
use crate::network::{Direction, Lane, LaneId, LayerPolicy, RoadNetwork};
const ROUTE_STEP: f32 = 2.0;
const LANE_CHANGE_COST: f32 = 30.0;
const LANE_CHANGE_DIST: f32 = 10.0;
impl RoadNetwork {
pub fn route(&self, from: Point, to: Point) -> Option<Vec<Point>> {
self.route_with(from, to, LayerPolicy::default())
}
pub fn route_with(&self, from: Point, to: Point, policy: LayerPolicy) -> Option<Vec<Point>> {
let (start, start_proj) = self.nearest_lane_with(from, policy)?;
let (goal, goal_proj) = self.nearest_lane_with(to, policy)?;
let path = self.lane_path((start, start_proj.s), (goal, goal_proj.s), policy)?;
Some(self.sample_route(&path, goal_proj.s))
}
fn lane_path(
&self,
(start, start_s): (LaneId, f32),
(goal, goal_s): (LaneId, f32),
policy: LayerPolicy,
) -> Option<Vec<(LaneId, Entry, f32)>> {
let goal_lane = self.lane(goal)?;
if start == goal && ahead(goal_lane.direction, start_s, goal_s) {
return Some(vec![(start, Entry::Change, start_s)]);
}
let mut search = Search::default();
search.add(
self,
Label {
lane: start,
entry: Entry::Change,
at: start_s,
stops: false,
cost: 0.0,
prev: None,
},
);
while let Some(State { label, .. }) = search.heap.pop() {
if search.dropped[label] {
continue;
}
if search.labels[label].stops {
return Some(search.path(label));
}
for mut next in self.edges(&search.labels[label], policy) {
next.prev = Some(label);
if next.lane == goal && ahead(goal_lane.direction, next.at, goal_s) {
search.add(
self,
Label {
stops: true,
cost: next.cost + (goal_s - next.at).abs(),
..next
},
);
}
search.add(self, next);
}
}
None
}
fn edges(&self, from: &Label, policy: LayerPolicy) -> Vec<Label> {
let Some(l) = self.lane(from.lane) else {
return Vec::new();
};
let len = l.center.length();
let label = |lane, entry, at, cost| Label {
lane,
entry,
at,
stops: false,
cost,
prev: None,
};
let leaves_at_end = |n: &LaneId| match (l.direction, from.entry) {
(Direction::Forward, _) => true,
(Direction::Backward, _) => false,
(Direction::Both, Entry::Start) => from.at < len / 2.0,
(Direction::Both, Entry::Change) => self.nearer_end(l, Some(n)),
};
let successors = l.successors.iter().filter_map(|&n| {
let next = self.lane(n)?;
let at_end = leaves_at_end(&n);
if l.direction == Direction::Both && at_end != self.nearer_end(l, Some(&n)) {
return None;
}
let at = match next.direction {
Direction::Forward => 0.0,
Direction::Backward => next.center.length(),
Direction::Both => {
let exit = l.center.point_at(if at_end { len } else { 0.0 });
let points = next.center.points();
let first = points[0].distance_squared_to(exit);
let last = points[points.len() - 1].distance_squared_to(exit);
if first <= last {
0.0
} else {
next.center.length()
}
}
};
let driven = (if at_end { len } else { 0.0 } - from.at).abs();
Some(label(n, Entry::Start, at, from.cost + driven))
});
let changes = l.neighbors.iter().filter_map(|&n| {
let next = self.lane(n)?;
let forward = match l.direction {
Direction::Forward => true,
Direction::Backward => false,
Direction::Both => self.nearer_end(l, Some(&n)),
};
let travel_end = if forward { len } else { 0.0 };
let exit = advance(from.at, travel_end, LANE_CHANGE_DIST);
let at = next.center.project(l.center.point_at(exit)).s;
let driven = (exit - from.at).abs();
Some(label(
n,
Entry::Change,
at,
from.cost + driven + LANE_CHANGE_COST,
))
});
successors
.chain(changes)
.filter(|next| self.lane(next.lane).is_some_and(|n| policy.admits(n)))
.collect()
}
fn sample_route(&self, path: &[(LaneId, Entry, f32)], goal_s: f32) -> Vec<Point> {
let last = path.len().saturating_sub(1);
let mut pts: Vec<Point> = Vec::new();
for (i, &(lid, how, at)) in path.iter().enumerate() {
let Some(lane) = self.lane(lid) else { continue };
let next = path.get(i + 1);
let len = lane.center.length();
let entry = at.clamp(0.0, len);
let forward = match (lane.direction, how) {
(Direction::Forward, _) => true,
(Direction::Backward, _) => false,
(Direction::Both, Entry::Start) => entry < len / 2.0,
(Direction::Both, Entry::Change) => self.nearer_end(lane, next.map(|n| &n.0)),
};
let travel_end = if forward { len } else { 0.0 };
let exit = if i == last {
let goal = goal_s.clamp(0.0, len);
if ahead(lane.direction, entry, goal) {
goal
} else {
entry
}
} else if next.is_some_and(|n| n.1 == Entry::Change) {
advance(entry, travel_end, LANE_CHANGE_DIST)
} else {
travel_end
};
sample_segment(lane, entry, exit, &mut pts);
}
pts
}
}
impl RoadNetwork {
fn nearer_end(&self, lane: &Lane, next: Option<&LaneId>) -> bool {
let Some(next) = next.and_then(|&id| self.lane(id)) else {
return true;
};
let points = lane.center.points();
let away = |p: Point| (next.center.project(p).point - p).length_squared();
away(points[points.len() - 1]) <= away(points[0])
}
}
fn ahead(direction: Direction, from: f32, to: f32) -> bool {
match direction {
Direction::Forward => to >= from,
Direction::Backward => to <= from,
Direction::Both => true,
}
}
fn advance(from: f32, toward: f32, dist: f32) -> f32 {
let step = (toward - from).signum() * dist;
(from + step).clamp(from.min(toward), from.max(toward))
}
fn sample_segment(lane: &Lane, entry: f32, exit: f32, pts: &mut Vec<Point>) {
let span = exit - entry;
if span.abs() < 1e-4 {
push_dedup(pts, lane.center.point_at(entry));
return;
}
let steps = (span.abs() / ROUTE_STEP).ceil().max(1.0) as usize;
for k in 0..=steps {
let s = entry + span * (k as f32 / steps as f32);
push_dedup(pts, lane.center.point_at(s));
}
}
fn push_dedup(pts: &mut Vec<Point>, p: Point) {
if pts.last().is_none_or(|q| q.distance_squared_to(p) > 1e-6) {
pts.push(p);
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, PartialOrd, Ord)]
enum Entry {
Start,
Change,
}
#[derive(Clone, Copy)]
struct Label {
lane: LaneId,
entry: Entry,
at: f32,
stops: bool,
cost: f32,
prev: Option<usize>,
}
const MAX_LABELS: usize = 8;
#[derive(Default)]
struct Search {
labels: Vec<Label>,
dropped: Vec<bool>,
kept: HashMap<(LaneId, Entry, bool), Vec<usize>>,
heap: BinaryHeap<State>,
}
impl Search {
fn add(&mut self, net: &RoadNetwork, label: Label) {
let Some(lane) = net.lane(label.lane) else {
return;
};
let beats = |a: &Label, b: &Label| a.cost <= b.cost && ahead(lane.direction, a.at, b.at);
let kept = self
.kept
.entry((label.lane, label.entry, label.stops))
.or_default();
if kept.iter().any(|&k| beats(&self.labels[k], &label)) {
return;
}
kept.retain(|&k| {
let beaten = beats(&label, &self.labels[k]);
self.dropped[k] |= beaten;
!beaten
});
if kept.len() >= MAX_LABELS {
return;
}
let id = self.labels.len();
kept.push(id);
self.labels.push(label);
self.dropped.push(false);
self.heap.push(State {
cost: label.cost,
label: id,
key: (label.lane.0, label.entry, label.stops, id),
});
}
fn path(&self, label: usize) -> Vec<(LaneId, Entry, f32)> {
let mut path = Vec::new();
let mut cur = Some(label);
while let Some(k) = cur {
let l = &self.labels[k];
path.push((l.lane, l.entry, l.at));
cur = l.prev;
}
path.reverse();
path
}
}
struct State {
cost: f32,
label: usize,
key: (usize, Entry, bool, usize),
}
impl PartialEq for State {
fn eq(&self, o: &Self) -> bool {
self.cmp(o) == Ordering::Equal
}
}
impl Eq for State {}
impl Ord for State {
fn cmp(&self, o: &Self) -> Ordering {
o.cost
.total_cmp(&self.cost)
.then_with(|| o.key.cmp(&self.key))
}
}
impl PartialOrd for State {
fn partial_cmp(&self, o: &Self) -> Option<Ordering> {
Some(self.cmp(o))
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geometry::Polyline;
use crate::network::LaneType;
fn lane(id: usize, pts: &[[f32; 3]], dir: Direction, succ: &[usize], nbrs: &[usize]) -> Lane {
Lane {
id: LaneId(id),
kind: LaneType::Driving,
layer: crate::Layer::Permanent,
direction: dir,
center: Polyline::new(pts.iter().map(|p| Point::from_array(*p)).collect()),
width: 3.5,
widths: Vec::new(),
bank: Vec::new(),
successors: succ.iter().map(|&s| LaneId(s)).collect(),
predecessors: Vec::new(),
neighbors: nbrs.iter().map(|&s| LaneId(s)).collect(),
}
}
#[test]
fn a_route_costs_what_it_drives_not_the_whole_lane_it_starts_on() {
let net = RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [1000.0, 0.0, 0.0]],
Direction::Forward,
&[1],
&[2],
),
lane(
1,
&[[1000.0, 0.0, 0.0], [1010.0, 0.0, 0.0]],
Direction::Forward,
&[3],
&[],
),
lane(
2,
&[[980.0, 3.5, 0.0], [1000.0, 3.5, 0.0]],
Direction::Forward,
&[3],
&[],
),
lane(
3,
&[[1010.0, 0.0, 0.0], [1100.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
]);
let path = net
.route(Point::new(990.0, 0.0, 0.0), Point::new(1050.0, 0.0, 0.0))
.expect("route");
assert!(
path.iter().all(|p| p.y.abs() < 1.0),
"stays in lane rather than changing for a shorter lane: {path:?}"
);
}
#[test]
fn same_lane_route() {
let net = RoadNetwork::new(vec![lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
)]);
let path = net
.route(Point::new(2.0, 0.0, 0.0), Point::new(15.0, 0.0, 0.0))
.expect("route");
assert!(path.first().unwrap().x < 4.0, "starts near x=2");
assert!(path.last().unwrap().x > 13.0, "ends near x=15");
for w in path.windows(2) {
assert!(w[1].x >= w[0].x - 0.01, "monotonic forward");
}
}
#[test]
fn two_lane_successor_route() {
let net = RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[1],
&[],
),
lane(
1,
&[[20.0, 0.0, 0.0], [40.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
]);
let path = net
.route(Point::new(2.0, 0.0, 0.0), Point::new(38.0, 0.0, 0.0))
.expect("route");
assert!(path.first().unwrap().x < 4.0);
assert!(path.last().unwrap().x > 36.0);
assert!(path.iter().any(|p| p.x > 25.0), "crosses into lane B");
}
fn side_by_side() -> RoadNetwork {
let a = [[0.0, 0.0, 0.0], [100.0, 0.0, 0.0]];
let b = [[0.0, -3.5, 0.0], [100.0, -3.5, 0.0]];
RoadNetwork::new(vec![
lane(0, &a, Direction::Forward, &[], &[1]),
lane(1, &b, Direction::Forward, &[], &[0]),
])
}
#[test]
fn a_lane_change_that_lands_past_the_goal_does_not_reach_it() {
let net = side_by_side();
let from = Point::new(50.0, 0.0, 0.0);
assert_eq!(net.route(from, Point::new(40.0, 0.0, 0.0)), None);
assert_eq!(net.route(from, Point::new(55.0, -3.5, 0.0)), None);
let ahead = net.route(from, Point::new(80.0, -3.5, 0.0)).expect("route");
let end = *ahead.last().unwrap();
assert!(
(end.x - 80.0).abs() < 0.01 && (end.y + 3.5).abs() < 0.01,
"{end:?}"
);
}
fn loop_back(y: f32) -> Vec<[f32; 3]> {
vec![
[100.0, y, 0.0],
[100.0, y + 50.0, 0.0],
[0.0, y + 50.0, 0.0],
[0.0, y, 0.0],
]
}
fn ends_at(route: Option<Vec<Point>>, x: f32, y: f32) {
let end = *route.expect("a route").last().unwrap();
assert!(
(end.x - x).abs() < 0.01 && (end.y - y).abs() < 0.01,
"{end:?}"
);
}
#[test]
fn a_lane_change_past_the_goal_does_not_hide_one_behind_it_round_a_loop() {
let a = [[0.0, 0.0, 0.0], [100.0, 0.0, 0.0]];
let g = [[0.0, -3.5, 0.0], [100.0, -3.5, 0.0]];
let net = RoadNetwork::new(vec![
lane(0, &a, Direction::Forward, &[1], &[2]),
lane(1, &loop_back(0.0), Direction::Forward, &[0], &[]),
lane(2, &g, Direction::Forward, &[], &[0]),
]);
let route = net.route(Point::new(50.0, 0.0, 0.0), Point::new(55.0, -3.5, 0.0));
ends_at(route, 55.0, -3.5);
}
#[test]
fn a_goal_behind_the_start_is_reached_by_changing_back_onto_the_start_lane() {
let a = [[0.0, 0.0, 0.0], [100.0, 0.0, 0.0]];
let g = [[0.0, -3.5, 0.0], [100.0, -3.5, 0.0]];
let net = RoadNetwork::new(vec![
lane(0, &a, Direction::Forward, &[], &[1]),
lane(1, &g, Direction::Forward, &[2], &[0]),
lane(2, &loop_back(-3.5), Direction::Forward, &[1], &[]),
]);
let route = net.route(Point::new(50.0, 0.0, 0.0), Point::new(20.0, 0.0, 0.0));
ends_at(route, 20.0, 0.0);
}
#[test]
fn a_lane_change_entered_past_where_the_next_one_must_start_hides_no_route() {
let at = |y: f32| [[0.0, y, 0.0], [100.0, y, 0.0]];
let net = RoadNetwork::new(vec![
lane(0, &at(0.0), Direction::Forward, &[1], &[2]),
lane(1, &loop_back(0.0), Direction::Forward, &[0], &[]),
lane(2, &at(-3.5), Direction::Forward, &[], &[0, 3]),
lane(3, &at(-7.0), Direction::Forward, &[], &[2]),
]);
let route = net.route(Point::new(85.0, 0.0, 0.0), Point::new(60.0, -7.0, 0.0));
ends_at(route, 60.0, -7.0);
}
#[test]
fn a_goal_behind_on_a_one_way_lane_is_reached_round_the_loop_or_not_at_all() {
let straight = lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
);
let net = RoadNetwork::new(vec![straight.clone()]);
let (from, to) = (Point::new(15.0, 0.0, 0.0), Point::new(5.0, 0.0, 0.0));
assert_eq!(net.route(from, to), None, "no driving backwards");
let back = lane(
1,
&[
[20.0, 0.0, 0.0],
[20.0, 30.0, 0.0],
[0.0, 30.0, 0.0],
[0.0, 0.0, 0.0],
],
Direction::Forward,
&[0],
&[],
);
let net = RoadNetwork::new(vec![
Lane {
successors: vec![LaneId(1)],
..straight
},
back,
]);
let path = net.route(from, to).expect("round the loop");
assert!(
(path[0].x - 15.0).abs() < 0.1 && path[1].x > path[0].x,
"sets off forward"
);
assert!(path.iter().any(|p| p.y > 29.0), "goes round");
assert!(
(path.last().unwrap().x - 5.0).abs() < 0.1,
"ends at the goal"
);
}
#[test]
fn a_goal_behind_on_a_two_way_lane_is_reached_directly() {
let net = RoadNetwork::new(vec![lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Both,
&[],
&[],
)]);
let path = net
.route(Point::new(15.0, 0.0, 0.0), Point::new(5.0, 0.0, 0.0))
.expect("route");
assert!((path.last().unwrap().x - 5.0).abs() < 0.1);
assert!(path.iter().all(|p| p.x <= 15.01));
}
#[test]
fn no_route_when_disconnected() {
let net = RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
lane(
1,
&[[100.0, 0.0, 0.0], [120.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
]);
assert!(net
.route(Point::new(2.0, 0.0, 0.0), Point::new(110.0, 0.0, 0.0))
.is_none());
}
fn two_components() -> RoadNetwork {
RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[1],
&[],
),
lane(
1,
&[[20.0, 0.0, 0.0], [40.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
lane(
2,
&[[500.0, 0.0, 0.0], [520.0, 0.0, 0.0]],
Direction::Forward,
&[3],
&[],
),
lane(
3,
&[[520.0, 0.0, 0.0], [540.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
])
}
#[test]
fn a_route_between_disconnected_components_is_none() {
let net = two_components();
assert!(net
.route(Point::new(2.0, 0.0, 0.0), Point::new(538.0, 0.0, 0.0))
.is_none());
assert!(net
.route(Point::new(502.0, 0.0, 0.0), Point::new(38.0, 0.0, 0.0))
.is_none());
assert!(net
.route(Point::new(2.0, 0.0, 0.0), Point::new(38.0, 0.0, 0.0))
.is_some());
assert!(net
.route(Point::new(502.0, 0.0, 0.0), Point::new(538.0, 0.0, 0.0))
.is_some());
}
#[test]
fn a_cyclic_lane_graph_terminates() {
let net = RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[1],
&[],
),
lane(
1,
&[[20.0, 0.0, 0.0], [20.0, 0.0, 20.0]],
Direction::Forward,
&[2],
&[],
),
lane(
2,
&[[20.0, 0.0, 20.0], [0.0, 0.0, 0.0]],
Direction::Forward,
&[0],
&[],
),
lane(
3,
&[[100.0, 0.0, 0.0], [120.0, 0.0, 0.0]],
Direction::Forward,
&[0],
&[],
),
]);
assert!(net
.route(Point::new(1.0, 0.0, 0.0), Point::new(20.0, 0.0, 18.0))
.is_some());
assert!(net
.route(Point::new(1.0, 0.0, 0.0), Point::new(19.0, 0.0, 0.0))
.is_some());
assert!(net
.route(Point::new(1.0, 0.0, 0.0), Point::new(118.0, 0.0, 0.0))
.is_none());
}
#[test]
fn routing_the_same_pair_twice_gives_an_identical_path() {
let net = RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[1, 2],
&[],
),
lane(
1,
&[[20.0, 0.0, 0.0], [40.0, 0.0, 5.0]],
Direction::Forward,
&[3],
&[],
),
lane(
2,
&[[20.0, 0.0, 0.0], [40.0, 0.0, -5.0]],
Direction::Forward,
&[3],
&[],
),
lane(
3,
&[[40.0, 0.0, 0.0], [60.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[],
),
]);
let first = net
.route(Point::new(1.0, 0.0, 0.0), Point::new(58.0, 0.0, 0.0))
.expect("route");
for _ in 0..20 {
let again = net
.route(Point::new(1.0, 0.0, 0.0), Point::new(58.0, 0.0, 0.0))
.expect("route");
assert_eq!(first, again, "the same request produced a different path");
}
}
#[test]
fn a_route_never_contains_a_non_finite_point() {
let net = two_components();
let route = net
.route(Point::new(2.0, 0.0, 0.0), Point::new(38.0, 0.0, 0.0))
.expect("route");
assert!(route.iter().all(|p| p.is_finite()));
let far = net.route(
Point::new(1.0e30, 0.0, -1.0e30),
Point::new(-1.0e30, 0.0, 1.0e30),
);
assert!(far.is_none_or(|r| r.iter().all(|p| p.is_finite())));
let demo = crate::fixtures::demo_road();
let (first, last) = (
demo.lanes()[0].center.point_at(0.0),
demo.lanes()[0]
.center
.point_at(demo.lanes()[0].center.length()),
);
if let Some(route) = demo.route(first, last) {
assert!(route.iter().all(|p| p.is_finite()));
}
}
#[test]
fn route_changes_lanes_to_reach_the_exit() {
let net = RoadNetwork::new(vec![
lane(
0,
&[[0.0, 0.0, 0.0], [20.0, 0.0, 0.0]],
Direction::Forward,
&[],
&[1],
),
lane(
1,
&[[0.0, 0.0, -3.5], [20.0, 0.0, -3.5]],
Direction::Forward,
&[2],
&[0],
),
lane(
2,
&[[20.0, 0.0, -3.5], [40.0, 0.0, -3.5]],
Direction::Forward,
&[],
&[],
),
]);
let path = net
.route(Point::new(0.0, 0.0, 0.0), Point::new(38.0, 0.0, -3.5))
.expect("route");
assert!(
path.iter().any(|p| p.z.abs() < 0.5),
"starts on lane A (z~0)"
);
assert!(
path.iter().any(|p| (p.z + 3.5).abs() < 0.5),
"moves onto the z=-3.5 lanes"
);
assert!(path.last().unwrap().x > 36.0, "reaches C");
}
}