rustsim_crowd/

threed.rs

//! Layered 2.5-D crowd locomotion for multi-storey environments.
//!
//! In real buildings (stations, airports, malls, stadiums) pedestrians
//! spend the vast majority of their time walking on a floor and only
//! occasionally transition vertically via stairs, escalators, ramps, or
//! lifts. Running full 3-D collision physics for every pedestrian
//! all of the time is wasteful and numerically fragile.
//!
//! The industry-standard approach (used by JuPedSim, MassMotion, Legion)
//! is **layered 2.5-D**: each pedestrian lives on a `floor` (integer
//! index) and its planar `(x, y)` motion is advanced by one of the 2-D
//! models in the crate (Social Force by default). Vertical motion is
//! scripted by the `rustsim-mobility` crate through
//! [`FloorTransition`]s — the pedestrian reaches a connector, is placed
//! on the next floor, and its planar state is preserved.
//!
//! This module defines the data model:
//!
//! - [`Pedestrian3D`] — 2-D pose plus floor index and vertical offset.
//! - [`WallPolygon3D`] — a polyline of 3-D segments, one per floor, used
//!   by renderers and per-floor-physics setup.
//! - [`FloorTransition`] — a connector (stair, escalator, ramp, lift)
//!   that maps a 2-D region on floor A to a 2-D region on floor B with
//!   an associated travel time.
//! - [`LayeredSpace`] — a container of floors, walls, and connectors.
//! - [`step_layered`] — advance every pedestrian by one tick, delegating
//!   per-floor motion to a provided 2-D model step function.

use rustsim_geometry::vec2::Vec2;
use rustsim_geometry::vec3::Vec3;

use crate::common::Pedestrian;
use crate::common::WallSegment;

/// Integer floor index. 0 = ground floor, 1 = first floor, etc.
pub type FloorId = i32;

/// A pedestrian in a layered 2.5-D environment.
///
/// Construct with [`Pedestrian3D::grounded`] or
/// [`Pedestrian3D::heading_to_floor`] — the type is `#[non_exhaustive]`
/// so future field additions (e.g. lift call-button state, floor
/// preferences) are non-breaking for downstream callers.
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub struct Pedestrian3D {
    /// 2-D pose on the current floor.
    pub base: Pedestrian,
    /// Floor index.
    pub floor: FloorId,
    /// Vertical offset from the floor slab (m). Non-zero while on a
    /// connector; 0 when grounded on the floor.
    pub z_offset: f64,
    /// Optional ongoing vertical transition.
    pub transition: Option<ActiveTransition>,
    /// Planner-owned floor intent. `Some(f)` means the pedestrian wants
    /// to reach floor `f`; `None` means "stay on current floor".
    ///
    /// Boarding a connector is gated on `target_floor != Some(self.floor)`,
    /// so an agent whose 2-D destination happens to overlap a boarding
    /// zone on the same floor (or an agent that just alighted *onto*
    /// that destination) will **not** re-enter the connector. This
    /// fixes a long-standing re-entry bug in naïve spatial-only boarding.
    pub target_floor: Option<FloorId>,
}

impl Pedestrian3D {
    /// Convenience constructor grounded on `floor` with no vertical intent.
    pub fn grounded(base: Pedestrian, floor: FloorId) -> Self {
        Self {
            base,
            floor,
            z_offset: 0.0,
            transition: None,
            target_floor: None,
        }
    }

    /// Grounded constructor that explicitly requests a target floor.
    pub fn heading_to_floor(base: Pedestrian, floor: FloorId, target: FloorId) -> Self {
        Self {
            base,
            floor,
            z_offset: 0.0,
            transition: None,
            target_floor: Some(target),
        }
    }

    /// Full 3-D position, combining the floor elevation provided by the
    /// [`LayeredSpace`] with the local 2-D pose and vertical offset.
    pub fn pos_3d(&self, space: &LayeredSpace) -> Vec3 {
        let z = space.floor_z(self.floor) + self.z_offset;
        [self.base.pos[0], self.base.pos[1], z]
    }
}

/// A polyline wall that spans a single floor.
#[derive(Debug, Clone)]
pub struct WallPolygon3D {
    /// Which floor this polygon belongs to.
    pub floor: FloorId,
    /// Vertices in 2-D floor coordinates, forming a closed loop if the
    /// first and last entries are equal.
    pub vertices: Vec<Vec2>,
}

