rustsim-crowd 0.0.1

//! SIMD-accelerated inner-loop helpers for `rustsim-crowd`.
//!
//! Closes the upstream-decision blocker for P1-5 in
//! `docs/rustsim-crowd.md`. The workspace evaluated three vectorisation
//! routes:
//!
//! 1. **Nightly `std::simd` (`portable_simd`).** Cleanest API, zero
//!    new deps, naturally portable. Blocked by MSRV 1.94.0 stable;
//!    enabling would force a workspace MSRV bump or a split nightly
//!    CI lane purely for one feature.
//! 2. **[`wide`] crate.** Stable, MIT, ~6 KB API (`f64x4`/`f32x8`),
//!    single transitive dep (`bytemuck`). Cross-platform (x86_64
//!    SSE/AVX, aarch64 NEON, wasm SIMD128) through the same source.
//!    Safe API; no `unsafe` at call sites.
//! 3. **`core::arch::x86_64` AVX2 intrinsics.** Maximum control,
//!    zero deps, but `unsafe` and x86_64-only — needs an aarch64
//!    NEON fallback. Highest implementation + audit cost.
//!
//! **Decision: `wide`.** It is the only option that stays on stable
//! today, ships portable across the architectures the workspace
//! targets, keeps the call sites safe, and ports trivially when
//! `std::simd` stabilises (the lane abstraction is identical). The
//! single transitive dep on `bytemuck` is a deliberate, well-scoped
//! addition.
//!
//! # Scope of this module
//!
//! Provides vectorised helpers for the per-model neighbour scans. The
//! caller remains in charge of grid-cell traversal, diagonal masks,
//! wall terms, and integration; this module turns the per-neighbour
//! arithmetic into 4-wide `f64x4` lanes.
//!
//! # Numerical contract
//!
//! Lane-summation order differs from the scalar accumulator, so the
//! SIMD path is **not** bit-exact with the scalar one — only
//! tolerance-equivalent (`tests/simd_tolerance.rs` pins this
//! contract). This matches the rayon-parallel kernels which document
//! the same per-tick associativity caveat.

#![cfg(feature = "simd")]

use wide::{f64x4, CmpGt};

use crate::common::Pedestrian;
use crate::social_force::Params;

#[inline]
fn one_mask(mask: wide::f64x4) -> f64x4 {
    mask & f64x4::splat(f64::from_bits(0x3FF0_0000_0000_0000))
}

