facett-map3d/src/nav.rs
//! **Nav pose model** — the pure, headless-testable levers behind the two remaining
//! Garmin camera behaviours: an **auto-zoom/tilt by speed** ([`chase_pose_for_speed`])
//! and a **2D↔3D nav-mode pose hand-off** ([`NavMode`] + [`NavPose::ease`], design
//! seam #9).
//!
//! No egui, no GPU, no state: each is a plain function of numbers, so the "faster ⇒
//! zoom out + flatten" property and the "overview ⇄ chase eases smoothly" property are
//! provable without a framebuffer, and [`crate::Map3D`] just writes the resulting
//! [`crate::OrbitCamera`] goals (bearing / tilt / distance) — no new renderer, the
//! existing damper does the motion.
use crate::camera::OrbitCamera;
use crate::route::Route;
/// How far ahead (**seconds of travel** at the current follow speed) the predictive
/// chase camera aims — the look-ahead horizon that makes the camera *anticipate* a bend
/// (swinging into it before the vehicle reaches the vertex) instead of snapping at the
/// corner. At `speed == 0` the horizon collapses onto the current position, so a stopped
/// camera simply holds the on-route heading.
pub const LOOKAHEAD_SECONDS: f32 = 2.0;
/// The follow speed (route progress `t` per second) that maps to **fully zoomed out** —
/// at/above it the chase camera holds its farthest, flattest pose. Below it the pose
/// eases in proportionally toward the close, steep at-rest pose.
pub const CRUISE_SPEED: f32 = 0.12;
/// How much farther (multiple of the base chase distance) the camera dollies out at
/// cruise speed — "see further ahead when you're moving fast".
pub const SPEED_DIST_GAIN: f32 = 1.6;
/// How many extra degrees of tilt (toward the horizon — flatter) the camera adds at
/// cruise speed. Capped by [`MAX_CHASE_TILT_DEG`].
pub const SPEED_TILT_GAIN_DEG: f32 = 13.0;
/// The flattest chase tilt the auto-zoom will reach (never look fully at the horizon).
pub const MAX_CHASE_TILT_DEG: f32 = 80.0;
/// **Auto-zoom / tilt by speed.** Given the current follow `speed` (route `t`/sec) and
/// the at-rest base `(distance, tilt_deg)` chase pose, return the `(distance, tilt_deg)`
/// the camera should hold: **faster ⇒ dolly out** (bigger distance) **and flatten**
/// (bigger tilt toward the horizon), so the driver sees further down the road; slower /
/// stopped ⇒ back to the close, steep pose. Monotonic + clamped — the red-when-broken
/// property a headless test asserts.
#[must_use]
pub fn chase_pose_for_speed(speed: f32, base_distance: f32, base_tilt_deg: f32) -> (f32, f32) {
let f = (speed / CRUISE_SPEED).clamp(0.0, 1.0);
let distance = base_distance * (1.0 + f * SPEED_DIST_GAIN);
let tilt = (base_tilt_deg + f * SPEED_TILT_GAIN_DEG).min(MAX_CHASE_TILT_DEG);
(distance, tilt)
}
/// **Predictive look-ahead chase goals** — the pure lever behind the anticipating
/// follow camera (design seam F1). Given the follow `speed` (route progress `t`/sec),
/// the live `progress` `t ∈ [0, 1]`, the `route`, and the at-rest chase
/// `(base_distance, base_tilt_deg)`, return the goals the chase camera should ease
/// toward: `(target, bearing_deg, distance, tilt_deg)`.
///
/// The **target** is aimed `speed · LOOKAHEAD_SECONDS` of progress *ahead* along the
/// route (clamped to the destination), so the camera leads into the turn; the
/// **bearing** is derived from the tangent of the look-ahead chord (`target − vehicle`),
/// so it swings toward the outgoing leg *before* the vehicle reaches the corner; the
/// `(distance, tilt)` are the speed-coupled pose from [`chase_pose_for_speed`] (faster ⇒
/// dolly out + flatten). At `speed == 0` the look-ahead point collapses onto the current
/// position: the target **locks on the vehicle** and the bearing falls back to the
/// on-route tangent at `progress` — the red-when-broken property a headless test asserts.
/// Pure: same route + numbers ⇒ same goals (no state, no framebuffer).
#[must_use]
pub fn compute_camera_goals(
speed: f32,
progress: f32,
route: &Route,
base_distance: f32,
base_tilt_deg: f32,
) -> ([f32; 3], f32, f32, f32) {
let t = progress.clamp(0.0, 1.0);
let vehicle = route.point_at(t);
// Scan AHEAD by the look-ahead horizon (in progress units); clamp at the destination.
let ahead_t = (t + speed.max(0.0) * LOOKAHEAD_SECONDS).min(1.0);
let target = route.point_at(ahead_t);
let (dx, dz) = (target[0] - vehicle[0], target[2] - vehicle[2]);
// Degenerate chord (stopped, or already at the destination) ⇒ hold the on-route
// tangent so a locked-on camera still faces down the road, never a NaN heading.
let bearing = if dx * dx + dz * dz > 1e-12 {
dx.atan2(dz).to_degrees().rem_euclid(360.0)
} else {
route.bearing_deg_at(t)
};
let (distance, tilt) = chase_pose_for_speed(speed, base_distance, base_tilt_deg);
(target, bearing, distance, tilt)
}
/// A **camera pose** in car-navigator vocabulary — the three levers a nav mode blends,
/// each a goal written straight onto the [`OrbitCamera`]. Bearing is the compass heading
/// looked along; tilt is degrees from top-down (0 = straight down, 90 = at the horizon);
/// distance is the dolly (design units).
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NavPose {
/// Compass heading (degrees) the camera looks along.
pub bearing_deg: f32,
/// Tilt from top-down (degrees): 0 = overhead, ~62 = a low chase, 90 = horizon.
pub tilt_deg: f32,
/// Dolly distance (design units) — small = close chase, large = high overview.
pub distance: f32,
}
impl NavPose {
/// Read the live pose off an [`OrbitCamera`] (the current hand-off source).
#[must_use]
pub fn from_camera(cam: &OrbitCamera) -> NavPose {
NavPose { bearing_deg: cam.bearing_deg().rem_euclid(360.0), tilt_deg: cam.tilt_deg(), distance: cam.distance }
}
/// **Ease from `self` toward `to`** by `t ∈ [0, 1]` (a smoothstep so the hand-off has
/// no velocity jump at the ends). Bearing takes the **shortest arc** (so a hand-off
/// across 350°→10° swings 20°, not 340°). `t = 0` ⇒ `self`, `t = 1` ⇒ `to`.
#[must_use]
pub fn ease(self, to: NavPose, t: f32) -> NavPose {
let s = smoothstep(t.clamp(0.0, 1.0));
NavPose {
bearing_deg: (self.bearing_deg + shortest_arc(self.bearing_deg, to.bearing_deg) * s).rem_euclid(360.0),
tilt_deg: lerp(self.tilt_deg, to.tilt_deg, s),
distance: lerp(self.distance, to.distance, s),
}
}
/// Write this pose onto an [`OrbitCamera`] as **goals** (the damper eases the live
/// camera toward them) — the one place the nav model touches the camera.
pub fn apply_goals(&self, cam: &mut OrbitCamera) {
cam.set_bearing_deg(self.bearing_deg);
cam.set_tilt_deg(self.tilt_deg);
cam.goal_distance = self.distance;
}
}
/// The **nav view mode** — the overview↔chase spectrum a Garmin flips between: a
/// top-down 2D-style **overview**, a leaned **2.5D** tilt, and the low **3D chase**
/// follow. Each maps to a canonical [`NavPose`] (given a bearing + a scene distance
/// scale); [`NavPose::ease`] hands off between them so the switch is one smooth camera
/// move, not two apps (design seam #9).
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum NavMode {
/// Straight-down 2D map: tilt 0, high above.
Overview,
/// A leaned 2.5D map: a moderate tilt, mid height.
Tilted,
/// The low, forward chase-follow behind the vehicle.
Chase,
}
impl NavMode {
/// The canonical [`NavPose`] for this mode at compass `bearing_deg`, scaled by the
/// scene's chase distance `chase_dist` (the overview/tilted heights are multiples of
/// it so the framing tracks the scene size).
#[must_use]
pub fn pose(self, bearing_deg: f32, chase_dist: f32) -> NavPose {
match self {
NavMode::Overview => NavPose { bearing_deg, tilt_deg: 0.0, distance: chase_dist * 6.0 },
NavMode::Tilted => NavPose { bearing_deg, tilt_deg: 35.0, distance: chase_dist * 2.5 },
NavMode::Chase => {
NavPose { bearing_deg, tilt_deg: OrbitCamera::CHASE_TILT_DEG, distance: chase_dist }
}
}
}
}
/// Shortest signed arc (degrees, `(−180, 180]`) from `a` to `b`.
fn shortest_arc(a: f32, b: f32) -> f32 {
(b - a + 180.0).rem_euclid(360.0) - 180.0
}
fn lerp(a: f32, b: f32, t: f32) -> f32 {
a + (b - a) * t
}
/// Classic cubic smoothstep on `[0, 1]`.
fn smoothstep(t: f32) -> f32 {
t * t * (3.0 - 2.0 * t)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn faster_zooms_out_and_flattens() {
let base_d = 0.55;
let base_t = 62.0;
let (d_slow, t_slow) = chase_pose_for_speed(0.0, base_d, base_t);
let (d_fast, t_fast) = chase_pose_for_speed(CRUISE_SPEED, base_d, base_t);
assert!((d_slow - base_d).abs() < 1e-5, "at rest the pose is the base distance");
assert!((t_slow - base_t).abs() < 1e-5, "at rest the pose is the base tilt");
assert!(d_fast > d_slow, "faster dollies OUT ({d_slow} → {d_fast})");
assert!(t_fast > t_slow, "faster FLATTENS the tilt ({t_slow} → {t_fast})");
// Monotone across the range, and clamped (above cruise doesn't keep growing).
let (d_over, t_over) = chase_pose_for_speed(CRUISE_SPEED * 3.0, base_d, base_t);
assert!((d_over - d_fast).abs() < 1e-4, "distance clamps at cruise speed");
assert!(t_over <= MAX_CHASE_TILT_DEG + 1e-4, "tilt never exceeds the horizon cap");
}
/// L-route: east along +x for 2, then NORTH (+z) for 2 — a left turn (compass 90° →
/// 0°). The corner sits at half progress; total length 4.
fn 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]])
}
#[test]
fn lookahead_locks_on_at_rest_and_scans_past_the_turn_when_fast() {
let r = l_route();
let (base_d, base_t) = (0.55, 62.0);
// Just before the corner, on the east leg (t = 0.4 ⇒ 1.6 units along ⇒ x = 1.6).
let progress = 0.4;
let here = r.point_at(progress);
// ── STOPPED: the look-ahead collapses — target LOCKS on the vehicle, bearing is
// the on-route tangent (due-east 90°), pose is the close base pose. ──
let (tgt0, brg0, d0, t0) = compute_camera_goals(0.0, progress, &r, base_d, base_t);
assert!(
(tgt0[0] - here[0]).abs() < 1e-5 && (tgt0[2] - here[2]).abs() < 1e-5,
"speed 0 locks the target on the current position ({tgt0:?} vs {here:?})"
);
assert!((brg0 - r.bearing_deg_at(progress)).abs() < 1e-3, "stopped faces the on-route tangent");
assert!((brg0 - 90.0).abs() < 1e-3, "the east leg bears due-east 90° (got {brg0})");
assert!((d0 - base_d).abs() < 1e-5 && (t0 - base_t).abs() < 1e-5, "at rest the pose is the base chase");
// ── FAST: the look-ahead scans PAST the corner onto the north leg, so the target
// is beyond the vertex (z > 0) and the bearing ANTICIPATES the left turn —
// swinging from the incoming 90° toward the outgoing 0° before the vehicle
// reaches the corner. ──
let fast = 0.3; // 0.3 t/s · 2.0 s = 0.6 t ahead ⇒ ahead_t = 1.0 ⇒ past the corner
let (tgt1, brg1, d1, _) = compute_camera_goals(fast, progress, &r, base_d, base_t);
assert!(tgt1[2] > 0.5, "fast look-ahead aims PAST the corner onto the north leg (z={})", tgt1[2]);
assert!(brg1 < brg0 - 5.0, "the bearing anticipates the left turn ({brg0}° → {brg1}°, toward 0°)");
assert!(brg1 > -1.0, "the anticipatory bearing stays within the turn (got {brg1})");
assert!(d1 > d0, "faster still dollies the chase OUT ({d0} → {d1})");
}
#[test]
fn navmode_overview_is_top_down_and_far_chase_is_low_and_near() {
let ov = NavMode::Overview.pose(90.0, 0.5);
let ch = NavMode::Chase.pose(90.0, 0.5);
assert_eq!(ov.tilt_deg, 0.0, "overview looks straight down");
assert!(ov.distance > ch.distance, "overview sits far above the chase");
assert!(ch.tilt_deg > 45.0, "chase is a low forward lean");
}
#[test]
fn ease_hands_off_smoothly_and_takes_the_short_way_round() {
let a = NavMode::Overview.pose(350.0, 0.5);
let b = NavMode::Chase.pose(10.0, 0.5);
assert_eq!(a.ease(b, 0.0), a, "t=0 is the source pose");
assert_eq!(a.ease(b, 1.0).tilt_deg, b.tilt_deg, "t=1 reaches the target tilt");
// Halfway the tilt is between the two (monotone hand-off).
let mid = a.ease(b, 0.5);
assert!(mid.tilt_deg > a.tilt_deg && mid.tilt_deg < b.tilt_deg, "tilt eases between");
// Bearing takes the SHORT arc 350°→10° (through 0°), staying near the ends, not
// swinging back through 180°.
let bmid = mid.bearing_deg;
assert!(bmid > 340.0 || bmid < 20.0, "shortest-arc bearing stays near 0° (got {bmid})");
}
}