impl WallPolygon3D {
    /// Expand this polygon into a sequence of [`WallSegment`]s consumable
    /// by the 2-D physics models.
    pub fn to_segments(&self) -> Vec<WallSegment> {
        if self.vertices.len() < 2 {
            return Vec::new();
        }
        self.vertices
            .windows(2)
            .map(|w| WallSegment { a: w[0], b: w[1] })
            .collect()
    }
}

/// Kind of vertical connector.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ConnectorKind {
    /// Bi-directional static stair.
    Stair,
    /// One-directional moving escalator.
    Escalator,
    /// Ramp (sloped floor).
    Ramp,
    /// Lift / elevator.
    Lift,
}

/// A connector between two floors.
#[derive(Debug, Clone)]
pub struct FloorTransition {
    /// Stable identifier.
    pub id: u64,
    /// Kind.
    pub kind: ConnectorKind,
    /// Floor where the connector begins.
    pub from_floor: FloorId,
    /// Centre of the boarding zone on the origin floor.
    pub from_pos: Vec2,
    /// Floor where the connector ends.
    pub to_floor: FloorId,
    /// Centre of the alighting zone on the destination floor.
    pub to_pos: Vec2,
    /// Zone radius within which boarding is accepted (m).
    pub boarding_radius: f64,
    /// Travel time from boarding to alighting (s).
    pub travel_time: f64,
}

/// An active transition a pedestrian is currently riding.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ActiveTransition {
    /// Connector id.
    pub connector_id: u64,
    /// Seconds remaining until the pedestrian arrives on the destination floor.
    pub remaining: f64,
}

/// A multi-floor environment.
#[derive(Debug, Clone)]
pub struct LayeredSpace {
    /// Z coordinate of each floor slab (indexed by `FloorId`).
    pub floor_elevations: Vec<(FloorId, f64)>,
    /// Walls per floor.
    pub walls: Vec<WallPolygon3D>,
    /// Connectors between floors.
    pub connectors: Vec<FloorTransition>,
}

impl LayeredSpace {
    /// Create an empty layered space.
    pub fn new() -> Self {
        Self {
            floor_elevations: Vec::new(),
            walls: Vec::new(),
            connectors: Vec::new(),
        }
    }

    /// Register a floor at elevation `z`. Overwrites any previous entry.
    pub fn set_floor(&mut self, floor: FloorId, z: f64) {
        if let Some(entry) = self.floor_elevations.iter_mut().find(|(f, _)| *f == floor) {
            entry.1 = z;
        } else {
            self.floor_elevations.push((floor, z));
        }
    }

    /// Elevation of a floor (0.0 if unknown).
    pub fn floor_z(&self, floor: FloorId) -> f64 {
        self.floor_elevations
            .iter()
            .find(|(f, _)| *f == floor)
            .map(|(_, z)| *z)
            .unwrap_or(0.0)
    }

    /// Gather walls on a specific floor as 2-D segments.
    pub fn segments_on_floor(&self, floor: FloorId) -> Vec<WallSegment> {
        let mut out = Vec::new();
        for p in &self.walls {
            if p.floor == floor {
                out.extend(p.to_segments());
            }
        }
        out
    }

    /// Connector with a matching id, if any.
    pub fn connector(&self, id: u64) -> Option<&FloorTransition> {
        self.connectors.iter().find(|c| c.id == id)
    }

    /// Pick the first connector on `floor` whose boarding zone contains `pos`.
    pub fn connector_at(&self, floor: FloorId, pos: Vec2) -> Option<&FloorTransition> {
        self.connectors.iter().find(|c| {
            c.from_floor == floor
                && rustsim_geometry::vec2::distance(pos, c.from_pos) <= c.boarding_radius
        })
    }
}

impl Default for LayeredSpace {
    fn default() -> Self {
        Self::new()
    }
}

/// Function that advances a slice of 2-D pedestrians on a single floor by `dt`.
///
/// The crate provides one for each 2-D model — e.g. pass
/// `crate::social_force::step` to use the Social Force physics.
pub type PlanarStepFn = fn(&mut [Pedestrian], &[WallSegment], &crate::social_force::Params, f64);

