facett-map3d-flight/src/camera_fly.rs
//! **Flight camera rig** (FLIGHT-SIM camera) — a deterministic, scriptable camera
//! state machine over the `f64` world frame, the rig that honestly exercises the
//! [`crate::origin`] floating origin. Three modes share one pose:
//!
//! - **Free-fly (primary):** 6DOF noclip — WASD/up-down translate along the camera
//! basis, mouse-look drives yaw + pitch. The foundation rig.
//! - **Cinematic orbit:** swing around a world target at a fixed radius (showcase).
//! - **Flight-stick:** pitch / yaw / **roll** rates + throttle along forward — the
//! sim feel layered on the same pose.
//!
//! The pose lives in `f64` world metres ([`crate::origin::WorldPos`]); the renderer
//! gets it as the camera-relative `f32` eye via the floating origin. Orientation is
//! yaw/pitch/roll (radians); the basis is derived so a unit test can assert the rig
//! moves where the keys say. Pure math, injected `dt` — no clock, no egui, so a
//! scripted robot-UI flythrough is bit-reproducible (FC-7).
use crate::origin::{FloatingOrigin, WorldPos};
/// Which control scheme is driving the shared pose.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum FlyMode {
/// 6DOF noclip — the primary rig.
FreeFly,
/// Orbit a world target (showcase).
Orbit,
/// Pitch/yaw/roll + throttle (sim).
FlightStick,
}
impl FlyMode {
#[must_use]
pub fn name(self) -> &'static str {
match self {
FlyMode::FreeFly => "free_fly",
FlyMode::Orbit => "orbit",
FlyMode::FlightStick => "flight_stick",
}
}
}
/// One frame of control input (all optional axes; zero = no input). Same struct
/// drives every mode — each mode reads the axes that apply to it.
#[derive(Clone, Copy, Debug, Default)]
pub struct FlyInput {
/// Move right(+)/left(−), metres/second (free-fly strafe; stick = none).
pub move_right: f64,
/// Move up(+)/down(−), metres/second (free-fly; stick = none).
pub move_up: f64,
/// Move forward(+)/back(−), metres/second (free-fly) or throttle (stick).
pub move_forward: f64,
/// Yaw rate, radians/second (+ = turn right). Mouse-look x / stick rudder.
pub yaw_rate: f64,
/// Pitch rate, radians/second (+ = nose up). Mouse-look y / stick elevator.
pub pitch_rate: f64,
/// Roll rate, radians/second (+ = roll right). Flight-stick aileron only.
pub roll_rate: f64,
/// Orbit dolly rate (metres/second toward/away from target) in Orbit mode.
pub orbit_dolly: f64,
}
/// The shared camera pose + active mode.
#[derive(Clone, Debug)]
pub struct FlightCamera {
/// Active control scheme.
pub mode: FlyMode,
/// Eye position in the `f64` world frame (Web-Mercator metres, z = altitude).
pub position: WorldPos,
/// Heading about world up (+Y), radians. 0 looks toward +X.
pub yaw: f64,
/// Nose lift, radians. Clamped shy of ±90° in free-fly to avoid gimbal flip.
pub pitch: f64,
/// Bank, radians (flight-stick). 0 in free-fly / orbit.
pub roll: f64,
/// Orbit target (Orbit mode) + the radius is `position.ground_distance(target)`-ish.
pub orbit_target: WorldPos,
/// Orbit radius, metres (Orbit mode).
pub orbit_radius: f64,
}
/// Pitch clamp for free-fly so the up vector never degenerates (≈ ±89.4°).
const PITCH_LIMIT: f64 = std::f64::consts::FRAC_PI_2 - 0.01;
impl FlightCamera {
/// A free-fly camera at a world position, looking along +X, level.
#[must_use]
pub fn free_fly(position: WorldPos) -> Self {
Self {
mode: FlyMode::FreeFly,
position,
yaw: 0.0,
pitch: 0.0,
roll: 0.0,
orbit_target: position,
orbit_radius: 1000.0,
}
}
/// **Forward** basis vector (world frame, unit) from yaw + pitch. Yaw rotates in
/// the world **ground plane `(x, y)`** — easting × northing — and pitch lifts
/// toward **+Z, the altitude axis**.
///
/// FIXED (was a real bug): this used to return `[cos·cp, sin(pitch), sin·cp]`,
/// i.e. it put the vertical component in slot **1**. But slot 1 of a
/// [`WorldPos`] is the Mercator **northing**, not altitude — `WorldPos` is
/// `(easting x, northing y, altitude z)`, as `WorldPos::to_lonlat` (which reads
/// `y` as the northing) and `terrain.rs`'s explicit world→render swap
/// (`[r[0], r[2], r[1]]` → "Y-up render frame") both attest. With the old
/// basis, **climbing moved the aircraft NORTH** (and reported a changed
/// latitude from `to_lonlat`), while turning changed its ALTITUDE. Nothing
/// caught it because every rig test was basis-relative (it compared motion to
/// `forward()` itself, so a consistently-wrong basis passed) and the crate had
/// no `Facet` tying the rig to the world frame. `rig_climbs_in_altitude_not_latitude`
/// below is the world-frame oracle that now pins it.
#[must_use]
pub fn forward(&self) -> [f64; 3] {
let cp = self.pitch.cos();
[self.yaw.cos() * cp, self.yaw.sin() * cp, self.pitch.sin()]
}
/// **Right** basis vector (unit), accounting for roll. With zero roll this is the
/// horizontal perpendicular to forward; roll banks it about the forward axis.
#[must_use]
pub fn right(&self) -> [f64; 3] {
let fwd = self.forward();
// World up is +Z (the altitude axis of `WorldPos`) — see `forward()`.
let world_up = [0.0, 0.0, 1.0];
let r = normalize(cross(fwd, world_up));
if self.roll.abs() < 1e-9 {
return r;
}
// Bank `r` about the forward axis by roll (Rodrigues).
rotate_about(r, fwd, self.roll)
}
/// **Up** basis vector (unit) = right × forward (so it banks with roll too).
#[must_use]
pub fn up(&self) -> [f64; 3] {
normalize(cross(self.right(), self.forward()))
}
/// Switch mode, re-deriving the orbit target from the current pose when entering
/// Orbit (so the swing starts centred on what the camera was looking at).
pub fn set_mode(&mut self, mode: FlyMode) {
if mode == FlyMode::Orbit && self.mode != FlyMode::Orbit {
let f = self.forward();
self.orbit_target = WorldPos::new(
self.position.x + f[0] * self.orbit_radius,
self.position.y + f[1] * self.orbit_radius,
self.position.z + f[2] * self.orbit_radius,
);
}
if mode != FlyMode::FlightStick {
self.roll = 0.0; // only the stick banks
}
self.mode = mode;
}
/// **Integrate** one frame of input over `dt` seconds. Pure: same pose+input+dt
/// → same next pose. Dispatches on the active mode.
pub fn integrate(&mut self, input: &FlyInput, dt: f64) {
let dt = dt.max(0.0);
match self.mode {
FlyMode::FreeFly => self.integrate_free_fly(input, dt),
FlyMode::FlightStick => self.integrate_flight_stick(input, dt),
FlyMode::Orbit => self.integrate_orbit(input, dt),
}
}
fn integrate_free_fly(&mut self, input: &FlyInput, dt: f64) {
self.yaw += input.yaw_rate * dt;
self.pitch = (self.pitch + input.pitch_rate * dt).clamp(-PITCH_LIMIT, PITCH_LIMIT);
self.roll = 0.0;
let f = self.forward();
let r = self.right();
let u = self.up();
let step = |axis: [f64; 3], amt: f64| [axis[0] * amt * dt, axis[1] * amt * dt, axis[2] * amt * dt];
let mr = step(r, input.move_right);
let mu = step(u, input.move_up);
let mf = step(f, input.move_forward);
self.position.x += mr[0] + mu[0] + mf[0];
self.position.y += mr[1] + mu[1] + mf[1];
self.position.z += mr[2] + mu[2] + mf[2];
}
fn integrate_flight_stick(&mut self, input: &FlyInput, dt: f64) {
// Stick rates: yaw (rudder), pitch (elevator), roll (aileron). Throttle is
// move_forward along the (banked) forward axis. No free strafe.
self.yaw += input.yaw_rate * dt;
self.pitch = (self.pitch + input.pitch_rate * dt).clamp(-PITCH_LIMIT, PITCH_LIMIT);
self.roll += input.roll_rate * dt;
let f = self.forward();
let thr = input.move_forward * dt;
self.position.x += f[0] * thr;
self.position.y += f[1] * thr;
self.position.z += f[2] * thr;
}
fn integrate_orbit(&mut self, input: &FlyInput, dt: f64) {
self.yaw += input.yaw_rate * dt;
self.pitch = (self.pitch + input.pitch_rate * dt).clamp(-PITCH_LIMIT + 0.05, PITCH_LIMIT - 0.05);
self.orbit_radius = (self.orbit_radius - input.orbit_dolly * dt).max(1.0);
// Eye sits on a sphere around the target; the pose looks back at it.
let cp = self.pitch.cos();
// Same world convention as `forward()`: vertical is slot 2 (altitude).
let off = [self.yaw.cos() * cp, self.yaw.sin() * cp, self.pitch.sin()];
self.position = WorldPos::new(
self.orbit_target.x - off[0] * self.orbit_radius,
self.orbit_target.y - off[1] * self.orbit_radius,
self.orbit_target.z - off[2] * self.orbit_radius,
);
}
/// The **camera-relative `f32` eye** for the renderer, via the floating origin —
/// always `≈[0,0,0]` when the origin is anchored at the camera, which is the
/// whole point (the camera sits at small coords, geometry is rebased around it).
#[must_use]
pub fn eye_relative(&self, origin: FloatingOrigin) -> [f32; 3] {
origin.rebase(self.position)
}
/// A floating origin **snapped to the camera** — the per-frame origin a renderer
/// would build, so all geometry rebases around the camera (kills jitter).
#[must_use]
pub fn make_origin(&self, grid_m: f64) -> FloatingOrigin {
FloatingOrigin::snapped(self.position, grid_m)
}
}
#[inline]
fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
[a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
}
#[inline]
fn normalize(v: [f64; 3]) -> [f64; 3] {
let l = (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt().max(1e-12);
[v[0] / l, v[1] / l, v[2] / l]
}
/// Rotate `v` about unit axis `k` by `angle` (Rodrigues' formula).
fn rotate_about(v: [f64; 3], k: [f64; 3], angle: f64) -> [f64; 3] {
let (s, c) = angle.sin_cos();
let kv = cross(k, v);
let kd = k[0] * v[0] + k[1] * v[1] + k[2] * v[2];
[
v[0] * c + kv[0] * s + k[0] * kd * (1.0 - c),
v[1] * c + kv[1] * s + k[1] * kd * (1.0 - c),
v[2] * c + kv[2] * s + k[2] * kd * (1.0 - c),
]
}
#[cfg(test)]
mod tests {
use super::*;
fn approx(a: [f64; 3], b: [f64; 3], eps: f64) -> bool {
(0..3).all(|i| (a[i] - b[i]).abs() < eps)
}
/// **INJECT-ASSERT (the basis is orthonormal):** forward/right/up are unit and
/// mutually perpendicular at any pose — the prerequisite for a correct view.
#[test]
fn basis_is_orthonormal_across_poses() {
for &(yaw, pitch, roll) in &[(0.0, 0.0, 0.0), (0.7, 0.3, 0.0), (-1.2, -0.5, 0.6), (2.5, 0.8, -0.9)] {
let mut c = FlightCamera::free_fly(WorldPos::new(0.0, 0.0, 0.0));
c.yaw = yaw;
c.pitch = pitch;
c.roll = roll;
let (f, r, u) = (c.forward(), c.right(), c.up());
let len = |v: [f64; 3]| (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt();
for v in [f, r, u] {
assert!((len(v) - 1.0).abs() < 1e-9, "basis vector unit-length");
}
let d = |a: [f64; 3], b: [f64; 3]| a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
assert!(d(f, r).abs() < 1e-9 && d(f, u).abs() < 1e-9 && d(r, u).abs() < 1e-9, "mutually perpendicular");
}
}
/// **INJECT-ASSERT (free-fly goes where the keys say):** pressing "forward" moves
/// the eye along the forward axis by speed·dt; strafing moves along right; and
/// mouse-look changes heading. Deterministic over a scripted input.
#[test]
fn free_fly_translates_along_its_basis() {
let mut c = FlightCamera::free_fly(WorldPos::new(1000.0, 0.0, 50.0));
c.yaw = 0.0; // looking +X
let f = c.forward();
let start = c.position;
// 1 s of 10 m/s forward.
c.integrate(&FlyInput { move_forward: 10.0, ..Default::default() }, 1.0);
let moved = [c.position.x - start.x, c.position.y - start.y, c.position.z - start.z];
assert!(approx(moved, [f[0] * 10.0, f[1] * 10.0, f[2] * 10.0], 1e-9), "moved 10 m forward");
// Mouse-look right turns the heading.
let yaw0 = c.yaw;
c.integrate(&FlyInput { yaw_rate: 0.5, ..Default::default() }, 1.0);
assert!((c.yaw - (yaw0 + 0.5)).abs() < 1e-12, "mouse-look yaws the camera");
// Determinism: replay from the same state → identical pose.
let mut a = FlightCamera::free_fly(WorldPos::new(0.0, 0.0, 0.0));
let mut b = a.clone();
let inp = FlyInput { move_right: 3.0, pitch_rate: 0.2, ..Default::default() };
a.integrate(&inp, 0.016);
b.integrate(&inp, 0.016);
assert_eq!([a.position.x, a.position.y, a.position.z], [b.position.x, b.position.y, b.position.z]);
}
/// **THE WORLD-FRAME ORACLE (RED-when-broken).** Every other rig test is
/// basis-relative — it compares the motion to `forward()`, so a *consistently
/// wrong* basis passes them all. This one is absolute: it asks the WORLD what
/// happened.
///
/// Climbing must raise the **altitude** (`WorldPos.z`) and leave the latitude
/// alone; yawing must move you across the ground (`x`/`y`) and leave the
/// altitude alone. Under the pre-fix basis (vertical in slot 1 = the Mercator
/// NORTHING) this test fails on both counts: a pure climb reported a changed
/// latitude and an unchanged altitude.
#[test]
fn rig_climbs_in_altitude_not_latitude() {
let start = WorldPos::from_lonlat(17.72, 59.62, 500.0);
let (lon0, lat0) = start.to_lonlat();
let _ = (lon0, lat0);
// (1) A pure vertical climb: nose up 45°, throttle forward.
let mut c = FlightCamera::free_fly(start);
c.pitch = std::f64::consts::FRAC_PI_2; // straight up (within the clamp band)
c.pitch = c.pitch.min(PITCH_LIMIT);
c.integrate(&FlyInput { move_forward: 100.0, ..Default::default() }, 1.0);
assert!(
c.position.z - start.z > 99.0,
"flying straight up must GAIN ALTITUDE (z {} → {})",
start.z,
c.position.z
);
// At the 89.4° pitch clamp a 100 m step still leaks ~1 m of ground travel
// (cos of the clamp); the contract is that essentially ALL of it is
// altitude. Under the pre-fix basis this was reversed: ~100 m of NORTHING
// and ~1 m of altitude.
let ground_moved = c.position.ground_distance(start);
assert!(
ground_moved < 2.0,
"flying straight up must not move you over the ground (moved {ground_moved:.3} m)"
);
// (2) A pure level cruise: nose level, throttle forward. Altitude is held.
let mut c = FlightCamera::free_fly(start);
c.yaw = 0.0; // due east
c.pitch = 0.0;
c.integrate(&FlyInput { move_forward: 1000.0, ..Default::default() }, 1.0);
let (lon2, lat2) = c.position.to_lonlat();
assert!(
(c.position.z - start.z).abs() < 1e-9,
"level flight must HOLD ALTITUDE (z {} → {})",
start.z,
c.position.z
);
assert!(lon2 > lon0, "heading 0 (yaw=0) flies EAST — longitude increases");
assert!((lat2 - lat0).abs() < 1e-9, "and does not drift in latitude");
// (3) Strafing up in free-fly is a pure altitude change.
let mut c = FlightCamera::free_fly(start);
c.integrate(&FlyInput { move_up: 250.0, ..Default::default() }, 1.0);
assert!((c.position.z - start.z - 250.0).abs() < 1e-6, "move_up is a climb of exactly 250 m");
let (lon3, lat3) = c.position.to_lonlat();
assert!((lon3 - lon0).abs() < 1e-9 && (lat3 - lat0).abs() < 1e-9, "and moves you nowhere on the map");
}
/// **INJECT-ASSERT (free-fly pitch can't gimbal-flip):** slamming the look up
/// clamps pitch shy of the pole, keeping the up vector well-defined.
#[test]
fn free_fly_pitch_is_clamped() {
let mut c = FlightCamera::free_fly(WorldPos::new(0.0, 0.0, 0.0));
c.integrate(&FlyInput { pitch_rate: 100.0, ..Default::default() }, 1.0);
assert!(c.pitch <= PITCH_LIMIT + 1e-12, "pitch clamped below the pole");
c.integrate(&FlyInput { pitch_rate: -100.0, ..Default::default() }, 2.0);
assert!(c.pitch >= -PITCH_LIMIT - 1e-12, "pitch clamped above the anti-pole");
}
/// **INJECT-ASSERT (the flight-stick rolls — the sim-only DOF):** the aileron
/// banks the camera (roll grows + the right vector tilts out of the horizontal),
/// and throttle drives the eye along the banked forward axis.
#[test]
fn flight_stick_rolls_and_throttles() {
let mut c = FlightCamera::free_fly(WorldPos::new(0.0, 1000.0, 0.0));
c.set_mode(FlyMode::FlightStick);
c.integrate(&FlyInput { roll_rate: 0.5, ..Default::default() }, 1.0);
assert!((c.roll - 0.5).abs() < 1e-12, "aileron banks the camera");
// With roll, the right vector picks up a vertical component (it banked).
assert!(c.right()[1].abs() > 1e-3, "the banked right vector tilts off horizontal");
let before = c.position;
let f = c.forward();
c.integrate(&FlyInput { move_forward: 200.0, ..Default::default() }, 1.0);
let d = [c.position.x - before.x, c.position.y - before.y, c.position.z - before.z];
assert!(approx(d, [f[0] * 200.0, f[1] * 200.0, f[2] * 200.0], 1e-6), "throttle flies along forward");
}
/// **INJECT-ASSERT (orbit swings around its target at radius):** in Orbit mode
/// the eye stays on a sphere of `orbit_radius` around the target while yaw sweeps,
/// and dolly shrinks the radius. The showcase mode.
#[test]
fn orbit_keeps_radius_around_the_target() {
let mut c = FlightCamera::free_fly(WorldPos::new(0.0, 0.0, 0.0));
c.orbit_target = WorldPos::new(500.0, 100.0, -200.0);
c.orbit_radius = 800.0;
c.set_mode(FlyMode::Orbit);
for _ in 0..20 {
c.integrate(&FlyInput { yaw_rate: 0.3, ..Default::default() }, 0.1);
let r = c.position.ground_distance(c.orbit_target);
let full = ((c.position.x - c.orbit_target.x).powi(2)
+ (c.position.y - c.orbit_target.y).powi(2)
+ (c.position.z - c.orbit_target.z).powi(2))
.sqrt();
assert!((full - 800.0).abs() < 1e-6, "eye stays on the orbit sphere (r={r:.1}, full={full:.1})");
}
// Dolly in shrinks the radius.
c.integrate(&FlyInput { orbit_dolly: 100.0, ..Default::default() }, 1.0);
assert!(c.orbit_radius < 800.0, "dolly pulls the orbit in, got {}", c.orbit_radius);
}
/// **INJECT-ASSERT (no NaN / gimbal blow-up under extreme + adversarial input):**
/// hammer every mode with absurd rates, a huge `dt`, and a negative `dt`; the pose
/// must stay finite, pitch stay inside its clamp, the basis stay orthonormal, and a
/// negative `dt` be treated as zero (no time travel). Robustness of the kinematics.
#[test]
fn extreme_inputs_stay_finite_and_clamped() {
let finite_pos = |c: &FlightCamera| c.position.x.is_finite() && c.position.y.is_finite() && c.position.z.is_finite();
let ortho = |c: &FlightCamera| {
let (f, r, u) = (c.forward(), c.right(), c.up());
let len = |v: [f64; 3]| (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt();
let dot = |a: [f64; 3], b: [f64; 3]| a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
[f, r, u].iter().all(|&v| (len(v) - 1.0).abs() < 1e-6) && dot(f, r).abs() < 1e-6 && dot(f, u).abs() < 1e-6
};
let wild = FlyInput {
move_right: 1e12,
move_up: -1e12,
move_forward: 1e12,
yaw_rate: 1e6,
pitch_rate: 1e6,
roll_rate: 1e6,
orbit_dolly: 1e9,
};
for mode in [FlyMode::FreeFly, FlyMode::FlightStick, FlyMode::Orbit] {
let mut c = FlightCamera::free_fly(WorldPos::from_lonlat(18.0, 59.0, 5000.0));
c.set_mode(mode);
for _ in 0..5 {
c.integrate(&wild, 1e6); // absurd dt
}
assert!(finite_pos(&c), "{mode:?}: position stays finite under extreme input");
assert!(c.pitch.abs() <= PITCH_LIMIT + 1e-9, "{mode:?}: pitch stays clamped ({})", c.pitch);
assert!(ortho(&c), "{mode:?}: basis stays orthonormal under extreme input");
}
// A negative dt is floored to zero — no reverse-time integration.
let mut c = FlightCamera::free_fly(WorldPos::new(100.0, 200.0, 300.0));
let before = c.position;
c.integrate(&FlyInput { move_forward: 50.0, yaw_rate: 1.0, ..Default::default() }, -10.0);
assert_eq!([c.position.x, c.position.y, c.position.z], [before.x, before.y, before.z], "negative dt is a no-op");
assert_eq!(c.yaw, 0.0, "negative dt doesn't turn the camera");
}
/// **INJECT-ASSERT (the rig drives the floating origin):** the camera-snapped
/// origin keeps the eye at small camera-relative `f32` coords even when the world
/// position is tens of millions of metres out — the rig + origin together kill
/// the jitter.
#[test]
fn rig_eye_is_small_under_the_floating_origin() {
let mut c = FlightCamera::free_fly(WorldPos::from_lonlat(178.0, 68.0, 3000.0));
assert!(c.position.x.abs() > 1.8e7, "camera is far from the projection origin");
c.integrate(&FlyInput { move_forward: 250.0, yaw_rate: 0.1, ..Default::default() }, 1.0);
let origin = c.make_origin(128.0);
let eye = c.eye_relative(origin);
let mag = eye.iter().map(|v| v.abs()).fold(0.0, f32::max);
assert!(mag <= 128.0 + 1e-3, "the rebased eye stays within a grid cell of the origin ({mag} m)");
}
}