rustsim-crowd 0.0.1

//! Generalized Centrifugal Force Model (Chraibi, Seyfried & Schadschneider 2010).
//!
//! A force-based 2nd-order model where the repulsive force between two
//! pedestrians depends on their relative velocity and an elliptical body
//! radius that grows with the pedestrian's own speed:
//!
//! ```text
//! r_i(v_i) = a + b * v_i
//! F_ij     = -m_i * (v0_i + v_ij·)^2 / (d_ij - r_i - r_j) * ê_ij
//! ```
//!
//! where `v_ij·` is the positive part of the approach speed along `ê_ji`.
//! A standard linear driving force pulls the pedestrian toward its goal.
//! Walls are handled with a similar centrifugal term against the closest
//! point on the segment.
//!
//! # Integrator
//!
//! Accelerations are integrated with **semi-implicit (symplectic)
//! Euler** (`v ← v + a·dt; p ← p + v·dt`): the velocity update
//! precedes the position advance and the new velocity is the one
//! that moves the agent. Together with `Params::max_accel` and the
//! `dt · max_accel ≤ max_speed` CFL gate in [`Params::validate`],
//! this keeps the stiff centrifugal pair repulsion stable at the
//! default timestep without sub-stepping.
//!
//! # References
//!
//! - Chraibi, M., Seyfried, A., & Schadschneider, A. (2010). "Generalized
//!   centrifugal-force model for pedestrian dynamics". *Physical Review E*,
//!   82(4), 046111.

use crate::broadphase::{NeighborGrid, Scratch};
use crate::common::{
    add, clamp_speed, closest_point_on_segment, dot, norm, scale, sub, Pedestrian, PedestrianModel,
    Vec2, WallSegment,
};

/// Parameters for the Generalized Centrifugal Force model.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Params {
    /// Relaxation time of the driving force (s).
    pub tau: f64,
    /// Pedestrian mass (kg).
    pub mass: f64,
    /// Constant term of the speed-dependent body radius (m).
    pub a: f64,
    /// Linear term of the speed-dependent body radius (s).
    pub b: f64,
    /// Wall repulsion gain (N·m).
    pub wall_gain: f64,
    /// Hard upper bound on speed after integration (m/s).
    pub max_speed: f64,
    /// Minimum clearance inside denominators (m). Prevents singularity at
    /// direct contact.
    pub min_clearance: f64,
    /// Arrival radius (m). See
    /// [`social_force::Params::arrival_radius`](crate::social_force::Params::arrival_radius).
    /// Default: 0.3 m.
    pub arrival_radius: f64,
    /// Hard upper bound on acceleration magnitude (m/s²).
    ///
    /// GCF's repulsion has a `1/clearance` tail that becomes very
    /// stiff near contact. Capping the net acceleration gives a
    /// CFL-like stability bound for explicit integration; see
    /// [`social_force::Params::max_accel`](crate::social_force::Params::max_accel)
    /// for the full rationale.
    pub max_accel: f64,
}

impl Default for Params {
    fn default() -> Self {
        Self {
            tau: 0.5,
            mass: 80.0,
            a: 0.18,
            b: 0.08,
            wall_gain: 5.0,
            max_speed: 2.5,
            min_clearance: 0.02,
            arrival_radius: 0.3,
            max_accel: 20.0,
        }
    }
}

impl Params {
    /// Validate this parameter set against `dt`.
    ///
    /// See [`social_force::Params::validate`](crate::social_force::Params::validate)
    /// for the full rationale. GCF additionally requires a strictly
    /// positive [`min_clearance`](Self::min_clearance) because the
    /// `1/clearance` repulsion tail would otherwise diverge at direct
    /// contact.
    pub fn validate(&self, dt: f64) -> Result<(), crate::error::CrowdError> {
        use crate::error::{require_dt, require_nonneg, require_positive, CrowdError};
        const M: &str = "GeneralizedCentrifugalForce";
        require_dt(M, dt)?;
        require_positive(M, "tau", self.tau)?;
        require_positive(M, "mass", self.mass)?;
        require_positive(M, "a", self.a)?;
        require_nonneg(M, "b", self.b)?;
        require_nonneg(M, "wall_gain", self.wall_gain)?;
        require_positive(M, "max_speed", self.max_speed)?;
        require_positive(M, "min_clearance", self.min_clearance)?;
        require_nonneg(M, "arrival_radius", self.arrival_radius)?;
        require_positive(M, "max_accel", self.max_accel)?;
        let product = dt * self.max_accel;
        if product > self.max_speed {
            return Err(CrowdError::CflViolation {
                model: M,
                product,
                max_speed: self.max_speed,
                max_dt: self.max_speed / self.max_accel,
            });
        }
        Ok(())
    }
}