/// Advance every pedestrian in `peds` by `dt` using `planar_step` as the
/// per-floor physics and `space` for walls and connectors.
///
/// Semantics:
/// 1. Pedestrians currently riding a connector have their `remaining` time
///    decremented; when it reaches zero they are teleported to the
///    connector's `to_floor` at `to_pos` and grounded.
/// 2. Grounded pedestrians are grouped by floor. For each floor the
///    provided planar step function is called with that floor's walls.
/// 3. After planar motion, each grounded pedestrian is checked against
///    the floor's connectors; if its 2-D position lands inside a
///    connector's boarding zone *and* its target is on the connector's
///    destination floor, it boards (a new [`ActiveTransition`] begins).
///
/// Boarding is only triggered for pedestrians whose target destination
/// (z-planar) is on a different floor; the selection of *which* connector
/// to take is the responsibility of the `rustsim-mobility` crate, which
/// would instead set the pedestrian's `destination` to the connector's
/// `from_pos` directly.
/// Reusable scratch buffers for [`step_layered_scratch`].
///
/// `step_layered` is convenient but allocates four `Vec`s per tick
/// (`floors`, `idxs`, `buf`, `walls`). For a 30–60 Hz station sim with
/// multiple floors this is measurable allocator traffic. Allocate one
/// `LayeredScratch` per simulation, pass it to [`step_layered_scratch`]
/// every tick, and the heap is untouched on the steady state.
///
/// The individual fields are `pub(crate)` so the wrapper can reuse
/// them; external callers should treat the type as opaque.
#[derive(Debug, Default)]
pub struct LayeredScratch {
    pub(crate) floors: Vec<FloorId>,
    pub(crate) idxs: Vec<usize>,
    pub(crate) buf: Vec<Pedestrian>,
    pub(crate) walls: Vec<WallSegment>,
}

impl LayeredScratch {
    /// Create an empty scratch with the default capacities.
    pub fn new() -> Self {
        Self::default()
    }

    /// Create a scratch pre-sized for `n_peds` pedestrians. `Vec`s are
    /// only reserved, not populated.
    pub fn with_capacity(n_peds: usize) -> Self {
        Self {
            floors: Vec::with_capacity(4),
            idxs: Vec::with_capacity(n_peds),
            buf: Vec::with_capacity(n_peds),
            walls: Vec::with_capacity(32),
        }
    }
}

/// Advance every pedestrian in `peds` by `dt`. Convenience wrapper that
/// allocates one [`LayeredScratch`] per call and immediately drops it;
/// prefer [`step_layered_scratch`] when the same simulation drives
/// successive ticks.
pub fn step_layered(
    peds: &mut [Pedestrian3D],
    space: &LayeredSpace,
    planar_step: PlanarStepFn,
    params: &crate::social_force::Params,
    dt: f64,
) {
    let mut scratch = LayeredScratch::with_capacity(peds.len());
    step_layered_scratch(peds, space, planar_step, params, dt, &mut scratch);
}

/// Zero-allocation variant of [`step_layered`].
///
/// Semantics are identical to [`step_layered`]; the only difference is
/// that all working buffers are borrowed from `scratch` and their
/// capacities are preserved across calls.
pub fn step_layered_scratch(
    peds: &mut [Pedestrian3D],
    space: &LayeredSpace,
    planar_step: PlanarStepFn,
    params: &crate::social_force::Params,
    dt: f64,
    scratch: &mut LayeredScratch,
) {
    step_layered_scratch_observed(
        peds,
        space,
        planar_step,
        params,
        dt,
        scratch,
        &mut NoopLayeredObserver,
    );
}

