rustsim_core/

avoidance.rs

//! Physical avoidance force primitives for pedestrian and agent dynamics.
//!
//! Provides reusable, pure-function force calculations based on the Social Force
//! Model (Helbing & Molnar 1995; Helbing, Farkas & Vicsek 2000). These primitives
//! can be composed by any agent step function that needs collision-avoidance behavior.
//!
//! # Unit contract
//!
//! All functions in this module (`desired_force_*`, `social_repulsion_*`,
//! `wall_repulsion_*`) return **accelerations in m/s²**, not Newtons. The
//! Helbing-calibrated parameters (`a_social`, `k_body`, `kappa_friction`,
//! `a_wall`, `k_wall`, `kappa_wall`) are still declared in Newton-scale units
//! matching the literature (e.g. Helbing, Farkas & Vicsek 2000 Table 1), but
//! are divided internally by [`SocialForceParams::mass`] (default 80 kg per
//! Helbing 2000). This keeps the parameter dashboard literature-compatible
//! while making all force outputs dimensionally consistent with the
//! `v += a * dt` integrator used by [`integrate_euler_2d`] /
//! [`integrate_euler_3d`].
//!
//! For mixed populations (children ≈ 30 kg, adults ≈ 80 kg, equipped
//! responders > 100 kg) override `mass` on the [`SocialForceParams`] struct;
//! the returned accelerations scale as `1/mass` automatically.
//!
//! # Force components
//!
//! | Force | Description |
//! |-------|-------------|
//! | Desired | Drives agent toward its destination at desired speed |
//! | Social repulsion | Exponential repulsion from nearby agents (psychological) |
//! | Physical contact | Body compression + sliding friction when agents overlap |
//! | Wall repulsion | Exponential repulsion from wall/obstacle segments |
//! | Wall contact | Body compression + sliding friction when agent overlaps a wall |
//!
//! # References
//!
//! - Helbing, D. & Molnar, P. (1995). "Social force model for pedestrian dynamics."
//!   Physical Review E, 51(5), 4282.
//! - Helbing, D., Farkas, I. & Vicsek, T. (2000). "Simulating dynamical features
//!   of escape panic." Nature, 407(6803), 487-490.

// ---------------------------------------------------------------------------
// Parameters
// ---------------------------------------------------------------------------

/// Parameters for the Social Force Model with physical contact forces.
///
/// Default values are from Helbing, Farkas & Vicsek (2000) Table 1.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SocialForceParams {
    /// Relaxation time tau (seconds). Controls how quickly the agent adjusts
    /// its velocity toward the desired velocity. Smaller = more responsive.
    pub tau: f64,

    /// Social repulsion strength A (Newtons).
    pub a_social: f64,

    /// Social repulsion range B (meters). The exponential decay length.
    pub b_social: f64,

    /// Body compression coefficient k (kg/s^2). Activated on physical overlap.
    pub k_body: f64,

    /// Sliding friction coefficient kappa (kg/(m*s)). Tangential force on overlap.
    pub kappa_friction: f64,

    /// Wall repulsion strength A_wall (Newtons).
    pub a_wall: f64,

    /// Wall repulsion range B_wall (meters).
    pub b_wall: f64,

    /// Wall body compression coefficient (kg/s^2).
    pub k_wall: f64,

    /// Wall sliding friction coefficient (kg/(m*s)).
    pub kappa_wall: f64,

    /// Maximum allowed speed (m/s). Integration clamps speed to this value.
    pub max_speed: f64,

    /// Agent mass (kg). Used internally to convert the Helbing-calibrated
    /// Newton-scale coefficients (`a_social`, `k_body`, `kappa_friction`,
    /// `a_wall`, `k_wall`, `kappa_wall`) into accelerations (m/s²) so that
    /// the unit contract of this module (see module docs) holds.
    ///
    /// Default: 80 kg, the value used by Helbing, Farkas & Vicsek (2000).
    pub mass: f64,
}

impl Default for SocialForceParams {
    /// Default parameters from Helbing, Farkas & Vicsek (2000) Table 1.
    ///
    /// `mass` defaults to 80 kg (same reference). All coefficients are
    /// published in Newton-scale units and divided by `mass` internally
    /// when force functions are evaluated, so outputs are in m/s².
    fn default() -> Self {
        Self {
            tau: 0.5,
            a_social: 2000.0,
            b_social: 0.08,
            k_body: 1.2e5,
            kappa_friction: 2.4e5,
            a_wall: 2000.0,
            b_wall: 0.08,
            k_wall: 1.2e5,
            kappa_wall: 2.4e5,
            max_speed: 2.5,
            mass: 80.0,
        }
    }
}