/// SIMD-vectorised SFM pair-repulsion helper.
///
/// Computes `ped_repulsion(p, q_k, params)` for `k in 0..4` in 4-wide
/// `f64x4` lanes and accumulates into the running `(fx, fy)` sum.
///
/// Each `q_k` may be `None` (e.g. the diagonal slot or a partial
/// chunk at the end of a neighbour list); `None` lanes contribute
/// nothing. The implementation realises this by setting a "valid"
/// mask: invalid lanes are computed against a placeholder neighbour
/// far away (so the exponential underflows to ~0) and then
/// arithmetically zeroed out at the end. The mask is the only
/// branch in the hot path; everything else is straight-line.
///
/// # Lane layout
///
/// All eight scalar inputs (`q.pos.x`, `q.pos.y`, `q.radius`) plus the
/// `valid` flag are laid out across the 4 SIMD lanes. The fixed `p`
/// fields are broadcast into all 4 lanes.
///
/// # Numerical contract
///
/// Returns `(sum_fx, sum_fy)` matching the scalar
/// `Σ ped_repulsion(p, q_k, params)` to within ~1e-12 relative
/// tolerance under default `Params`. See `tests/simd_tolerance.rs`.
#[inline]
pub fn pair_force_x4(
    p: &Pedestrian,
    neighbours: [Option<&Pedestrian>; 4],
    params: &Params,
) -> [f64; 2] {
    // Broadcast `p` into all 4 lanes.
    let pxi = f64x4::splat(p.pos[0]);
    let pyi = f64x4::splat(p.pos[1]);
    let ri = f64x4::splat(p.radius);
    let a_ped = f64x4::splat(params.a_ped);
    let b_ped = f64x4::splat(params.b_ped);

    // Pack q-side columns into 4-lane arrays; mark invalid lanes with
    // a sentinel position far away so the exponential underflows to
    // a negligible magnitude. Then zero them out via the valid mask
    // for absolute correctness on the boundary lanes.
    let mut qxs = [0.0f64; 4];
    let mut qys = [0.0f64; 4];
    let mut qrs = [0.0f64; 4];
    let mut valid = [0.0f64; 4];
    for (k, slot) in neighbours.iter().enumerate() {
        if let Some(q) = slot {
            qxs[k] = q.pos[0];
            qys[k] = q.pos[1];
            qrs[k] = q.radius;
            valid[k] = 1.0;
        } else {
            // Placeholder at +1e9 metres on x: makes exp((r - d)/B)
            // a number around exp(-1e9 / B) which is a hard zero in
            // f64. The valid-mask below makes this defensive only.
            qxs[k] = p.pos[0] + 1.0e9;
            qys[k] = p.pos[1];
            qrs[k] = 0.0;
            valid[k] = 0.0;
        }
    }
    let qx = f64x4::new(qxs);
    let qy = f64x4::new(qys);
    let qr = f64x4::new(qrs);
    let mask = f64x4::new(valid);

    // diff = p - q
    let dx = pxi - qx;
    let dy = pyi - qy;
    let d2 = dx * dx + dy * dy;
    let d = d2.sqrt();

    // Skip the degenerate-overlap case scalar-faithfully: lanes with
    // `d < 1e-9` should contribute zero. Multiply by an extra mask
    // built from `d > 1e-9` to enforce that without branching.
    let eps = f64x4::splat(1.0e-9);
    let nondegen = one_mask(d.cmp_gt(eps)); // 1.0 on true, 0.0 on false

    let r_sum = ri + qr;
    let inv_d = f64x4::splat(1.0) / (d + f64x4::splat(1.0e-300)); // additive ε guards lanes already masked off
    let ex = dx * inv_d;
    let ey = dy * inv_d;
    let arg = (r_sum - d) / b_ped;
    let mag = a_ped * arg.exp();

    // Apply both masks (lane-validity from the caller + non-degenerate
    // distance) to the magnitude. Sum across the 4 lanes.
    let mag = mag * mask * nondegen;
    let fx = mag * ex;
    let fy = mag * ey;
    let fx_arr = fx.to_array();
    let fy_arr = fy.to_array();
    let sum_fx = fx_arr[0] + fx_arr[1] + fx_arr[2] + fx_arr[3];
    let sum_fy = fy_arr[0] + fy_arr[1] + fy_arr[2] + fy_arr[3];
    [sum_fx, sum_fy]
}

