rustsim-crowd 0.0.1

//! Optimal Steps Model (Seitz & Köster 2012).
//!
//! A *discrete step* locomotion model. On each update each pedestrian
//! samples a set of candidate foot-placement points on a circle of radius
//! equal to its stride length. The candidate with the lowest scalar
//! *utility* is chosen as the next position. Utility is the sum of:
//!
//! - a target-attraction term proportional to the candidate's distance
//!   to the destination,
//! - a pairwise repulsion from every neighbour (monotonic in clearance),
//! - a wall penalty from the closest point on every wall segment.
//!
//! Unlike force-based models, position updates are step-sized rather than
//! velocity-integrated. This implementation uses a deterministic polar
//! sampling grid so the model is reproducible without an external RNG;
//! randomised sampling can be layered on by shuffling the result of
//! [`candidate_offsets`] before calling [`best_candidate`].
//!
//! # References
//!
//! - Seitz, M. J., & Köster, G. (2012). "Natural discretization of
//!   pedestrian movement in continuous space". *Physical Review E*, 86(4),
//!   046108.

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

/// Parameters for the Optimal Steps model.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Params {
    /// Number of candidate points sampled on the stride circle.
    pub num_candidates: usize,
    /// Stride length for a pedestrian walking at `desired_speed` (m).
    /// Candidate radius is clamped to `stride_at_free_flow * v/v0`.
    pub stride_at_free_flow: f64,
    /// Half-angle of the forward search cone (rad). Candidates outside this
    /// cone relative to the desired direction are discarded.
    pub cone_half_angle: f64,
    /// Weight of the destination-attraction term (per m).
    pub target_weight: f64,
    /// Strength of the pairwise pedestrian repulsion penalty.
    pub ped_strength: f64,
    /// Range of the pairwise pedestrian repulsion penalty (m).
    pub ped_range: f64,
    /// Strength of the wall repulsion penalty.
    pub wall_strength: f64,
    /// Range of the wall repulsion penalty (m).
    pub wall_range: f64,
    /// Arrival radius (m). See
    /// [`social_force::Params::arrival_radius`](crate::social_force::Params::arrival_radius).
    /// Default: 0.3 m.
    pub arrival_radius: f64,
}

impl Default for Params {
    fn default() -> Self {
        Self {
            num_candidates: 24,
            stride_at_free_flow: 0.5,
            cone_half_angle: std::f64::consts::FRAC_PI_2,
            target_weight: 1.0,
            ped_strength: 6.0,
            ped_range: 0.4,
            wall_strength: 8.0,
            wall_range: 0.3,
            arrival_radius: 0.3,
        }
    }
}

impl Params {
    /// Validate this parameter set against `dt`.
    ///
    /// OSM is a discrete-step kinematic model; there is no
    /// explicit-Euler CFL condition to check. Validation focuses on
    /// strictly-positive physical ranges and a non-zero candidate count.
    pub fn validate(&self, dt: f64) -> Result<(), crate::error::CrowdError> {
        use crate::error::{require_count, require_dt, require_nonneg, require_positive};
        const M: &str = "OptimalSteps";
        require_dt(M, dt)?;
        require_count(M, "num_candidates", self.num_candidates)?;
        require_positive(M, "stride_at_free_flow", self.stride_at_free_flow)?;
        require_positive(M, "cone_half_angle", self.cone_half_angle)?;
        require_nonneg(M, "target_weight", self.target_weight)?;
        require_nonneg(M, "ped_strength", self.ped_strength)?;
        require_positive(M, "ped_range", self.ped_range)?;
        require_nonneg(M, "wall_strength", self.wall_strength)?;
        require_positive(M, "wall_range", self.wall_range)?;
        require_nonneg(M, "arrival_radius", self.arrival_radius)?;
        Ok(())
    }
}

/// Unit marker type implementing [`PedestrianModel`] for the Optimal Steps model.
#[derive(Debug, Clone, Copy, Default)]
pub struct OptimalSteps;

impl PedestrianModel for OptimalSteps {
    type Params = Params;

    fn name(&self) -> &'static str {
        "Optimal Steps"
    }

    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.
///
/// `dt` is the duration represented by a single step; it drives stride
/// length as `v * dt` but is otherwise unused (the model is kinematic).
#[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 new_pos = vec![[0.0f64; 2]; n];

    for i in 0..n {
        let p = &peds[i];
        let stride = stride_length(p, params, dt);
        new_pos[i] = best_candidate(i, peds, walls, params, stride);
    }

    for (p, np) in peds.iter_mut().zip(new_pos.iter()) {
        let delta = sub(*np, p.pos);
        // Back-derive an effective velocity so other systems can inspect it.
        if dt > 0.0 {
            p.vel = scale(delta, 1.0 / dt);
        }
        p.pos = *np;
    }
}