/// A wall segment defined by two endpoints.
///
/// Wall segments are used for wall-repulsion force calculations. The closest
/// point on the segment to the queried position is computed analytically.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WallSegment {
    /// Start point x.
    pub x1: f64,
    /// Start point y.
    pub y1: f64,
    /// End point x.
    pub x2: f64,
    /// End point y.
    pub y2: f64,
}

impl WallSegment {
    /// Create a new wall segment from `(x1, y1)` to `(x2, y2)`.
    pub fn new(x1: f64, y1: f64, x2: f64, y2: f64) -> Self {
        Self { x1, y1, x2, y2 }
    }

    /// Closest point on this wall segment to point `(px, py)`.
    ///
    /// Uses the standard parametric line clamped to `[0, 1]`.
    pub fn closest_point(&self, px: f64, py: f64) -> (f64, f64) {
        let dx = self.x2 - self.x1;
        let dy = self.y2 - self.y1;
        let len_sq = dx * dx + dy * dy;
        if len_sq < 1e-12 {
            return (self.x1, self.y1);
        }
        let t = ((px - self.x1) * dx + (py - self.y1) * dy) / len_sq;
        let t = t.clamp(0.0, 1.0);
        (self.x1 + t * dx, self.y1 + t * dy)
    }
}

// ---------------------------------------------------------------------------
// 2D force functions
// ---------------------------------------------------------------------------

/// Desired force driving an agent toward its destination in 2D.
///
/// `f_desired = (v0 * e_hat - v) / tau`
///
/// where `e_hat` is the unit vector toward the destination, `v0` is the desired
/// speed, `v = (vx, vy)` is the current velocity, and `tau` is the relaxation
/// time.
///
/// Returns `(fx, fy)`. If the agent is already at the destination (within
/// `1e-12`), returns `(0, 0)`.
#[allow(clippy::too_many_arguments)]
pub fn desired_force_2d(
    x: f64,
    y: f64,
    vx: f64,
    vy: f64,
    dest_x: f64,
    dest_y: f64,
    desired_speed: f64,
    tau: f64,
) -> (f64, f64) {
    let dx = dest_x - x;
    let dy = dest_y - y;
    let dist = (dx * dx + dy * dy).sqrt();
    if dist < 1e-12 {
        return (0.0, 0.0);
    }
    let ex = dx / dist;
    let ey = dy / dist;
    let fx = (desired_speed * ex - vx) / tau;
    let fy = (desired_speed * ey - vy) / tau;
    (fx, fy)
}

/// Social repulsion + physical contact **acceleration** from a single neighbor in 2D.
///
/// Implements Helbing, Farkas & Vicsek (2000) Eq. 3, returning the
/// mass-normalized result (m/s²) so it can be added directly to
/// [`desired_force_2d`] and fed to [`integrate_euler_2d`]:
///
/// - Exponential social repulsion: `A * exp((r_ij - d) / B) * n_hat`
/// - Body compression (overlap only): `k * g(r_ij - d) * n_hat`
/// - Sliding friction (overlap only): `kappa * g(r_ij - d) * dv_t * t_hat`
///
/// where `r_ij = r_i + r_j` (sum of body radii), `d` is center-to-center
/// distance, `n_hat` is the unit normal pointing from the neighbor to self,
/// `g(x) = max(0, x)`, and `dv_t` is the tangential velocity difference.
/// The Newton-scale coefficients are divided by `params.mass` before
/// returning.
///
/// # Arguments
///
/// - `(x, y)` - current agent position
/// - `(vx, vy)` - current agent velocity
/// - `radius` - current agent body radius
/// - `(nx, ny)` - neighbor position
/// - `(nvx, nvy)` - neighbor velocity
/// - `n_radius` - neighbor body radius
/// - `params` - [`SocialForceParams`] (uses `mass` for normalization)
///
/// Returns `(ax, ay)` - acceleration (m/s²) on the current agent.
#[allow(clippy::too_many_arguments)]
pub fn social_repulsion_2d(
    x: f64,
    y: f64,
    vx: f64,
    vy: f64,
    radius: f64,
    nx: f64,
    ny: f64,
    nvx: f64,
    nvy: f64,
    n_radius: f64,
    params: &SocialForceParams,
) -> (f64, f64) {
    let dx = x - nx;
    let dy = y - ny;
    let dist = (dx * dx + dy * dy).sqrt();
    if dist < 1e-12 {
        return (0.0, 0.0);
    }

    let r_ij = radius + n_radius;
    // Positive when bodies overlap.
    let overlap = r_ij - dist;

    // Unit normal: neighbor -> self.
    let ux = dx / dist;
    let uy = dy / dist;

    // Tangent (perpendicular to normal, 90 degrees counterclockwise).
    let tx = -uy;
    let ty = ux;

    // Scale all Newton-scale coefficients by 1/mass so the accumulated
    // output is an acceleration in m/s² (see module-level unit contract).
    let inv_m = 1.0 / params.mass;

    // Social repulsion (always active).
    let f_social = params.a_social * (overlap / params.b_social).exp() * inv_m;
    let mut fx = f_social * ux;
    let mut fy = f_social * uy;

    // Physical contact forces (only when overlapping).
    if overlap > 0.0 {
        // Body compression: k * overlap * n_hat.
        fx += params.k_body * inv_m * overlap * ux;
        fy += params.k_body * inv_m * overlap * uy;

        // Sliding friction: kappa * overlap * dv_t * t_hat.
        let dvx = nvx - vx;
        let dvy = nvy - vy;
        let dv_tangential = dvx * tx + dvy * ty;
        fx += params.kappa_friction * inv_m * overlap * dv_tangential * tx;
        fy += params.kappa_friction * inv_m * overlap * dv_tangential * ty;
    }

    (fx, fy)
}