/// SIMD-vectorised GCF pair-repulsion helper.
///
/// Computes `generalized_centrifugal_force::ped_force(p, q_k, params)`
/// for `k in 0..4` in 4-wide `f64x4` lanes and accumulates into
/// `(sum_fx, sum_fy)`. Mirrors the lane-mask + degenerate-overlap
/// handling of [`pair_force_x4`]: invalid lanes (`None` slots) and
/// near-zero-distance lanes are arithmetically zeroed.
///
/// Fixed `p`-side scalars (`p.pos`, `p.vel`, `p.desired_speed`,
/// `r_i = a + b * |p.vel|`) are broadcast into all 4 lanes; the
/// per-lane `q.pos`, `q.vel`, and `r_j = a + b * |q.vel|` differ
/// across lanes. The closing-speed `max(0, -dot(v_rel, e_ji))` is
/// realised with a `cmp_gt(0)` mask, matching the scalar
/// `(-dot(v_rel, e_ji)).max(0.0)`.
///
/// # Numerical contract
///
/// Returns `(sum_fx, sum_fy)` matching the scalar
/// `Σ ped_force(p, q_k, params)` to within ~1e-9 absolute tolerance
/// under default GCF `Params`. See `tests/simd_tolerance.rs`.
#[inline]
pub fn gcf_pair_force_x4(
    p: &Pedestrian,
    neighbours: [Option<&Pedestrian>; 4],
    params: &crate::generalized_centrifugal_force::Params,
) -> [f64; 2] {
    use crate::common::norm;

    // Broadcast `p` into all 4 lanes. `r_i` is constant across lanes
    // (depends only on `p.vel`), so we precompute it scalar-side.
    let pxi = f64x4::splat(p.pos[0]);
    let pyi = f64x4::splat(p.pos[1]);
    let pvxi = f64x4::splat(p.vel[0]);
    let pvyi = f64x4::splat(p.vel[1]);
    let r_i = f64x4::splat(params.a + params.b * norm(p.vel));
    let v0 = f64x4::splat(p.desired_speed);
    let mass = f64x4::splat(params.mass);
    let min_clearance = f64x4::splat(params.min_clearance);

    let mut qxs = [0.0f64; 4];
    let mut qys = [0.0f64; 4];
    let mut qvxs = [0.0f64; 4];
    let mut qvys = [0.0f64; 4];
    let mut rjs = [0.0f64; 4];
    let mut valid = [0.0f64; 4];
    for (k, slot) in neighbours.iter().enumerate() {
        if let Some(q) = slot {
            qxs[k] = q.pos[0];
            qys[k] = q.pos[1];
            qvxs[k] = q.vel[0];
            qvys[k] = q.vel[1];
            rjs[k] = params.a + params.b * norm(q.vel);
            valid[k] = 1.0;
        } else {
            // Sentinel far away so the centrifugal magnitude is
            // negligible; the valid-mask zeroes it defensively.
            qxs[k] = p.pos[0] + 1.0e9;
            qys[k] = p.pos[1];
            qvxs[k] = 0.0;
            qvys[k] = 0.0;
            rjs[k] = 0.0;
            valid[k] = 0.0;
        }
    }
    let qx = f64x4::new(qxs);
    let qy = f64x4::new(qys);
    let qvx = f64x4::new(qvxs);
    let qvy = f64x4::new(qvys);
    let r_j = f64x4::new(rjs);
    let mask = f64x4::new(valid);

    let dx = pxi - qx;
    let dy = pyi - qy;
    let d2 = dx * dx + dy * dy;
    let d = d2.sqrt();

    let eps = f64x4::splat(1.0e-9);
    let nondegen = one_mask(d.cmp_gt(eps));

    let inv_d = f64x4::splat(1.0) / (d + f64x4::splat(1.0e-300));
    let ex = dx * inv_d;
    let ey = dy * inv_d;

    // approach = max(0, -dot(v_rel, e_ji)) where v_rel = p.vel - q.vel
    let vrx = pvxi - qvx;
    let vry = pvyi - qvy;
    let neg_dot = -(vrx * ex + vry * ey);
    let pos_mask = one_mask(neg_dot.cmp_gt(f64x4::splat(0.0)));
    let approach = neg_dot * pos_mask;

    // clearance = max(d - r_i - r_j, min_clearance)
    let raw_clearance = d - r_i - r_j;
    let above = one_mask(raw_clearance.cmp_gt(min_clearance));
    let one = f64x4::splat(1.0);
    let clearance = raw_clearance * above + min_clearance * (one - above);

    let speed_term = v0 + approach;
    let mag = mass * speed_term * speed_term / clearance;
    let mag = mag * mask * nondegen;
    let fx = mag * ex;
    let fy = mag * ey;
    let fx_arr = fx.to_array();
    let fy_arr = fy.to_array();
    [
        fx_arr[0] + fx_arr[1] + fx_arr[2] + fx_arr[3],
        fy_arr[0] + fy_arr[1] + fy_arr[2] + fy_arr[3],
    ]
}