/// Recommended neighbour cutoff radius for grid queries (metres).
///
/// The pairwise penalty is `ped_strength * exp(-clearance/ped_range)`.
/// At a clearance of `8 * ped_range` it has decayed to `~3.4e-4 *
/// ped_strength`, which is negligible next to the stride's target
/// weight. The cutoff adds the free-flow stride length plus a 1 m
/// buffer so candidate points sampled at stride distance still see
/// their neighbours inside the grid.
#[inline]
pub fn neighbor_cutoff(params: &Params) -> f64 {
    8.0 * params.ped_range + params.stride_at_free_flow + 1.0
}

/// Grid-accelerated step variant. Semantically equivalent to [`step`]
/// up to numerical noise for pairs inside `neighbor_cutoff(params)`.
#[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 new_pos = vec![[0.0f64; 2]; n];

    for i in 0..n {
        let p = &peds[i];
        let stride = stride_length(p, params, dt);
        new_pos[i] = best_candidate_grid(i, peds, walls, params, stride, grid, cutoff);
    }

    for (p, np) in peds.iter_mut().zip(new_pos.iter()) {
        let delta = sub(*np, p.pos);
        if dt > 0.0 {
            p.vel = scale(delta, 1.0 / dt);
        }
        p.pos = *np;
    }
}

/// 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 (new_pos, grid) = scratch.split();

    for i in 0..n {
        let p = &peds[i];
        let stride = stride_length(p, params, dt);
        new_pos[i] = best_candidate_grid(i, peds, walls, params, stride, grid, cutoff);
    }

    for (p, np) in peds.iter_mut().zip(new_pos.iter()) {
        let delta = sub(*np, p.pos);
        if dt > 0.0 {
            p.vel = scale(delta, 1.0 / dt);
        }
        p.pos = *np;
    }
}

/// Rayon-parallel drop-in replacement for [`step_scratch`].
///
/// See [`social_force::step_scratch_par`](crate::social_force::step_scratch_par)
/// for the contract. Each worker writes a distinct `new_pos[i]`
/// from an immutable view of `peds`. Enable the `rayon` feature to
/// use this entry point.
#[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 (new_pos, grid) = scratch.split();
    let peds_ro: &[Pedestrian] = peds;

    new_pos.par_iter_mut().enumerate().for_each(|(i, p_slot)| {
        let p = &peds_ro[i];
        let stride = stride_length(p, params, dt);
        *p_slot = best_candidate_grid(i, peds_ro, walls, params, stride, grid, cutoff);
    });

    for (p, np) in peds.iter_mut().zip(new_pos.iter()) {
        let delta = sub(*np, p.pos);
        if dt > 0.0 {
            p.vel = scale(delta, 1.0 / dt);
        }
        p.pos = *np;
    }
}

/// Grid-backed version of [`utility`].
fn utility_grid(
    candidate: Vec2,
    self_idx: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    grid: &NeighborGrid,
    cutoff: f64,
) -> f64 {
    let p = &peds[self_idx];
    let to_target = norm(sub(p.destination, candidate));
    let mut score = params.target_weight * to_target;

    grid.for_each_neighbor(self_idx, cutoff, peds, |_j, q| {
        let d = norm(sub(candidate, q.pos));
        let clearance = (d - (p.radius + q.radius)).max(0.0);
        score += params.ped_strength * (-clearance / params.ped_range).exp();
    });

    for w in walls {
        let closest = closest_point_on_segment(candidate, w.a, w.b);
        let d = norm(sub(candidate, closest));
        let clearance = (d - p.radius).max(0.0);
        score += params.wall_strength * (-clearance / params.wall_range).exp();
    }

    score
}

/// Grid-backed version of [`best_candidate`].
fn best_candidate_grid(
    self_idx: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    stride: f64,
    grid: &NeighborGrid,
    cutoff: f64,
) -> Vec2 {
    let p = &peds[self_idx];
    let mut best_pos = p.pos;
    let mut best_score = utility_grid(p.pos, self_idx, peds, walls, params, grid, cutoff);

    let e_dest = p.desired_direction();
    let cos_cutoff = params.cone_half_angle.cos();
    let dest_present = e_dest != [0.0, 0.0];

    for offset in candidate_offsets(stride, params.num_candidates) {
        if dest_present {
            let dir = normalize(offset);
            if dir[0] * e_dest[0] + dir[1] * e_dest[1] < cos_cutoff {
                continue;
            }
        }
        let candidate = add(p.pos, offset);
        let score = utility_grid(candidate, self_idx, peds, walls, params, grid, cutoff);
        if score < best_score {
            best_score = score;
            best_pos = candidate;
        }
    }

    best_pos
}