/// Per-pedestrian post-step observation hook for the layered 2.5-D
/// drive.
///
/// Mirrors [`crate::integration::CrowdObserver`] for the 2-D
/// `AgentStore` path: closes the layered half of the P1-6 "telemetry
/// hooks" item from `docs/rustsim-crowd.md`. Invoked by
/// [`step_layered_scratch_observed`] **after** every tick stage has
/// completed (rider advance, per-floor physics, boarding decisions),
/// so the `Pedestrian3D` argument carries the authoritative
/// post-tick `floor`, `z_offset`, `transition`, `target_floor`, and
/// planar state. The `index` argument is the position of the
/// pedestrian inside `peds`, which is stable across the call (the
/// layered drive never reorders the slice).
///
/// The trait is blanket-implemented for every `FnMut(usize,
/// &Pedestrian3D)`, so callers typically pass a closure that
/// forwards into a `TelemetryPipeline`, a per-floor occupancy
/// counter, or a CSV logger — without `rustsim-crowd` itself taking
/// a dependency on any sink implementation.
///
/// Observation order is `0..peds.len()`, deterministic on every run.
pub trait LayeredObserver {
    /// Observe the post-tick state of one pedestrian.
    fn observe(&mut self, index: usize, ped: &Pedestrian3D);
}

impl<F> LayeredObserver for F
where
    F: FnMut(usize, &Pedestrian3D),
{
    #[inline]
    fn observe(&mut self, index: usize, ped: &Pedestrian3D) {
        self(index, ped);
    }
}

/// No-op observer used by [`step_layered_scratch`] to share code
/// with [`step_layered_scratch_observed`]. Monomorphisation erases
/// the observer entirely in the non-observing path.
#[derive(Debug, Clone, Copy, Default)]
struct NoopLayeredObserver;

impl LayeredObserver for NoopLayeredObserver {
    #[inline]
    fn observe(&mut self, _index: usize, _ped: &Pedestrian3D) {}
}

/// Observed variant of [`step_layered_scratch`]: identical semantics,
/// plus a post-tick callback for every pedestrian.
///
/// `observer.observe(i, &peds[i])` is invoked once per pedestrian,
/// in `0..peds.len()` order, **after** all three stages of the tick
/// have completed (rider advance, per-floor planar step, boarding).
/// This is the production telemetry entry point for the 2.5-D
/// drive: a closure that forwards into
/// `rustsim::TelemetryPipeline::push_row` (or any other sink — CSV,
/// Parquet, in-memory floor-occupancy counter) gets per-agent
/// per-tick coverage with zero allocation on the hot path beyond
/// what the sink itself may do.
///
/// Panic safety: if `observer.observe` panics the callback loop
/// unwinds; the `peds` slice is left in its full post-tick state
/// because the observer runs after every write-back.
#[allow(clippy::too_many_arguments)]
pub fn step_layered_scratch_observed<O>(
    peds: &mut [Pedestrian3D],
    space: &LayeredSpace,
    planar_step: PlanarStepFn,
    params: &crate::social_force::Params,
    dt: f64,
    scratch: &mut LayeredScratch,
    observer: &mut O,
) where
    O: LayeredObserver + ?Sized,
{
    step_layered_scratch_inner(peds, space, planar_step, params, dt, scratch);
    for (i, ped) in peds.iter().enumerate() {
        observer.observe(i, ped);
    }
}