/// Wall repulsion + physical contact **acceleration** from a single wall segment in 2D.
///
/// Same structure as agent-agent repulsion but uses the closest point on
/// the wall segment instead of a neighbor center. Coefficients are divided
/// by `params.mass` before returning, so the result is in m/s².
///
/// # Arguments
///
/// - `(x, y)` - agent position
/// - `(vx, vy)` - agent velocity
/// - `radius` - agent body radius
/// - `wall` - wall segment
/// - `params` - [`SocialForceParams`] (uses `mass` for normalization)
///
/// Returns `(ax, ay)` - acceleration (m/s²) on the agent.
pub fn wall_repulsion_2d(
    x: f64,
    y: f64,
    vx: f64,
    vy: f64,
    radius: f64,
    wall: &WallSegment,
    params: &SocialForceParams,
) -> (f64, f64) {
    let (cx, cy) = wall.closest_point(x, y);
    let dx = x - cx;
    let dy = y - cy;
    let dist = (dx * dx + dy * dy).sqrt();
    if dist < 1e-12 {
        return (0.0, 0.0);
    }

    // Positive when agent overlaps wall.
    let overlap = radius - dist;

    // Unit normal: wall -> agent.
    let ux = dx / dist;
    let uy = dy / dist;

    // Tangent (perpendicular to normal).
    let tx = -uy;
    let ty = ux;

    let inv_m = 1.0 / params.mass;

    // Exponential repulsion (always active).
    let f_rep = params.a_wall * (overlap / params.b_wall).exp() * inv_m;
    let mut fx = f_rep * ux;
    let mut fy = f_rep * uy;

    // Physical contact (only when overlapping).
    if overlap > 0.0 {
        // Body compression.
        fx += params.k_wall * inv_m * overlap * ux;
        fy += params.k_wall * inv_m * overlap * uy;

        // Sliding friction: tangential velocity projected.
        let v_tangential = vx * tx + vy * ty;
        fx -= params.kappa_wall * inv_m * overlap * v_tangential * tx;
        fy -= params.kappa_wall * inv_m * overlap * v_tangential * ty;
    }

    (fx, fy)
}

/// Euler integration with speed clamping in 2D.
///
/// Updates velocity with the given **acceleration** (m/s²), clamps the
/// speed to `params.max_speed`, then advances position. All force primitives
/// in this module already return accelerations (they divide Newton-scale
/// coefficients by `params.mass` internally), so the sum of their outputs
/// can be passed directly as `(fx, fy)` here.
///
/// Returns `(new_x, new_y, new_vx, new_vy)`.
#[allow(clippy::too_many_arguments)]
pub fn integrate_euler_2d(
    x: f64,
    y: f64,
    vx: f64,
    vy: f64,
    fx: f64,
    fy: f64,
    dt: f64,
    params: &SocialForceParams,
) -> (f64, f64, f64, f64) {
    let mut new_vx = vx + fx * dt;
    let mut new_vy = vy + fy * dt;

    // Speed clamping.
    let speed = (new_vx * new_vx + new_vy * new_vy).sqrt();
    if speed > params.max_speed {
        let scale = params.max_speed / speed;
        new_vx *= scale;
        new_vy *= scale;
    }

    let new_x = x + new_vx * dt;
    let new_y = y + new_vy * dt;
    (new_x, new_y, new_vx, new_vy)
}

