rustsim_crowd/

common.rs

//! Shared types and trait for all pedestrian models in this crate.

/// 2-D vector alias used throughout the crate.
pub type Vec2 = [f64; 2];

/// A single pedestrian agent as seen by every model in this crate.
///
/// This is intentionally a small, copy-friendly struct so model `step`
/// functions can be called on `&mut [Pedestrian]` without any indirection.
///
/// # Construction
///
/// Outside this crate, **construct via [`Pedestrian::new`]**, not struct
/// literal syntax: the type is `#[non_exhaustive]` so that adding a field
/// in a future minor version is a non-breaking change for downstream
/// callers. Field-access (`ped.pos`, `ped.vel = …`) remains stable and
/// is intentionally left `pub` so the SoA-friendly hot path stays
/// indirection-free.
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub struct Pedestrian {
    /// Position in metres.
    pub pos: Vec2,
    /// Velocity in m/s.
    pub vel: Vec2,
    /// Body radius in metres (typical pedestrian: 0.2–0.3 m).
    pub radius: f64,
    /// Desired free-flow walking speed in m/s (Weidmann mean: 1.34 m/s).
    pub desired_speed: f64,
    /// Target destination in metres.
    pub destination: Vec2,
}

impl Pedestrian {
    /// Constructs a [`Pedestrian`] from its five core fields.
    ///
    /// This is the **stable construction path** for downstream callers:
    /// because [`Pedestrian`] is `#[non_exhaustive]`, adding a field in a
    /// future minor version will not break callers that go through
    /// `Pedestrian::new`. Each new field will get a paired `with_*`
    /// setter (e.g. a hypothetical `with_target_floor`) so the
    /// extension stays additive.
    #[inline]
    pub fn new(pos: Vec2, vel: Vec2, radius: f64, desired_speed: f64, destination: Vec2) -> Self {
        Self {
            pos,
            vel,
            radius,
            desired_speed,
            destination,
        }
    }
    /// Vector from `pos` to `destination`.
    #[inline]
    pub fn to_destination(&self) -> Vec2 {
        sub(self.destination, self.pos)
    }

    /// Unit vector pointing at the destination, or `[0, 0]` if already there.
    #[inline]
    pub fn desired_direction(&self) -> Vec2 {
        normalize(self.to_destination())
    }

    /// Current speed `|vel|`.
    #[inline]
    pub fn speed(&self) -> f64 {
        norm(self.vel)
    }

    /// Euclidean distance from `pos` to `destination`.
    #[inline]
    pub fn distance_to_destination(&self) -> f64 {
        norm(self.to_destination())
    }

    /// Returns `true` if the pedestrian is inside its arrival radius.
    #[inline]
    pub fn has_arrived(&self, arrival_radius: f64) -> bool {
        self.distance_to_destination() <= arrival_radius
    }

    /// Speed the pedestrian should aim for given how close it is to
    /// its destination. Produces a smooth linear taper from the full
    /// [`desired_speed`](Self::desired_speed) at `d ≥ arrival_radius`
    /// down to `0` at `d = 0`, so agents **do not overshoot** their
    /// goal and then oscillate back toward it.
    ///
    /// `arrival_radius ≤ 0` disables the taper (returns
    /// [`desired_speed`](Self::desired_speed) unchanged).
    #[inline]
    pub fn effective_desired_speed(&self, arrival_radius: f64) -> f64 {
        if arrival_radius <= 0.0 {
            return self.desired_speed;
        }
        let d = self.distance_to_destination();
        if d >= arrival_radius {
            self.desired_speed
        } else {
            self.desired_speed * (d / arrival_radius)
        }
    }
}

/// A 2-D line segment describing a static obstacle (wall, barrier, etc.).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WallSegment {
    /// First endpoint in metres.
    pub a: Vec2,
    /// Second endpoint in metres.
    pub b: Vec2,
}

/// Common contract for every pedestrian locomotion model in this crate.
///
/// Implementations advance a slice of [`Pedestrian`] in place by one timestep
/// of `dt` seconds, considering static [`WallSegment`] obstacles and a
/// model-specific parameter bundle.
///
/// All trait objects are safe; every concrete `Params` is `Clone + Debug`.
pub trait PedestrianModel {
    /// Model-specific parameter bundle (typically with a calibrated `Default`).
    type Params;

    /// Short human-readable name, e.g. `"Social Force"`.
    fn name(&self) -> &'static str;

    /// Advance every pedestrian in `peds` by `dt` seconds.
    ///
    /// `walls` is a slice of static obstacles; pass an empty slice if none.
    fn step(&self, peds: &mut [Pedestrian], walls: &[WallSegment], params: &Self::Params, dt: f64);
}

// ---------------------------------------------------------------------------
// Vector math primitives (crate-internal).
// ---------------------------------------------------------------------------