fn step_layered_scratch_inner(
    peds: &mut [Pedestrian3D],
    space: &LayeredSpace,
    planar_step: PlanarStepFn,
    params: &crate::social_force::Params,
    dt: f64,
    scratch: &mut LayeredScratch,
) {
    // 1. Advance riders and complete transitions.
    for p in peds.iter_mut() {
        if let Some(t) = p.transition.as_mut() {
            t.remaining -= dt;
            if t.remaining <= 0.0 {
                if let Some(c) = space.connector(t.connector_id) {
                    p.floor = c.to_floor;
                    p.base.pos = c.to_pos;
                    p.base.vel = [0.0, 0.0];
                    p.z_offset = 0.0;
                }
                p.transition = None;
            } else if let Some(c) = space.connector(t.connector_id) {
                // Linearly interpolate z_offset for visualisation.
                let travel = c.travel_time.max(1e-9);
                let t_frac = 1.0 - (t.remaining / travel).clamp(0.0, 1.0);
                let dz = space.floor_z(c.to_floor) - space.floor_z(c.from_floor);
                p.z_offset = t_frac * dz;
            }
        }
    }

    // 2. Group grounded pedestrians by floor (reuse scratch.floors).
    scratch.floors.clear();
    for p in peds.iter() {
        if p.transition.is_none() && !scratch.floors.contains(&p.floor) {
            scratch.floors.push(p.floor);
        }
    }
    scratch.floors.sort();

    // Iterate by index (rather than `for floor in &scratch.floors`) so
    // the scratch can be re-borrowed for `idxs` / `buf` / `walls`.
    let n_floors = scratch.floors.len();
    for fi in 0..n_floors {
        let floor = scratch.floors[fi];

        scratch.idxs.clear();
        scratch.buf.clear();
        for (i, p) in peds.iter().enumerate() {
            if p.transition.is_none() && p.floor == floor {
                scratch.idxs.push(i);
                scratch.buf.push(p.base);
            }
        }

        scratch.walls.clear();
        for polygon in &space.walls {
            if polygon.floor == floor {
                // `to_segments` still allocates, but it's a property of
                // the space rather than of the pedestrian pass; inline
                // to skip the intermediate Vec.
                if polygon.vertices.len() >= 2 {
                    for w in polygon.vertices.windows(2) {
                        scratch.walls.push(WallSegment { a: w[0], b: w[1] });
                    }
                }
            }
        }

        planar_step(&mut scratch.buf, &scratch.walls, params, dt);

        for (k, &i) in scratch.idxs.iter().enumerate() {
            peds[i].base = scratch.buf[k];
        }
    }

    // 3. Board connectors where applicable.
    for p in peds.iter_mut() {
        if p.transition.is_some() {
            continue;
        }
        // Boarding is only valid if the planner explicitly wants the
        // pedestrian on a different floor. This prevents the classic
        // "alight → re-board immediately" loop that occurs when a
        // pedestrian's destination lies inside (or next to) a
        // connector boarding zone on the destination floor.
        let wants_transfer = match p.target_floor {
            Some(f) => f != p.floor,
            None => false,
        };
        if !wants_transfer {
            continue;
        }
        if let Some(c) = space.connector_at(p.floor, p.base.pos) {
            // Only board connectors whose `to_floor` matches the
            // planner intent. A multi-hop plan should update
            // `target_floor` after each alighting.
            if Some(c.to_floor) != p.target_floor {
                continue;
            }
            p.transition = Some(ActiveTransition {
                connector_id: c.id,
                remaining: c.travel_time,
            });
        }
    }
}

#[cfg(test)]
#[allow(deprecated)] // intentional: pins 3D-projection equivalence vs the deprecated O(n²) `step`.
mod tests {
    use super::*;
    use crate::common::Pedestrian;
    use crate::social_force;

    fn ped(pos: Vec2, dest: Vec2) -> Pedestrian {
        Pedestrian {
            pos,
            vel: [0.0, 0.0],
            radius: 0.25,
            desired_speed: 1.34,
            destination: dest,
        }
    }

    fn two_floor_space() -> LayeredSpace {
        let mut s = LayeredSpace::new();
        s.set_floor(0, 0.0);
        s.set_floor(1, 4.0);
        s.connectors.push(FloorTransition {
            id: 1,
            kind: ConnectorKind::Escalator,
            from_floor: 0,
            from_pos: [10.0, 0.0],
            to_floor: 1,
            to_pos: [10.0, 0.0],
            boarding_radius: 0.4,
            travel_time: 10.0,
        });
        s
    }

    #[test]
    fn grounded_pedestrian_moves_on_its_floor() {
        let space = two_floor_space();
        let mut peds = vec![Pedestrian3D::grounded(ped([0.0, 0.0], [5.0, 0.0]), 0)];
        for _ in 0..30 {
            step_layered(
                &mut peds,
                &space,
                social_force::step,
                &social_force::Params::default(),
                0.1,
            );
        }
        assert!(peds[0].base.pos[0] > 0.5);
        assert_eq!(peds[0].floor, 0);
    }

    #[test]
    fn boarding_and_alighting_transitions_between_floors() {
        let space = two_floor_space();
        let mut peds = vec![Pedestrian3D::heading_to_floor(
            ped([10.0, 0.0], [10.0, 0.0]),
            0,
            1,
        )];
        // First step: triggers boarding.
        step_layered(
            &mut peds,
            &space,
            social_force::step,
            &social_force::Params::default(),
            0.1,
        );
        assert!(peds[0].transition.is_some());
        // Many steps: ride out the 10s travel time.
        for _ in 0..200 {
            step_layered(
                &mut peds,
                &space,
                social_force::step,
                &social_force::Params::default(),
                0.1,
            );
        }
        assert!(peds[0].transition.is_none());
        assert_eq!(peds[0].floor, 1);
    }