// ---------------------------------------------------------------------------
// 3D force functions
// ---------------------------------------------------------------------------

/// Desired force driving an agent toward its destination in 3D.
///
/// Same formulation as [`desired_force_2d`] extended to three dimensions.
///
/// Returns `(fx, fy, fz)`.
#[allow(clippy::too_many_arguments)]
pub fn desired_force_3d(
    x: f64,
    y: f64,
    z: f64,
    vx: f64,
    vy: f64,
    vz: f64,
    dest_x: f64,
    dest_y: f64,
    dest_z: f64,
    desired_speed: f64,
    tau: f64,
) -> (f64, f64, f64) {
    let dx = dest_x - x;
    let dy = dest_y - y;
    let dz = dest_z - z;
    let dist = (dx * dx + dy * dy + dz * dz).sqrt();
    if dist < 1e-12 {
        return (0.0, 0.0, 0.0);
    }
    let ex = dx / dist;
    let ey = dy / dist;
    let ez = dz / dist;
    let fx = (desired_speed * ex - vx) / tau;
    let fy = (desired_speed * ey - vy) / tau;
    let fz = (desired_speed * ez - vz) / tau;
    (fx, fy, fz)
}

/// Social repulsion + physical contact **acceleration** from a single neighbor in 3D.
///
/// The tangential sliding-friction direction is computed by subtracting the
/// normal component from the relative velocity difference vector.
/// Coefficients are divided by `params.mass` before returning, so the result
/// is in m/s².
///
/// Returns `(ax, ay, az)` - acceleration (m/s²) on the current agent.
#[allow(clippy::too_many_arguments)]
pub fn social_repulsion_3d(
    x: f64,
    y: f64,
    z: f64,
    vx: f64,
    vy: f64,
    vz: f64,
    radius: f64,
    nx: f64,
    ny: f64,
    nz: f64,
    nvx: f64,
    nvy: f64,
    nvz: f64,
    n_radius: f64,
    params: &SocialForceParams,
) -> (f64, f64, f64) {
    let dx = x - nx;
    let dy = y - ny;
    let dz = z - nz;
    let dist = (dx * dx + dy * dy + dz * dz).sqrt();
    if dist < 1e-12 {
        return (0.0, 0.0, 0.0);
    }

    let r_ij = radius + n_radius;
    let overlap = r_ij - dist;

    // Unit normal: neighbor -> self.
    let ux = dx / dist;
    let uy = dy / dist;
    let uz = dz / dist;

    let inv_m = 1.0 / params.mass;

    // Social repulsion.
    let f_social = params.a_social * (overlap / params.b_social).exp() * inv_m;
    let mut fx = f_social * ux;
    let mut fy = f_social * uy;
    let mut fz = f_social * uz;

    // Physical contact.
    if overlap > 0.0 {
        // Body compression.
        fx += params.k_body * inv_m * overlap * ux;
        fy += params.k_body * inv_m * overlap * uy;
        fz += params.k_body * inv_m * overlap * uz;

        // Sliding friction: tangential component of delta-v.
        let dvx = nvx - vx;
        let dvy = nvy - vy;
        let dvz = nvz - vz;
        let dv_normal = dvx * ux + dvy * uy + dvz * uz;
        let tvx = dvx - dv_normal * ux;
        let tvy = dvy - dv_normal * uy;
        let tvz = dvz - dv_normal * uz;
        fx += params.kappa_friction * inv_m * overlap * tvx;
        fy += params.kappa_friction * inv_m * overlap * tvy;
        fz += params.kappa_friction * inv_m * overlap * tvz;
    }

    (fx, fy, fz)
}

/// A 3D wall segment defined by two endpoints.
///
/// The closest point on the segment to a query position is computed
/// analytically via parametric projection.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WallSegment3D {
    /// Start point x.
    pub x1: f64,
    /// Start point y.
    pub y1: f64,
    /// Start point z.
    pub z1: f64,
    /// End point x.
    pub x2: f64,
    /// End point y.
    pub y2: f64,
    /// End point z.
    pub z2: f64,
}