/// SIMD-vectorised Collision-Free Speed direction contribution helper.
///
/// Computes the neighbour-repulsion part of CFS `chosen_direction` for
/// up to four neighbours and returns the lane-summed `(x, y)`
/// contribution to the direction accumulator. Invalid lanes and
/// near-degenerate overlaps contribute zero, matching the scalar path.
#[inline]
pub fn cfs_direction_x4(
    p: &Pedestrian,
    neighbours: [Option<&Pedestrian>; 4],
    params: &crate::collision_free_speed::Params,
) -> [f64; 2] {
    let pxi = f64x4::splat(p.pos[0]);
    let pyi = f64x4::splat(p.pos[1]);
    let ri = f64x4::splat(p.radius);
    let strength = f64x4::splat(params.interaction_strength);
    let range = f64x4::splat(params.interaction_range);

    let mut qxs = [0.0f64; 4];
    let mut qys = [0.0f64; 4];
    let mut qrs = [0.0f64; 4];
    let mut valid = [0.0f64; 4];
    for (k, slot) in neighbours.iter().enumerate() {
        if let Some(q) = slot {
            qxs[k] = q.pos[0];
            qys[k] = q.pos[1];
            qrs[k] = q.radius;
            valid[k] = 1.0;
        } else {
            qxs[k] = p.pos[0] + 1.0e9;
            qys[k] = p.pos[1];
            qrs[k] = 0.0;
            valid[k] = 0.0;
        }
    }

    let qx = f64x4::new(qxs);
    let qy = f64x4::new(qys);
    let qr = f64x4::new(qrs);
    let lane_valid = f64x4::new(valid);

    let dx = pxi - qx;
    let dy = pyi - qy;
    let d = (dx * dx + dy * dy).sqrt();
    let nondegen = one_mask(d.cmp_gt(f64x4::splat(1.0e-9)));

    let inv_d = f64x4::splat(1.0) / (d + f64x4::splat(1.0e-300));
    let ex = dx * inv_d;
    let ey = dy * inv_d;
    let raw_clearance = d - (ri + qr);
    let positive = one_mask(raw_clearance.cmp_gt(f64x4::splat(0.0)));
    let clearance = raw_clearance * positive;
    let weight = strength * (-(clearance / range)).exp() * lane_valid * nondegen;
    let fx = (weight * ex).to_array();
    let fy = (weight * ey).to_array();
    [fx[0] + fx[1] + fx[2] + fx[3], fy[0] + fy[1] + fy[2] + fy[3]]
}

/// SIMD-vectorised Collision-Free Speed headroom helper.
///
/// Returns the minimum centre-to-centre distance among neighbours that
/// are in front of `p` along `e` and whose lateral displacement overlaps
/// the combined body radius. If no lane qualifies, returns infinity.
#[inline]
pub fn cfs_headroom_x4(p: &Pedestrian, e: [f64; 2], neighbours: [Option<&Pedestrian>; 4]) -> f64 {
    let pxi = f64x4::splat(p.pos[0]);
    let pyi = f64x4::splat(p.pos[1]);
    let ex = f64x4::splat(e[0]);
    let ey = f64x4::splat(e[1]);
    let ri = f64x4::splat(p.radius);

    let mut qxs = [0.0f64; 4];
    let mut qys = [0.0f64; 4];
    let mut qrs = [0.0f64; 4];
    let mut valid = [false; 4];
    for (k, slot) in neighbours.iter().enumerate() {
        if let Some(q) = slot {
            qxs[k] = q.pos[0];
            qys[k] = q.pos[1];
            qrs[k] = q.radius;
            valid[k] = true;
        } else {
            qxs[k] = p.pos[0] + 1.0e9;
            qys[k] = p.pos[1];
            qrs[k] = 0.0;
        }
    }

    let rel_x = f64x4::new(qxs) - pxi;
    let rel_y = f64x4::new(qys) - pyi;
    let forward = rel_x * ex + rel_y * ey;
    let proj_x = ex * forward;
    let proj_y = ey * forward;
    let lat_x = rel_x - proj_x;
    let lat_y = rel_y - proj_y;
    let lat = (lat_x * lat_x + lat_y * lat_y).sqrt();
    let d = (rel_x * rel_x + rel_y * rel_y).sqrt();
    let r_sum = ri + f64x4::new(qrs);

    let forward = forward.to_array();
    let lat = lat.to_array();
    let d = d.to_array();
    let r_sum = r_sum.to_array();
    let mut best = f64::INFINITY;
    for k in 0..4 {
        if valid[k] && forward[k] > 0.0 && lat[k] <= r_sum[k] && d[k] < best {
            best = d[k];
        }
    }
    best
}