    #[test]
    fn does_not_reboard_after_alighting_into_same_boarding_zone() {
        // Regression: a pedestrian whose 2-D destination on the upper
        // floor lies inside the connector's boarding zone used to
        // re-board the connector every tick forever. Now that
        // boarding is gated on `target_floor`, alighting clears the
        // intent (planner's responsibility) and the ride-back loop
        // is impossible with a single-hop intent.
        let space = two_floor_space();
        let mut peds = vec![Pedestrian3D::heading_to_floor(
            ped([10.0, 0.0], [10.0, 0.0]),
            0,
            1,
        )];
        // Board, ride, alight.
        for _ in 0..200 {
            step_layered(
                &mut peds,
                &space,
                social_force::step,
                &social_force::Params::default(),
                0.1,
            );
        }
        assert_eq!(peds[0].floor, 1);

        // Planner reached its target, clears the intent.
        peds[0].target_floor = None;

        // Now drive many more ticks with the pedestrian sitting on
        // top of floor 1's side of the connector. It must never
        // re-board.
        for _ in 0..500 {
            step_layered(
                &mut peds,
                &space,
                social_force::step,
                &social_force::Params::default(),
                0.1,
            );
            assert!(peds[0].transition.is_none(), "pedestrian re-boarded");
            assert_eq!(peds[0].floor, 1);
        }
    }

    #[test]
    fn pos_3d_combines_floor_elevation_and_z_offset() {
        let space = two_floor_space();
        let p = Pedestrian3D {
            base: ped([1.0, 2.0], [0.0, 0.0]),
            floor: 1,
            z_offset: 0.5,
            transition: None,
            target_floor: None,
        };
        let p3 = p.pos_3d(&space);
        assert_eq!(p3, [1.0, 2.0, 4.5]);
    }

    #[test]
    fn step_layered_scratch_matches_step_layered_bit_exact() {
        // Seed a heterogeneous state (both floors, one rider) and verify
        // scratched vs non-scratched variants produce bit-identical output.
        let space = two_floor_space();
        let params = social_force::Params::default();

        let seed_peds = || -> Vec<Pedestrian3D> {
            vec![
                Pedestrian3D::heading_to_floor(ped([0.0, 0.0], [10.0, 0.0]), 0, 1),
                Pedestrian3D::heading_to_floor(ped([2.0, 1.0], [10.0, 0.0]), 0, 1),
                Pedestrian3D::grounded(ped([5.0, -0.5], [20.0, 0.0]), 0),
                Pedestrian3D::grounded(ped([7.0, 0.5], [-5.0, 0.0]), 1),
            ]
        };

        let mut a = seed_peds();
        let mut b = seed_peds();
        let mut scratch = LayeredScratch::with_capacity(a.len());

        for _ in 0..50 {
            step_layered(&mut a, &space, social_force::step, &params, 0.1);
            step_layered_scratch(
                &mut b,
                &space,
                social_force::step,
                &params,
                0.1,
                &mut scratch,
            );
        }

        assert_eq!(a.len(), b.len());
        for (pa, pb) in a.iter().zip(b.iter()) {
            assert_eq!(pa.floor, pb.floor);
            assert_eq!(pa.base.pos, pb.base.pos);
            assert_eq!(pa.base.vel, pb.base.vel);
            assert_eq!(pa.transition.is_some(), pb.transition.is_some());
        }
    }