/// Effective stride length for pedestrian `p` over one `dt`-second step.
#[inline]
pub fn stride_length(p: &Pedestrian, params: &Params, dt: f64) -> f64 {
    // Stride scales with actual intent speed; default to the
    // arrival-tapered desired speed if the pedestrian is stationary
    // (e.g. first tick). Inside the arrival radius this shrinks
    // smoothly toward zero so agents do not overshoot their goal.
    let tapered = p.effective_desired_speed(params.arrival_radius);
    let intended = tapered.max(p.speed());
    let stride_free = params.stride_at_free_flow.max(1e-3);
    let stride = stride_free * (intended / p.desired_speed.max(1e-6));
    // Clamp stride so a huge `dt` does not let the pedestrian teleport.
    stride.min(intended * dt)
}

/// Deterministic polar sampling of candidate offsets around the origin on a
/// circle of the given `stride` radius.
pub fn candidate_offsets(stride: f64, num: usize) -> Vec<Vec2> {
    let mut out = Vec::with_capacity(num);
    if num == 0 || stride <= 0.0 {
        return out;
    }
    let two_pi = std::f64::consts::TAU;
    for k in 0..num {
        let theta = (k as f64) * two_pi / (num as f64);
        out.push([stride * theta.cos(), stride * theta.sin()]);
    }
    out
}

/// Utility score for a candidate point (lower is better).
pub fn utility(
    candidate: Vec2,
    self_idx: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
) -> f64 {
    let p = &peds[self_idx];
    let to_target = norm(sub(p.destination, candidate));
    let mut score = params.target_weight * to_target;

    for (j, q) in peds.iter().enumerate() {
        if j == self_idx {
            continue;
        }
        let d = norm(sub(candidate, q.pos));
        let clearance = (d - (p.radius + q.radius)).max(0.0);
        score += params.ped_strength * (-clearance / params.ped_range).exp();
    }

    for w in walls {
        let closest = closest_point_on_segment(candidate, w.a, w.b);
        let d = norm(sub(candidate, closest));
        let clearance = (d - p.radius).max(0.0);
        score += params.wall_strength * (-clearance / params.wall_range).exp();
    }

    score
}

/// Returns the best candidate position for pedestrian `self_idx`.
pub fn best_candidate(
    self_idx: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    stride: f64,
) -> Vec2 {
    let p = &peds[self_idx];
    // Always evaluate the "stand still" candidate so that a pedestrian at
    // its destination does not wander off the discrete ring.
    let mut best_pos = p.pos;
    let mut best_score = utility(p.pos, self_idx, peds, walls, params);

    let e_dest = p.desired_direction();
    let cos_cutoff = params.cone_half_angle.cos();
    let dest_present = e_dest != [0.0, 0.0];

    for offset in candidate_offsets(stride, params.num_candidates) {
        // Restrict to forward cone toward the goal when a goal exists.
        if dest_present {
            let dir = normalize(offset);
            if dir[0] * e_dest[0] + dir[1] * e_dest[1] < cos_cutoff {
                continue;
            }
        }
        let candidate = add(p.pos, offset);
        let score = utility(candidate, self_idx, peds, walls, params);
        if score < best_score {
            best_score = score;
            best_pos = candidate;
        }
    }

    best_pos
}

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

    #[test]
    fn single_agent_advances_toward_goal() {
        let mut peds = vec![Pedestrian {
            pos: [0.0, 0.0],
            vel: [0.0, 0.0],
            radius: 0.2,
            desired_speed: 1.34,
            destination: [10.0, 0.0],
        }];
        let start = peds[0].pos[0];
        for _ in 0..40 {
            step(&mut peds, &[], &Params::default(), 0.4);
        }
        assert!(peds[0].pos[0] > start + 1.0);
        assert!(peds[0].pos[1].abs() < 0.2);
    }

    #[test]
    fn wall_repels_in_utility() {
        let peds = vec![Pedestrian {
            pos: [0.0, 0.0],
            vel: [0.0, 0.0],
            radius: 0.2,
            desired_speed: 1.0,
            destination: [5.0, 0.0],
        }];
        let walls = vec![WallSegment {
            a: [1.0, -0.1],
            b: [1.0, 0.1],
        }];
        let params = Params::default();
        let near = utility([0.9, 0.0], 0, &peds, &walls, &params);
        let far = utility([0.0, 0.0], 0, &peds, &walls, &params);
        assert!(near > far, "wall penalty should make near-wall worse");
    }

    #[test]
    fn stationary_agent_at_destination_does_not_drift() {
        let mut peds = vec![Pedestrian {
            pos: [1.0, 1.0],
            vel: [0.0, 0.0],
            radius: 0.2,
            desired_speed: 1.0,
            destination: [1.0, 1.0],
        }];
        for _ in 0..10 {
            step(&mut peds, &[], &Params::default(), 0.4);
        }
        let d = norm(sub(peds[0].pos, [1.0, 1.0]));
        assert!(d < 0.05);
    }
}