/// SIMD-vectorised Anticipation Velocity headroom helper.
///
/// This mirrors [`cfs_headroom_x4`] but evaluates neighbour positions
/// after the AVM look-ahead horizon (`q.pos + q.vel * anticipation_time`).
/// It returns the minimum predicted centre-to-centre distance for lanes
/// that lie in front of `p` along `e` and overlap the combined body radius.
#[inline]
pub fn avm_headroom_x4(
    p: &Pedestrian,
    e: [f64; 2],
    neighbours: [Option<&Pedestrian>; 4],
    params: &crate::anticipation_velocity::Params,
) -> f64 {
    let pxi = f64x4::splat(p.pos[0]);
    let pyi = f64x4::splat(p.pos[1]);
    let ex = f64x4::splat(e[0]);
    let ey = f64x4::splat(e[1]);
    let ri = f64x4::splat(p.radius);
    let anticipation_time = f64x4::splat(params.anticipation_time);

    let mut qxs = [0.0f64; 4];
    let mut qys = [0.0f64; 4];
    let mut qvxs = [0.0f64; 4];
    let mut qvys = [0.0f64; 4];
    let mut qrs = [0.0f64; 4];
    let mut valid = [false; 4];
    for (k, slot) in neighbours.iter().enumerate() {
        if let Some(q) = slot {
            qxs[k] = q.pos[0];
            qys[k] = q.pos[1];
            qvxs[k] = q.vel[0];
            qvys[k] = q.vel[1];
            qrs[k] = q.radius;
            valid[k] = true;
        } else {
            qxs[k] = p.pos[0] + 1.0e9;
            qys[k] = p.pos[1];
            qvxs[k] = 0.0;
            qvys[k] = 0.0;
            qrs[k] = 0.0;
        }
    }

    let q_future_x = f64x4::new(qxs) + f64x4::new(qvxs) * anticipation_time;
    let q_future_y = f64x4::new(qys) + f64x4::new(qvys) * anticipation_time;
    let rel_x = q_future_x - pxi;
    let rel_y = q_future_y - pyi;
    let forward = rel_x * ex + rel_y * ey;
    let proj_x = ex * forward;
    let proj_y = ey * forward;
    let lat_x = rel_x - proj_x;
    let lat_y = rel_y - proj_y;
    let lat = (lat_x * lat_x + lat_y * lat_y).sqrt();
    let d = (rel_x * rel_x + rel_y * rel_y).sqrt();
    let r_sum = ri + f64x4::new(qrs);

    let forward = forward.to_array();
    let lat = lat.to_array();
    let d = d.to_array();
    let r_sum = r_sum.to_array();
    let mut best = f64::INFINITY;
    for k in 0..4 {
        if valid[k] && forward[k] > 0.0 && lat[k] <= r_sum[k] && d[k] < best {
            best = d[k];
        }
    }
    best
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::common::Pedestrian;
    use crate::social_force::ped_repulsion;

    /// Helper: build a small fixture of one focal `p` plus 4 neighbours
    /// at varying distances and radii.
    fn fixture() -> (Pedestrian, [Pedestrian; 4]) {
        let p = Pedestrian::new([0.0, 0.0], [0.5, 0.0], 0.25, 1.34, [50.0, 0.0]);
        let q = [
            Pedestrian::new([0.4, 0.1], [0.0, 0.0], 0.20, 1.34, [50.0, 0.0]),
            Pedestrian::new([-0.6, 0.05], [0.0, 0.0], 0.30, 1.34, [50.0, 0.0]),
            Pedestrian::new([0.0, 1.5], [0.0, 0.0], 0.25, 1.34, [50.0, 0.0]),
            Pedestrian::new([3.0, -2.5], [0.0, 0.0], 0.25, 1.34, [50.0, 0.0]),
        ];
        (p, q)
    }

    #[test]
    fn simd_pair_force_matches_scalar_within_tolerance() {
        let (p, qs) = fixture();
        let params = Params::default();
        let scalar: [f64; 2] = qs
            .iter()
            .map(|q| ped_repulsion(&p, q, &params))
            .fold([0.0, 0.0], |acc, f| [acc[0] + f[0], acc[1] + f[1]]);
        let simd = pair_force_x4(
            &p,
            [Some(&qs[0]), Some(&qs[1]), Some(&qs[2]), Some(&qs[3])],
            &params,
        );
        let dx = (simd[0] - scalar[0]).abs();
        let dy = (simd[1] - scalar[1]).abs();
        assert!(
            dx < 1.0e-9 && dy < 1.0e-9,
            "SIMD pair force diverged from scalar baseline: scalar={:?} simd={:?} (dx={dx}, dy={dy})",
            scalar,
            simd
        );
    }

    #[test]
    fn simd_pair_force_with_partial_chunk_zeroes_invalid_lanes() {
        // A 2-of-4 chunk: only the first two slots are real
        // neighbours. The SIMD result must match the scalar sum over
        // just those two.
        let (p, qs) = fixture();
        let params = Params::default();
        let scalar: [f64; 2] = qs
            .iter()
            .take(2)
            .map(|q| ped_repulsion(&p, q, &params))
            .fold([0.0, 0.0], |acc, f| [acc[0] + f[0], acc[1] + f[1]]);
        let simd = pair_force_x4(&p, [Some(&qs[0]), Some(&qs[1]), None, None], &params);
        let dx = (simd[0] - scalar[0]).abs();
        let dy = (simd[1] - scalar[1]).abs();
        assert!(
            dx < 1.0e-9 && dy < 1.0e-9,
            "SIMD partial-chunk diverged: scalar={:?} simd={:?} (dx={dx}, dy={dy})",
            scalar,
            simd
        );
    }

    #[test]
    fn simd_pair_force_with_self_overlap_returns_zero() {
        // A neighbour positioned exactly at `p` (degenerate overlap)
        // must contribute zero on both the scalar and SIMD paths.
        let p = Pedestrian::new([1.0, 1.0], [0.0, 0.0], 0.25, 1.34, [50.0, 0.0]);
        let q = Pedestrian::new([1.0, 1.0], [0.0, 0.0], 0.25, 1.34, [50.0, 0.0]);
        let params = Params::default();
        let simd = pair_force_x4(&p, [Some(&q), None, None, None], &params);
        assert!(
            simd[0].abs() < 1.0e-12 && simd[1].abs() < 1.0e-12,
            "SIMD self-overlap should be zero, got {:?}",
            simd
        );
    }

    #[test]
    fn simd_gcf_pair_force_matches_scalar_within_tolerance() {
        use crate::generalized_centrifugal_force::{ped_force, Params as GcfParams};
        // Focal agent moving right; mix of approaching and receding neighbours
        // plus one with non-zero velocity to exercise the v_rel branch.
        let p = Pedestrian::new([0.0, 0.0], [1.0, 0.0], 0.25, 1.34, [50.0, 0.0]);
        let qs = [
            Pedestrian::new([0.7, 0.05], [-0.8, 0.0], 0.25, 1.34, [-50.0, 0.0]),
            Pedestrian::new([-0.6, 0.1], [1.2, 0.0], 0.30, 1.34, [50.0, 0.0]),
            Pedestrian::new([0.0, 1.5], [0.0, -0.5], 0.25, 1.34, [50.0, -50.0]),
            Pedestrian::new([3.0, -2.5], [0.0, 0.0], 0.25, 1.34, [-50.0, 50.0]),
        ];
        let params = GcfParams::default();
        let scalar: [f64; 2] = qs
            .iter()
            .map(|q| ped_force(&p, q, &params))
            .fold([0.0, 0.0], |acc, f| [acc[0] + f[0], acc[1] + f[1]]);
        let simd = gcf_pair_force_x4(
            &p,
            [Some(&qs[0]), Some(&qs[1]), Some(&qs[2]), Some(&qs[3])],
            &params,
        );
        let dx = (simd[0] - scalar[0]).abs();
        let dy = (simd[1] - scalar[1]).abs();
        assert!(
            dx < 1.0e-9 && dy < 1.0e-9,
            "SIMD GCF pair force diverged from scalar: scalar={:?} simd={:?} (dx={dx}, dy={dy})",
            scalar,
            simd
        );
    }

    #[test]
    fn simd_gcf_pair_force_with_partial_chunk_zeroes_invalid_lanes() {
        use crate::generalized_centrifugal_force::{ped_force, Params as GcfParams};
        let p = Pedestrian::new([0.0, 0.0], [1.0, 0.0], 0.25, 1.34, [50.0, 0.0]);
        let qs = [
            Pedestrian::new([0.7, 0.05], [-0.8, 0.0], 0.25, 1.34, [-50.0, 0.0]),
            Pedestrian::new([-0.6, 0.1], [1.2, 0.0], 0.30, 1.34, [50.0, 0.0]),
        ];
        let params = GcfParams::default();
        let scalar: [f64; 2] = qs
            .iter()
            .map(|q| ped_force(&p, q, &params))
            .fold([0.0, 0.0], |acc, f| [acc[0] + f[0], acc[1] + f[1]]);
        let simd = gcf_pair_force_x4(&p, [Some(&qs[0]), Some(&qs[1]), None, None], &params);
        let dx = (simd[0] - scalar[0]).abs();
        let dy = (simd[1] - scalar[1]).abs();
        assert!(
            dx < 1.0e-9 && dy < 1.0e-9,
            "SIMD GCF partial-chunk diverged: scalar={:?} simd={:?} (dx={dx}, dy={dy})",
            scalar,
            simd
        );
    }
}