impl WallSegment3D {
    /// Create a new 3D wall segment.
    pub fn new(x1: f64, y1: f64, z1: f64, x2: f64, y2: f64, z2: f64) -> Self {
        Self {
            x1,
            y1,
            z1,
            x2,
            y2,
            z2,
        }
    }

    /// Closest point on this wall segment to `(px, py, pz)`.
    pub fn closest_point(&self, px: f64, py: f64, pz: f64) -> (f64, f64, f64) {
        let dx = self.x2 - self.x1;
        let dy = self.y2 - self.y1;
        let dz = self.z2 - self.z1;
        let len_sq = dx * dx + dy * dy + dz * dz;
        if len_sq < 1e-12 {
            return (self.x1, self.y1, self.z1);
        }
        let t = ((px - self.x1) * dx + (py - self.y1) * dy + (pz - self.z1) * dz) / len_sq;
        let t = t.clamp(0.0, 1.0);
        (self.x1 + t * dx, self.y1 + t * dy, self.z1 + t * dz)
    }
}

/// Wall repulsion + physical contact **acceleration** from a 3D wall segment.
///
/// Coefficients are divided by `params.mass` before returning.
///
/// Returns `(ax, ay, az)` - acceleration (m/s²) on the agent.
#[allow(clippy::too_many_arguments)]
pub fn wall_repulsion_3d(
    x: f64,
    y: f64,
    z: f64,
    vx: f64,
    vy: f64,
    vz: f64,
    radius: f64,
    wall: &WallSegment3D,
    params: &SocialForceParams,
) -> (f64, f64, f64) {
    let (cx, cy, cz) = wall.closest_point(x, y, z);
    let dx = x - cx;
    let dy = y - cy;
    let dz = z - cz;
    let dist = (dx * dx + dy * dy + dz * dz).sqrt();
    if dist < 1e-12 {
        return (0.0, 0.0, 0.0);
    }

    let overlap = radius - dist;

    // Unit normal: wall -> agent.
    let ux = dx / dist;
    let uy = dy / dist;
    let uz = dz / dist;

    let inv_m = 1.0 / params.mass;

    // Exponential repulsion.
    let f_rep = params.a_wall * (overlap / params.b_wall).exp() * inv_m;
    let mut fx = f_rep * ux;
    let mut fy = f_rep * uy;
    let mut fz = f_rep * uz;

    // Physical contact.
    if overlap > 0.0 {
        // Body compression.
        fx += params.k_wall * inv_m * overlap * ux;
        fy += params.k_wall * inv_m * overlap * uy;
        fz += params.k_wall * inv_m * overlap * uz;

        // Sliding friction: tangential component of velocity.
        let v_normal = vx * ux + vy * uy + vz * uz;
        let tvx = vx - v_normal * ux;
        let tvy = vy - v_normal * uy;
        let tvz = vz - v_normal * uz;
        fx -= params.kappa_wall * inv_m * overlap * tvx;
        fy -= params.kappa_wall * inv_m * overlap * tvy;
        fz -= params.kappa_wall * inv_m * overlap * tvz;
    }

    (fx, fy, fz)
}