    #[test]
    fn layered_scratch_reuses_capacity_across_ticks() {
        let space = two_floor_space();
        let params = social_force::Params::default();
        let mut peds = vec![
            Pedestrian3D::grounded(ped([0.0, 0.0], [5.0, 0.0]), 0),
            Pedestrian3D::grounded(ped([1.0, 1.0], [5.0, 0.0]), 0),
            Pedestrian3D::grounded(ped([2.0, -1.0], [5.0, 0.0]), 1),
        ];
        let mut scratch = LayeredScratch::with_capacity(peds.len());

        // First tick allocates up to the working-set sizes.
        step_layered_scratch(
            &mut peds,
            &space,
            social_force::step,
            &params,
            0.1,
            &mut scratch,
        );
        let (cap_floors, cap_idxs, cap_buf, cap_walls) = (
            scratch.floors.capacity(),
            scratch.idxs.capacity(),
            scratch.buf.capacity(),
            scratch.walls.capacity(),
        );

        // Subsequent ticks must not grow any of the reusable buffers.
        for _ in 0..20 {
            step_layered_scratch(
                &mut peds,
                &space,
                social_force::step,
                &params,
                0.1,
                &mut scratch,
            );
        }
        assert_eq!(scratch.floors.capacity(), cap_floors);
        assert_eq!(scratch.idxs.capacity(), cap_idxs);
        assert_eq!(scratch.buf.capacity(), cap_buf);
        assert_eq!(scratch.walls.capacity(), cap_walls);
    }

    #[test]
    fn observed_layered_step_matches_unobserved_and_streams_in_index_order() {
        // Pin the layered observer contract:
        //
        //   1. `step_layered_scratch_observed` produces the same
        //      post-tick `peds` slice as `step_layered_scratch` for
        //      identical inputs (the observer is purely a callback,
        //      it must not perturb state);
        //   2. the observer sees one row per pedestrian per tick, in
        //      `0..peds.len()` order, with the **post-tick**
        //      `Pedestrian3D` (so transitions, floor moves, and
        //      planar updates are all visible to telemetry);
        //   3. the boarding gate continues to fire under the
        //      observer (i.e. the rewrite did not regress the
        //      `target_floor` semantics from blocker #8).
        let space = two_floor_space();
        let params = social_force::Params::default();

        let make_peds = || {
            vec![
                // Heading from floor 0 to floor 1 via the connector
                // at [10, 0]; should board within a few ticks.
                Pedestrian3D::heading_to_floor(ped([9.7, 0.0], [10.0, 0.0]), 0, 1),
                // Stays on floor 0; just walks a bit.
                Pedestrian3D::grounded(ped([0.0, 0.0], [3.0, 0.0]), 0),
                // Already on floor 1, no transfer intent.
                Pedestrian3D::grounded(ped([0.0, 5.0], [3.0, 5.0]), 1),
            ]
        };

        let mut peds_a = make_peds();
        let mut peds_b = make_peds();
        let mut scratch_a = LayeredScratch::with_capacity(peds_a.len());
        let mut scratch_b = LayeredScratch::with_capacity(peds_b.len());

        // Each tick the observer logs (index, post-tick floor,
        // post-tick `transition.is_some()`, post-tick pos[0]).
        let mut log: Vec<(usize, FloorId, bool, f64)> = Vec::new();

        let total_ticks = 20;
        for _ in 0..total_ticks {
            step_layered_scratch(
                &mut peds_a,
                &space,
                social_force::step,
                &params,
                0.1,
                &mut scratch_a,
            );
            step_layered_scratch_observed(
                &mut peds_b,
                &space,
                social_force::step,
                &params,
                0.1,
                &mut scratch_b,
                &mut |i: usize, ped: &Pedestrian3D| {
                    log.push((i, ped.floor, ped.transition.is_some(), ped.base.pos[0]));
                },
            );
        }

        // (1) identical post-tick state.
        assert_eq!(
            peds_a, peds_b,
            "observed step must not perturb the underlying tick"
        );
        // (2) the log has exactly `n_peds * total_ticks` rows in
        //     deterministic index-major / tick-major order.
        let n = peds_a.len();
        assert_eq!(log.len(), n * total_ticks);
        for (row_idx, row) in log.iter().enumerate() {
            assert_eq!(row.0, row_idx % n, "observer must stream in index order");
        }
        // (3) the boarding gate still fires: agent 0 must end up on
        //     floor 1 (or in transition to it) by the end. The log's
        //     last entry for agent 0 carries the final state.
        let last_for_zero = log
            .iter()
            .rev()
            .find(|r| r.0 == 0)
            .expect("agent 0 must appear in the log");
        assert!(
            last_for_zero.1 == 1 || last_for_zero.2,
            "agent 0 should have boarded the connector or already alighted on floor 1, got {:?}",
            last_for_zero
        );
    }
}