/// Unit marker type implementing [`PedestrianModel`] for the Generalized
/// Centrifugal Force model.
#[derive(Debug, Clone, Copy, Default)]
pub struct GeneralizedCentrifugalForce;

impl PedestrianModel for GeneralizedCentrifugalForce {
    type Params = Params;

    fn name(&self) -> &'static str {
        "Generalized Centrifugal Force"
    }

    fn step(&self, peds: &mut [Pedestrian], walls: &[WallSegment], params: &Params, dt: f64) {
        #[allow(deprecated)]
        step(peds, walls, params, dt);
    }
}

/// Free-function step for callers that do not need trait dispatch.
///
/// **Deprecated.** O(n²) reference path with per-tick heap allocation.
/// Use [`step_scratch`] (zero-alloc) or [`step_with_grid`] (broadphase)
/// in production. Retained for parity tests and back-compat.
#[deprecated(
    since = "0.0.3",
    note = "O(n²) reference path with per-tick heap allocation; use \
            `step_scratch` (zero-alloc) or `step_with_grid` (broadphase) \
            instead. See docs/rustsim-crowd.md P1-7."
)]
#[allow(clippy::needless_range_loop)]
pub fn step(peds: &mut [Pedestrian], walls: &[WallSegment], params: &Params, dt: f64) {
    let n = peds.len();
    let mut accels = vec![[0.0f64; 2]; n];

    for i in 0..n {
        let p = &peds[i];
        let mut f = driving_force(p, params);

        for j in 0..n {
            if i == j {
                continue;
            }
            f = add(f, ped_force(p, &peds[j], params));
        }

        for w in walls {
            f = add(f, wall_force(p, w, params));
        }

        accels[i] = crate::social_force::cap_accel(scale(f, 1.0 / params.mass), params.max_accel);
    }

    for (p, a) in peds.iter_mut().zip(accels.iter()) {
        p.vel = add(p.vel, scale(*a, dt));
        p.vel = clamp_speed(p.vel, params.max_speed);
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Recommended neighbour cutoff radius for grid queries (metres).
///
/// Unlike the exponential-decay models, the centrifugal repulsion
/// `m * (v0 + v_approach)^2 / clearance` has only a `1/clearance`
/// tail, so truncation always introduces a small bias. The default
/// of `5 m` makes that bias negligible: at 5 m the magnitude is
/// `~28 N` (compared to `~14 kN` inside 0.1 m), i.e. well below the
/// driving force. Callers who need exact parity with the O(n²) path
/// should pass a cutoff of `f64::INFINITY` and use [`step_with_grid`]
/// only as a speed-up on dense populations.
#[inline]
pub fn neighbor_cutoff(_params: &Params) -> f64 {
    5.0
}

/// Grid-accelerated step variant. Approximates [`step`] for pairs
/// further than `neighbor_cutoff(params)`, which introduces a small
/// truncation bias on the `1/clearance` tail — see
/// [`neighbor_cutoff`] for the default justification.
#[allow(clippy::needless_range_loop)]
pub fn step_with_grid(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    grid: &NeighborGrid,
) {
    let n = peds.len();
    let cutoff = neighbor_cutoff(params);
    let mut accels = vec![[0.0f64; 2]; n];

    for i in 0..n {
        let p = &peds[i];
        let mut f = driving_force(p, params);

        grid.for_each_neighbor(i, cutoff, peds, |_j, q| {
            f = add(f, ped_force(p, q, params));
        });

        for w in walls {
            f = add(f, wall_force(p, w, params));
        }

        accels[i] = crate::social_force::cap_accel(scale(f, 1.0 / params.mass), params.max_accel);
    }

    for (p, a) in peds.iter_mut().zip(accels.iter()) {
        p.vel = add(p.vel, scale(*a, dt));
        p.vel = clamp_speed(p.vel, params.max_speed);
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Zero-allocation step variant. See
/// [`social_force::step_scratch`](crate::social_force::step_scratch)
/// for the motivation.
#[allow(clippy::needless_range_loop)]
pub fn step_scratch(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    scratch: &mut Scratch,
) {
    let n = peds.len();
    let cutoff = neighbor_cutoff(params);
    scratch.prepare(peds);
    let (accels, grid) = scratch.split();

    for i in 0..n {
        let p = &peds[i];
        let mut f = driving_force(p, params);
        grid.for_each_neighbor(i, cutoff, peds, |_j, q| {
            f = add(f, ped_force(p, q, params));
        });
        for w in walls {
            f = add(f, wall_force(p, w, params));
        }
        accels[i] = crate::social_force::cap_accel(scale(f, 1.0 / params.mass), params.max_accel);
    }

    for (p, a) in peds.iter_mut().zip(accels.iter()) {
        p.vel = add(p.vel, scale(*a, dt));
        p.vel = clamp_speed(p.vel, params.max_speed);
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Rayon-parallel drop-in replacement for [`step_scratch`].
///
/// See [`social_force::step_scratch_par`](crate::social_force::step_scratch_par)
/// for the contract: bit-exact with the serial path, each worker
/// writes only its own `accels[i]` slot from an immutable view of
/// `peds`. Enable the `rayon` feature of `rustsim-crowd` to use this.
#[cfg(feature = "rayon")]
#[allow(clippy::needless_range_loop)]
pub fn step_scratch_par(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    scratch: &mut Scratch,
) {
    use rayon::prelude::*;

    let cutoff = neighbor_cutoff(params);
    scratch.prepare(peds);
    let (accels, grid) = scratch.split();
    let peds_ro: &[Pedestrian] = peds;

    accels.par_iter_mut().enumerate().for_each(|(i, a_slot)| {
        let p = &peds_ro[i];
        let mut f = driving_force(p, params);
        grid.for_each_neighbor(i, cutoff, peds_ro, |_j, q| {
            f = add(f, ped_force(p, q, params));
        });
        for w in walls {
            f = add(f, wall_force(p, w, params));
        }
        *a_slot = crate::social_force::cap_accel(scale(f, 1.0 / params.mass), params.max_accel);
    });

    for (p, a) in peds.iter_mut().zip(accels.iter()) {
        p.vel = add(p.vel, scale(*a, dt));
        p.vel = clamp_speed(p.vel, params.max_speed);
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// SIMD-vectorised drop-in replacement for [`step_scratch`].
///
/// Lifts [`crate::simd::gcf_pair_force_x4`] into the per-agent inner
/// loop: broadphase neighbours are buffered in 4-wide index chunks
/// (indices, not refs, to keep the `for_each_neighbor` closure
/// borrow-checker-clean), the lane-summed pair-force is added to the
/// running force total, and the trailing partial chunk is flushed
/// via the helper's `None`-lane masking. Driving force, wall
/// repulsion, `cap_accel`, and the semi-implicit Euler integrator
/// stay scalar — only the inner pair-force sum changes.
///
/// Numerical contract: lane re-ordering is the only divergence
/// source vs [`step_scratch`], so this path is **tolerance-equivalent**,
/// not bit-exact — the same associativity caveat that
/// [`step_scratch_par`] documents for the rayon path. Pinned by
/// `tests/simd_tolerance.rs::gcf_step_scratch_simd_matches_scalar_within_tolerance`.
#[cfg(feature = "simd")]
#[allow(clippy::needless_range_loop)]
pub fn step_scratch_simd(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    scratch: &mut Scratch,
) {
    let n = peds.len();
    let cutoff = neighbor_cutoff(params);
    scratch.prepare(peds);
    let (accels, grid) = scratch.split();

    for i in 0..n {
        let p = &peds[i];
        let mut f = driving_force(p, params);

        let mut idxs: [Option<usize>; 4] = [None, None, None, None];
        let mut filled: usize = 0;
        grid.for_each_neighbor(i, cutoff, peds, |j, _q| {
            idxs[filled] = Some(j);
            filled += 1;
            if filled == 4 {
                let buf: [Option<&Pedestrian>; 4] = [
                    Some(&peds[idxs[0].unwrap()]),
                    Some(&peds[idxs[1].unwrap()]),
                    Some(&peds[idxs[2].unwrap()]),
                    Some(&peds[idxs[3].unwrap()]),
                ];
                let pf = crate::simd::gcf_pair_force_x4(p, buf, params);
                f = add(f, pf);
                idxs = [None, None, None, None];
                filled = 0;
            }
        });
        if filled > 0 {
            let buf: [Option<&Pedestrian>; 4] = [
                idxs[0].map(|k| &peds[k]),
                idxs[1].map(|k| &peds[k]),
                idxs[2].map(|k| &peds[k]),
                idxs[3].map(|k| &peds[k]),
            ];
            let pf = crate::simd::gcf_pair_force_x4(p, buf, params);
            f = add(f, pf);
        }

        for w in walls {
            f = add(f, wall_force(p, w, params));
        }
        accels[i] = crate::social_force::cap_accel(scale(f, 1.0 / params.mass), params.max_accel);
    }

    for (p, a) in peds.iter_mut().zip(accels.iter()) {
        p.vel = add(p.vel, scale(*a, dt));
        p.vel = clamp_speed(p.vel, params.max_speed);
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Driving force `m * (v0 * e_dest - v) / tau`.
#[inline]
pub fn driving_force(p: &Pedestrian, params: &Params) -> Vec2 {
    let target = scale(
        p.desired_direction(),
        p.effective_desired_speed(params.arrival_radius),
    );
    let delta = sub(target, p.vel);
    scale(delta, params.mass / params.tau)
}

/// Speed-dependent body radius `r(v) = a + b * |v|`.
#[inline]
pub fn body_radius(p: &Pedestrian, params: &Params) -> f64 {
    params.a + params.b * p.speed()
}

/// Centrifugal repulsion between two pedestrians.
#[inline]
pub fn ped_force(p: &Pedestrian, q: &Pedestrian, params: &Params) -> Vec2 {
    let diff = sub(p.pos, q.pos);
    let d = norm(diff);
    if d < 1e-9 {
        return [0.0, 0.0];
    }
    let e_ji = scale(diff, 1.0 / d);

    // Closing speed along e_ji (0 if receding or parallel).
    let v_rel = sub(p.vel, q.vel);
    let approach = (-dot(v_rel, e_ji)).max(0.0);

    let r_i = body_radius(p, params);
    let r_j = body_radius(q, params);
    let clearance = (d - r_i - r_j).max(params.min_clearance);

    let magnitude = params.mass * (p.desired_speed + approach).powi(2) / clearance;
    scale(e_ji, magnitude)
}

/// Centrifugal repulsion from the closest point on a wall segment.
#[inline]
pub fn wall_force(p: &Pedestrian, wall: &WallSegment, params: &Params) -> Vec2 {
    let closest = closest_point_on_segment(p.pos, wall.a, wall.b);
    let diff = sub(p.pos, closest);
    let d = norm(diff);
    if d < 1e-9 {
        return [0.0, 0.0];
    }
    let e = scale(diff, 1.0 / d);
    let r = body_radius(p, params);
    let clearance = (d - r).max(params.min_clearance);
    let magnitude = params.wall_gain * params.mass * (p.desired_speed).powi(2) / clearance;
    scale(e, magnitude)
}

#[cfg(test)]
#[allow(deprecated)] // intentional: pins grid/scratch equivalence vs the deprecated O(n²) `step`.
mod tests {
    use super::*;

    #[test]
    fn single_agent_accelerates_toward_goal() {
        let mut peds = vec![Pedestrian {
            pos: [0.0, 0.0],
            vel: [0.0, 0.0],
            radius: 0.25,
            desired_speed: 1.34,
            destination: [10.0, 0.0],
        }];
        for _ in 0..50 {
            step(&mut peds, &[], &Params::default(), 0.05);
        }
        assert!(peds[0].vel[0] > 0.5);
    }

    #[test]
    fn head_on_pair_does_not_overlap() {
        let mut peds = vec![
            Pedestrian {
                pos: [-4.0, 0.1],
                vel: [0.0, 0.0],
                radius: 0.25,
                desired_speed: 1.34,
                destination: [4.0, 0.1],
            },
            Pedestrian {
                pos: [4.0, -0.1],
                vel: [0.0, 0.0],
                radius: 0.25,
                desired_speed: 1.34,
                destination: [-4.0, -0.1],
            },
        ];
        for _ in 0..400 {
            step(&mut peds, &[], &Params::default(), 0.02);
        }
        let d = norm(sub(peds[0].pos, peds[1].pos));
        let min_sep =
            body_radius(&peds[0], &Params::default()) + body_radius(&peds[1], &Params::default());
        assert!(
            d + 0.02 >= min_sep,
            "agents too close: d={d}, min={min_sep}"
        );
    }
}