/// Euler integration with speed clamping in 3D.
///
/// Takes an **acceleration** (m/s²), updates velocity, clamps speed,
/// advances position.
///
/// Returns `(new_x, new_y, new_z, new_vx, new_vy, new_vz)`.
#[allow(clippy::too_many_arguments)]
pub fn integrate_euler_3d(
    x: f64,
    y: f64,
    z: f64,
    vx: f64,
    vy: f64,
    vz: f64,
    fx: f64,
    fy: f64,
    fz: f64,
    dt: f64,
    params: &SocialForceParams,
) -> (f64, f64, f64, f64, f64, f64) {
    let mut new_vx = vx + fx * dt;
    let mut new_vy = vy + fy * dt;
    let mut new_vz = vz + fz * dt;

    let speed = (new_vx * new_vx + new_vy * new_vy + new_vz * new_vz).sqrt();
    if speed > params.max_speed {
        let scale = params.max_speed / speed;
        new_vx *= scale;
        new_vy *= scale;
        new_vz *= scale;
    }

    let new_x = x + new_vx * dt;
    let new_y = y + new_vy * dt;
    let new_z = z + new_vz * dt;
    (new_x, new_y, new_z, new_vx, new_vy, new_vz)
}

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

    #[test]
    fn desired_force_points_toward_destination() {
        let (fx, fy) = desired_force_2d(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 1.3, 0.5);
        assert!(fx > 0.0, "force should point toward destination (+x)");
        assert!(
            fy.abs() < 1e-12,
            "no y component when destination is on x axis"
        );
    }

    #[test]
    fn desired_force_zero_at_destination() {
        let (fx, fy) = desired_force_2d(5.0, 5.0, 1.0, 1.0, 5.0, 5.0, 1.3, 0.5);
        assert!(fx.abs() < 1e-10);
        assert!(fy.abs() < 1e-10);
    }

    #[test]
    fn social_repulsion_pushes_agents_apart() {
        let params = SocialForceParams::default();
        let (fx, _fy) =
            social_repulsion_2d(1.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
        assert!(fx > 0.0, "repulsion should push agent in +x direction");
    }

    #[test]
    fn physical_contact_activates_on_overlap() {
        let params = SocialForceParams::default();
        // Two agents close together (overlap).
        let (fx_overlap, _) =
            social_repulsion_2d(0.3, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
        // Two agents far apart (no overlap).
        let (fx_far, _) =
            social_repulsion_2d(5.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
        assert!(
            fx_overlap > fx_far,
            "overlapping agents should have stronger force"
        );
    }

    #[test]
    fn wall_repulsion_pushes_away() {
        let params = SocialForceParams::default();
        let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
        // Agent above wall, close.
        let (fx, fy) = wall_repulsion_2d(5.0, 0.1, 0.0, 0.0, 0.25, &wall, &params);
        assert!(
            fx.abs() < fy.abs(),
            "force should be mostly in +y (away from wall)"
        );
        assert!(fy > 0.0, "should push agent away from wall in +y direction");
    }

    #[test]
    fn integration_clamps_speed() {
        let params = SocialForceParams {
            max_speed: 2.0,
            ..Default::default()
        };
        // Apply huge force.
        let (_, _, new_vx, new_vy) = integrate_euler_2d(0.0, 0.0, 0.0, 0.0, 1e6, 0.0, 0.1, &params);
        let speed = (new_vx * new_vx + new_vy * new_vy).sqrt();
        assert!(
            (speed - 2.0).abs() < 1e-10,
            "speed should be clamped to max_speed"
        );
    }

    #[test]
    fn wall_segment_closest_point() {
        let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
        let (cx, cy) = wall.closest_point(5.0, 3.0);
        assert!((cx - 5.0).abs() < 1e-12);
        assert!(cy.abs() < 1e-12);

        // Past the end.
        let (cx, cy) = wall.closest_point(15.0, 3.0);
        assert!((cx - 10.0).abs() < 1e-12);
        assert!(cy.abs() < 1e-12);

        // Before the start.
        let (cx, cy) = wall.closest_point(-5.0, 3.0);
        assert!(cx.abs() < 1e-12);
        assert!(cy.abs() < 1e-12);
    }

    #[test]
    fn wall_segment_3d_closest_point() {
        let wall = WallSegment3D::new(0.0, 0.0, 0.0, 10.0, 0.0, 0.0);
        let (cx, cy, cz) = wall.closest_point(5.0, 3.0, 4.0);
        assert!((cx - 5.0).abs() < 1e-12);
        assert!(cy.abs() < 1e-12);
        assert!(cz.abs() < 1e-12);
    }

    #[test]
    fn desired_force_3d_points_toward_destination() {
        let (fx, fy, fz) = desired_force_3d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 1.3, 0.5);
        assert!(fx > 0.0);
        assert!(fy.abs() < 1e-12);
        assert!(fz.abs() < 1e-12);
    }

    #[test]
    fn social_repulsion_3d_pushes_apart() {
        let params = SocialForceParams::default();
        let (fx, _, _) = social_repulsion_3d(
            1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, &params,
        );
        assert!(fx > 0.0);
    }

    #[test]
    fn integration_3d_clamps_speed() {
        let params = SocialForceParams {
            max_speed: 2.0,
            ..Default::default()
        };
        let (_, _, _, nvx, nvy, nvz) =
            integrate_euler_3d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e6, 1e6, 1e6, 0.1, &params);
        let speed = (nvx * nvx + nvy * nvy + nvz * nvz).sqrt();
        assert!((speed - 2.0).abs() < 1e-10);
    }
}