#[inline]
pub(crate) fn add(a: Vec2, b: Vec2) -> Vec2 {
    [a[0] + b[0], a[1] + b[1]]
}

#[inline]
pub(crate) fn sub(a: Vec2, b: Vec2) -> Vec2 {
    [a[0] - b[0], a[1] - b[1]]
}

#[inline]
pub(crate) fn scale(a: Vec2, s: f64) -> Vec2 {
    [a[0] * s, a[1] * s]
}

#[inline]
pub(crate) fn dot(a: Vec2, b: Vec2) -> f64 {
    a[0] * b[0] + a[1] * b[1]
}

#[inline]
pub(crate) fn norm(a: Vec2) -> f64 {
    (a[0] * a[0] + a[1] * a[1]).sqrt()
}

#[inline]
pub(crate) fn normalize(a: Vec2) -> Vec2 {
    let n = norm(a);
    if n < 1e-12 {
        [0.0, 0.0]
    } else {
        [a[0] / n, a[1] / n]
    }
}

/// Returns the closest point on segment `ab` to point `p`.
#[inline]
pub(crate) fn closest_point_on_segment(p: Vec2, a: Vec2, b: Vec2) -> Vec2 {
    let ab = sub(b, a);
    let denom = dot(ab, ab);
    if denom < 1e-18 {
        return a;
    }
    let t = (dot(sub(p, a), ab) / denom).clamp(0.0, 1.0);
    add(a, scale(ab, t))
}

// ---------------------------------------------------------------------------
// Small helpers shared by several models.
// ---------------------------------------------------------------------------

/// Clamp the magnitude of `v` to `max_speed`. Returns `v` unchanged if its
/// magnitude is already within bounds or near zero.
#[inline]
pub(crate) fn clamp_speed(v: Vec2, max_speed: f64) -> Vec2 {
    let s = norm(v);
    if s > max_speed && s > 1e-12 {
        scale(v, max_speed / s)
    } else {
        v
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn normalize_zero_is_zero() {
        assert_eq!(normalize([0.0, 0.0]), [0.0, 0.0]);
    }

    #[test]
    fn closest_point_on_segment_endpoints_and_middle() {
        let a = [0.0, 0.0];
        let b = [10.0, 0.0];
        assert_eq!(closest_point_on_segment([-5.0, 5.0], a, b), a);
        assert_eq!(closest_point_on_segment([15.0, 5.0], a, b), b);
        assert_eq!(closest_point_on_segment([5.0, 5.0], a, b), [5.0, 0.0]);
    }

    #[test]
    fn clamp_speed_basic() {
        let v = clamp_speed([3.0, 4.0], 2.5);
        let s = norm(v);
        assert!((s - 2.5).abs() < 1e-9);
    }

    #[test]
    fn pedestrian_new_matches_literal_construction() {
        // Pins the P2-8 construction-stability contract: `Pedestrian::new`
        // is the future-proof construction path, and it must produce a
        // value bit-equal to the legacy literal-syntax construction so
        // downstream callers can migrate without observable behavioural
        // change. (Inside this crate `#[non_exhaustive]` does not block
        // literal syntax, so we can compare against it here.)
        let via_new = Pedestrian::new([1.0, 2.0], [0.3, 0.4], 0.25, 1.34, [10.0, 0.0]);
        let via_lit = Pedestrian {
            pos: [1.0, 2.0],
            vel: [0.3, 0.4],
            radius: 0.25,
            desired_speed: 1.34,
            destination: [10.0, 0.0],
        };
        assert_eq!(via_new, via_lit);
    }

    #[test]
    fn effective_desired_speed_tapers_inside_arrival_radius() {
        let p = Pedestrian {
            pos: [4.7, 0.0], // 0.3 m from destination
            vel: [0.0, 0.0],
            radius: 0.25,
            desired_speed: 1.0,
            destination: [5.0, 0.0],
        };
        // At exactly `arrival_radius`: full speed.
        assert!((p.effective_desired_speed(0.3) - 1.0).abs() < 1e-12);
        // At half the radius: half speed.
        let p_half = Pedestrian {
            pos: [4.85, 0.0],
            ..p
        };
        assert!((p_half.effective_desired_speed(0.3) - 0.5).abs() < 1e-12);
        // At the destination: zero.
        let p_there = Pedestrian {
            pos: [5.0, 0.0],
            ..p
        };
        assert_eq!(p_there.effective_desired_speed(0.3), 0.0);
        // arrival_radius == 0 disables the taper.
        assert_eq!(p_there.effective_desired_speed(0.0), 1.0);
    }

    #[test]
    fn has_arrived_reports_inside_radius() {
        let p = Pedestrian {
            pos: [4.9, 0.0],
            vel: [0.0, 0.0],
            radius: 0.25,
            desired_speed: 1.0,
            destination: [5.0, 0.0],
        };
        assert!(p.has_arrived(0.2));
        assert!(!p.has_arrived(0.05));
    }
}