facett-map3d/src/maneuver.rs
//! **Maneuver model** — the biggest Garmin gap made concrete: turning a projected
//! route polyline into the ordered, discrete **turn-by-turn instructions** a car
//! navigator speaks ("turn left in 200 m onto Main St").
//!
//! Pure + headless-testable, no I/O, no RNG, no GPU: a [`Route`] is a design-plane
//! polyline, and [`maneuvers`] walks its vertices, computes the **signed turn angle**
//! between the incoming and outgoing segment at each one, [`classify_turn`]s it
//! (straight / slight / normal / sharp / u-turn, left or right), measures the
//! along-route **distance** to it, and joins the **road name** it turns onto. The
//! result is an ordered [`ManeuverPlan`]; [`ManeuverPlan::next_maneuver`] resolves the
//! live "what's coming up, and how far" a next-turn banner reads.
//!
//! ## Conventions
//! The **compass bearing** of a segment is `atan2(dx, dz)` (clockwise from +z) — the
//! SAME convention as [`Route::bearing_deg_at`], the [`crate::OrbitCamera`] bearing and
//! the minimap heading, so a maneuver's turn direction agrees with everything else the
//! nav chrome draws. The **signed turn angle** is `outgoing − incoming` normalised to
//! `(−180, 180]`: **positive = a right turn** (the compass heading increases,
//! clockwise), **negative = a left turn**. Distances are metres — the design-unit arc
//! length scaled by the map's `metres_per_design_unit` calibration.
use crate::route::Route;
/// Below this |turn angle| (degrees) a vertex is "keep straight" — no maneuver emitted.
const STRAIGHT_MAX_DEG: f32 = 20.0;
/// `[STRAIGHT_MAX, SLIGHT_MAX)` → a **slight** bear left/right (a gentle fork).
const SLIGHT_MAX_DEG: f32 = 45.0;
/// `[SLIGHT_MAX, NORMAL_MAX)` → a **normal** left/right (a real street-corner turn).
const NORMAL_MAX_DEG: f32 = 135.0;
/// `[NORMAL_MAX, SHARP_MAX)` → a **sharp** left/right; `≥ SHARP_MAX` → a **U-turn**.
const SHARP_MAX_DEG: f32 = 160.0;
/// The classified kind of a route maneuver — the glyph + phrase the banner shows.
/// `Depart`/`Arrive` bookend the route; the rest are the turn family (left/right ×
/// slight/normal/sharp) plus the `UTurn`.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum ManeuverKind {
/// The route start ("head onto …").
Depart,
/// Below the turn threshold — continue straight (never emitted as a maneuver,
/// returned by [`classify_turn`] so a caller can detect "no turn here").
Straight,
/// A gentle bear to the left (a fork), `[20°, 45°)`.
SlightLeft,
/// A normal left turn, `[45°, 135°)`.
Left,
/// A hard left, `[135°, 160°)`.
SharpLeft,
/// A gentle bear to the right (a fork), `[20°, 45°)`.
SlightRight,
/// A normal right turn, `[45°, 135°)`.
Right,
/// A hard right, `[135°, 160°)`.
SharpRight,
/// A ~180° reversal, `≥ 160°`.
UTurn,
/// The route destination.
Arrive,
}
impl ManeuverKind {
/// Whether this kind is an actual **turn instruction** (not the Depart/Arrive
/// bookends and not a straight-through). The red-when-broken oracle a "how many
/// turns does this route have?" assertion counts.
#[must_use]
pub fn is_turn(self) -> bool {
!matches!(self, ManeuverKind::Depart | ManeuverKind::Arrive | ManeuverKind::Straight)
}
/// Whether the turn bends **left** (including slight/sharp). `false` for the
/// bookends, straight, right turns, and the U-turn.
#[must_use]
pub fn is_left(self) -> bool {
matches!(self, ManeuverKind::SlightLeft | ManeuverKind::Left | ManeuverKind::SharpLeft)
}
/// Whether the turn bends **right** (including slight/sharp).
#[must_use]
pub fn is_right(self) -> bool {
matches!(self, ManeuverKind::SlightRight | ManeuverKind::Right | ManeuverKind::SharpRight)
}
/// A single **arrow glyph** for the banner + the on-ribbon marker (the direction
/// the driver turns), from the Unicode arrow block so it needs no icon atlas.
#[must_use]
pub fn glyph(self) -> &'static str {
match self {
ManeuverKind::Depart => "▲",
ManeuverKind::Straight => "↑",
ManeuverKind::SlightLeft => "↖",
ManeuverKind::Left => "↰",
ManeuverKind::SharpLeft => "⮢",
ManeuverKind::SlightRight => "↗",
ManeuverKind::Right => "↱",
ManeuverKind::SharpRight => "⮣",
ManeuverKind::UTurn => "⮐",
ManeuverKind::Arrive => "◉",
}
}
/// The human **instruction phrase** for the banner ("Turn left", "Make a U-turn").
#[must_use]
pub fn instruction(self) -> &'static str {
match self {
ManeuverKind::Depart => "Head out",
ManeuverKind::Straight => "Continue straight",
ManeuverKind::SlightLeft => "Bear left",
ManeuverKind::Left => "Turn left",
ManeuverKind::SharpLeft => "Sharp left",
ManeuverKind::SlightRight => "Bear right",
ManeuverKind::Right => "Turn right",
ManeuverKind::SharpRight => "Sharp right",
ManeuverKind::UTurn => "Make a U-turn",
ManeuverKind::Arrive => "Arrive",
}
}
}
/// **Classify a signed turn angle** (degrees, `outgoing − incoming` normalised to
/// `(−180, 180]`; positive = right, negative = left) into a [`ManeuverKind`]. The one
/// pure decision the whole model rests on — thresholds only, no state.
#[must_use]
pub fn classify_turn(signed_deg: f32) -> ManeuverKind {
let a = signed_deg.abs();
if a < STRAIGHT_MAX_DEG {
return ManeuverKind::Straight;
}
if a >= SHARP_MAX_DEG {
return ManeuverKind::UTurn;
}
let left = signed_deg < 0.0;
if a >= NORMAL_MAX_DEG {
if left { ManeuverKind::SharpLeft } else { ManeuverKind::SharpRight }
} else if a >= SLIGHT_MAX_DEG {
if left { ManeuverKind::Left } else { ManeuverKind::Right }
} else if left {
ManeuverKind::SlightLeft
} else {
ManeuverKind::SlightRight
}
}
/// One discrete **maneuver** along the route: what to do, where, how far along, and
/// onto which road.
#[derive(Clone, Debug, PartialEq)]
pub struct Maneuver {
/// The classified turn (or the Depart/Arrive bookend).
pub kind: ManeuverKind,
/// The design-plane point `(x, y, z)` the maneuver happens at (a route vertex).
pub at_point: [f32; 3],
/// Cumulative distance from the route **start** to this maneuver, in metres.
pub arc_m: f32,
/// Distance from the **previous** maneuver to this one (the leg length), in metres —
/// the "in X m" a Garmin maneuver-list row shows for this step.
pub distance_m: f32,
/// The signed turn angle in degrees (positive = right, negative = left; `0` for the
/// Depart/Arrive bookends).
pub turn_deg: f32,
/// The name of the road this maneuver turns **onto** (empty when unknown).
pub road_name: String,
}
impl Maneuver {
/// The next-turn **banner line** for a live remaining distance (metres): glyph +
/// instruction + the rounded distance (reusing [`crate::garmin::fmt_distance`]) +
/// the road it turns onto. E.g. `"↰ Turn left in 200 m onto Storgatan"`.
#[must_use]
pub fn banner(&self, dist_remaining_m: f32) -> String {
let glyph = self.kind.glyph();
let instr = self.kind.instruction();
match self.kind {
ManeuverKind::Arrive => {
if self.road_name.is_empty() {
format!("{glyph} {instr} in {}", crate::garmin::fmt_distance(dist_remaining_m))
} else {
format!(
"{glyph} {instr} at {} in {}",
self.road_name,
crate::garmin::fmt_distance(dist_remaining_m)
)
}
}
_ => {
let onto = if self.road_name.is_empty() {
String::new()
} else {
format!(" onto {}", self.road_name)
};
format!("{glyph} {instr} in {}{onto}", crate::garmin::fmt_distance(dist_remaining_m))
}
}
}
}
/// The ordered **maneuver plan** for a route: the Depart → turns… → Arrive list plus
/// the total route length (metres). Built by [`maneuvers`]; queried live by
/// [`Self::next_maneuver`].
#[derive(Clone, Debug, PartialEq)]
pub struct ManeuverPlan {
/// The maneuvers in route order (always at least `Depart` + `Arrive` for a real route).
pub maneuvers: Vec<Maneuver>,
/// Total route length in metres.
pub total_m: f32,
}
impl ManeuverPlan {
/// An empty plan (no route).
#[must_use]
pub fn empty() -> Self {
ManeuverPlan { maneuvers: Vec::new(), total_m: 0.0 }
}
/// How many **turn** maneuvers the route has (excludes the Depart/Arrive bookends).
/// The red-when-broken oracle: a straight route ⇒ `0`, an L-route ⇒ `1`.
#[must_use]
pub fn turn_count(&self) -> usize {
self.maneuvers.iter().filter(|m| m.kind.is_turn()).count()
}
/// The **next maneuver** at (or ahead of) arc position `arc_m` metres from the start,
/// skipping the Depart bookend, with the remaining distance to it. `None` past the
/// end. The banner's data source: `(maneuver, dist_remaining)`.
#[must_use]
pub fn next_maneuver(&self, arc_m: f32) -> Option<(&Maneuver, f32)> {
self.maneuvers
.iter()
.filter(|m| !matches!(m.kind, ManeuverKind::Depart))
.find(|m| m.arc_m >= arc_m - 1e-3)
.map(|m| (m, (m.arc_m - arc_m).max(0.0)))
}
/// [`Self::next_maneuver`] addressed by normalised progress `t ∈ [0, 1]` (the
/// [`crate::Map3D`] follow-camera lever) instead of raw metres.
#[must_use]
pub fn next_maneuver_at_t(&self, t: f32) -> Option<(&Maneuver, f32)> {
self.next_maneuver(t.clamp(0.0, 1.0) * self.total_m)
}
}
/// Segment compass bearing (degrees, `[0, 360)`, clockwise from +z) `a → b`, matching
/// [`Route::bearing_deg_at`].
fn seg_bearing(a: [f32; 3], b: [f32; 3]) -> f32 {
let (dx, dz) = (b[0] - a[0], b[2] - a[2]);
dx.atan2(dz).to_degrees().rem_euclid(360.0)
}
/// Normalise a degree delta into `(−180, 180]` — the shortest signed turn.
fn normalize180(deg: f32) -> f32 {
(deg + 180.0).rem_euclid(360.0) - 180.0
}
/// **Derive the maneuver plan** from a projected [`Route`], the per-segment road
/// `names` (`names[k]` = the road of segment `k`, i.e. vertex `k → k+1`; see
/// [`road_names_along`]), and the map's `metres_per_du` calibration (design units →
/// metres). Pure: same route + names ⇒ same plan, so a pinned-clock A/B snapshot is
/// bit-stable.
///
/// Emits a `Depart` at the start, a maneuver at every interior vertex whose turn angle
/// clears the straight threshold (classified + distance-measured + named by the road it
/// turns onto), and an `Arrive` at the destination. A degenerate route (< 2 vertices)
/// yields an [`ManeuverPlan::empty`].
#[must_use]
pub fn maneuvers(route: &Route, names: &[String], metres_per_du: f32) -> ManeuverPlan {
let pts = &route.points;
if route.is_empty() || pts.len() < 2 {
return ManeuverPlan::empty();
}
let scale = metres_per_du.max(0.0);
let n = pts.len();
// Cumulative XZ arc length (design units) at each vertex.
let mut cum = vec![0.0f32; n];
for i in 1..n {
let (dx, dz) = (pts[i][0] - pts[i - 1][0], pts[i][2] - pts[i - 1][2]);
cum[i] = cum[i - 1] + (dx * dx + dz * dz).sqrt();
}
let name_at = |seg: usize| names.get(seg).cloned().unwrap_or_default();
let mut out: Vec<Maneuver> = Vec::new();
let mut last_arc = 0.0f32;
out.push(Maneuver {
kind: ManeuverKind::Depart,
at_point: pts[0],
arc_m: 0.0,
distance_m: 0.0,
turn_deg: 0.0,
road_name: name_at(0),
});
for i in 1..n - 1 {
let incoming = seg_bearing(pts[i - 1], pts[i]);
let outgoing = seg_bearing(pts[i], pts[i + 1]);
let delta = normalize180(outgoing - incoming);
let kind = classify_turn(delta);
if !kind.is_turn() {
continue; // keep-straight vertex — no instruction
}
let arc = cum[i] * scale;
out.push(Maneuver {
kind,
at_point: pts[i],
arc_m: arc,
distance_m: arc - last_arc,
turn_deg: delta,
// The road TURNED ONTO is the outgoing segment `i`.
road_name: name_at(i),
});
last_arc = arc;
}
let arc = cum[n - 1] * scale;
out.push(Maneuver {
kind: ManeuverKind::Arrive,
at_point: pts[n - 1],
arc_m: arc,
distance_m: arc - last_arc,
turn_deg: 0.0,
// The final road (last segment) is the destination street.
road_name: name_at(n.saturating_sub(2)),
});
ManeuverPlan { maneuvers: out, total_m: route.length * scale }
}
/// **Join road names to a route** by nearest [`crate::extrude::StreetLabelAnchor`]:
/// returns one name per route **segment** (`out[k]` = the road segment `k` runs along),
/// picked as the nearest named-road label anchor to the segment midpoint (XZ). The
/// design's "join to street_label_anchors" seam — a fallback for when the routing data
/// carries geometry only. Empty string when no anchor is near / none exist. Pure.
#[must_use]
pub fn road_names_along(route: &Route, anchors: &[crate::extrude::StreetLabelAnchor]) -> Vec<String> {
let pts = &route.points;
if pts.len() < 2 {
return Vec::new();
}
(0..pts.len() - 1)
.map(|k| {
let mx = (pts[k][0] + pts[k + 1][0]) * 0.5;
let mz = (pts[k][2] + pts[k + 1][2]) * 0.5;
anchors
.iter()
.map(|a| {
let dx = a.pos[0] - mx;
let dz = a.pos[2] - mz;
(dx * dx + dz * dz, a.text.as_str())
})
.min_by(|l, r| l.0.partial_cmp(&r.0).unwrap_or(std::cmp::Ordering::Equal))
.map(|(_, name)| name.to_string())
.unwrap_or_default()
})
.collect()
}
#[cfg(test)]
mod tests {
use super::*;
fn names(n: usize) -> Vec<String> {
(0..n).map(|i| format!("Road {i}")).collect()
}
/// East then NORTH (+x then +z): a left turn (compass heading 90° → 0°, delta −90°).
fn left_l_route() -> Route {
Route::from_design_points(vec![[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [2.0, 0.0, 2.0]])
}
/// East then SOUTH (+x then −z): a right turn (compass 90° → 180°, delta +90°).
fn right_l_route() -> Route {
Route::from_design_points(vec![[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [2.0, 0.0, -2.0]])
}
/// A straight run (three colinear vertices): no turn at the middle.
fn straight_route() -> Route {
Route::from_design_points(vec![[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [3.0, 0.0, 0.0]])
}
/// East then almost-back-west: a hairpin/U-turn (~180°).
fn hairpin_route() -> Route {
Route::from_design_points(vec![[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [0.0, 0.0, 0.05]])
}
#[test]
fn classify_covers_every_band_left_and_right() {
assert_eq!(classify_turn(0.0), ManeuverKind::Straight);
assert_eq!(classify_turn(10.0), ManeuverKind::Straight);
assert_eq!(classify_turn(-30.0), ManeuverKind::SlightLeft);
assert_eq!(classify_turn(30.0), ManeuverKind::SlightRight);
assert_eq!(classify_turn(-90.0), ManeuverKind::Left);
assert_eq!(classify_turn(90.0), ManeuverKind::Right);
assert_eq!(classify_turn(-150.0), ManeuverKind::SharpLeft);
assert_eq!(classify_turn(150.0), ManeuverKind::SharpRight);
assert_eq!(classify_turn(180.0), ManeuverKind::UTurn);
assert_eq!(classify_turn(-175.0), ManeuverKind::UTurn);
}
#[test]
fn l_route_yields_one_left_turn_at_the_corner_with_the_right_distance() {
let r = left_l_route();
let plan = maneuvers(&r, &names(2), 1.0); // metres_per_du = 1 → distances in design units
assert_eq!(plan.turn_count(), 1, "an L-route has exactly one turn");
let turn = plan.maneuvers.iter().find(|m| m.kind.is_turn()).expect("a turn maneuver");
assert_eq!(turn.kind, ManeuverKind::Left, "east→north is a LEFT turn (got {:?})", turn.kind);
// The corner sits 2 units (the first leg) along the route.
assert!((turn.arc_m - 2.0).abs() < 1e-3, "the turn is 2 m along the route (got {})", turn.arc_m);
assert!((turn.distance_m - 2.0).abs() < 1e-3, "the leg to the turn is 2 m (got {})", turn.distance_m);
// It turns ONTO the outgoing segment's road (segment index 1).
assert_eq!(turn.road_name, "Road 1", "the turn names the road it turns onto");
// Bookended by Depart + Arrive.
assert_eq!(plan.maneuvers.first().unwrap().kind, ManeuverKind::Depart);
assert_eq!(plan.maneuvers.last().unwrap().kind, ManeuverKind::Arrive);
}
#[test]
fn right_l_route_yields_a_right_turn() {
let plan = maneuvers(&right_l_route(), &names(2), 1.0);
let turn = plan.maneuvers.iter().find(|m| m.kind.is_turn()).expect("a turn");
assert_eq!(turn.kind, ManeuverKind::Right, "east→south is a RIGHT turn (got {:?})", turn.kind);
assert!(turn.turn_deg > 0.0, "a right turn has a positive signed angle");
}
#[test]
fn straight_route_yields_no_turns() {
let plan = maneuvers(&straight_route(), &names(2), 1.0);
assert_eq!(plan.turn_count(), 0, "a straight route has no turn maneuvers");
// Still bookended (a route you drive start→end, just no turns).
assert_eq!(plan.maneuvers.len(), 2, "Depart + Arrive only");
}
#[test]
fn hairpin_yields_a_u_turn() {
let plan = maneuvers(&hairpin_route(), &names(2), 1.0);
let turn = plan.maneuvers.iter().find(|m| m.kind.is_turn()).expect("a turn");
assert_eq!(turn.kind, ManeuverKind::UTurn, "a hairpin is a U-turn (got {:?})", turn.kind);
}
#[test]
fn next_maneuver_tracks_remaining_distance_and_banner_reads() {
let plan = maneuvers(&left_l_route(), &names(2), 1.0);
// Just after the start, the next maneuver is the corner, 2 m out (minus a hair).
let (m, rem) = plan.next_maneuver(0.5).expect("a maneuver ahead");
assert_eq!(m.kind, ManeuverKind::Left);
assert!((rem - 1.5).abs() < 1e-3, "1.5 m remaining to the corner from 0.5 (got {rem})");
let banner = m.banner(rem);
assert!(banner.contains("Turn left"), "banner names the turn: {banner}");
assert!(banner.contains("onto Road 1"), "banner names the road: {banner}");
// Past the corner, the next (and last) maneuver is Arrive.
let (m2, _) = plan.next_maneuver(3.0).expect("arrive is still ahead at 3 m");
assert_eq!(m2.kind, ManeuverKind::Arrive);
// Past the destination there is nothing ahead.
assert!(plan.next_maneuver(plan.total_m + 1.0).is_none(), "nothing past the destination");
}
#[test]
fn metres_scale_applies() {
// metres_per_du = 10 → the 2-unit first leg reads as 20 m.
let plan = maneuvers(&left_l_route(), &names(2), 10.0);
let turn = plan.maneuvers.iter().find(|m| m.kind.is_turn()).unwrap();
assert!((turn.arc_m - 20.0).abs() < 1e-2, "scaled to 20 m (got {})", turn.arc_m);
assert!((plan.total_m - 40.0).abs() < 1e-2, "total 4 units × 10 = 40 m");
}
#[test]
fn degenerate_routes_are_empty_and_never_panic() {
let empty = maneuvers(&Route::from_design_points(vec![]), &[], 1.0);
assert!(empty.maneuvers.is_empty() && empty.turn_count() == 0);
assert!(empty.next_maneuver(0.0).is_none());
let single = maneuvers(&Route::from_design_points(vec![[1.0, 0.0, 2.0]]), &[], 1.0);
assert!(single.maneuvers.is_empty(), "a one-point route has no maneuvers");
}
}