alice-physics 2.2.0

//! Sub-stepping temporal Gauss-Seidel solver primitives with impulse
//! warm-starting and connected-component islands.
//!
//! This module provides trait-abstracted building blocks that layer on
//! top of any concrete body/contact representation. The design is
//! independent of the existing PGS-style solver in [`crate::solver`]
//! so that both can coexist during a gradual migration.
//!
//! The three techniques implemented here are standard textbook
//! rigid-body dynamics:
//!
//! * **Impulse warm-starting** — the applied impulse from the previous
//!   frame is stored per stable contact ID and re-applied as the
//!   initial guess of the next frame's velocity iteration, which
//!   collapses convergence for stationary pile-ups.
//! * **Sub-stepping TGS** — a physics step is split into `N` equal
//!   sub-steps. Each sub-step runs a small number of velocity
//!   iterations followed by a positional relaxation pass. Splitting
//!   the frame keeps high-mass-ratio stacks stable without paying for
//!   `N × velocity_iters` global iterations.
//! * **Connected-component islands** — bodies coupled through
//!   contacts or joints are grouped into disjoint islands with a
//!   union-find pass. Independent islands can be solved without any
//!   cross-communication, which unlocks deterministic per-island
//!   parallelism (dispatched by consumers of this module, e.g. via
//!   `rayon`).
//!
//! The trait shape (`BodyLike`, `ContactLike`, `JointLike`) is
//! deliberately narrow so that adapters can be written for the crate's
//! existing [`RigidBody`] / [`ContactConstraint`] types
//! without leaking their internals into this module.
//!
//! [`RigidBody`]: crate::solver::RigidBody
//! [`ContactConstraint`]: crate::solver::ContactConstraint
//!
//! # Visibility (v0.14.0-preview.8+)
//!
//! The whole `solver_tgs` family is `pub(crate)`. It exists as a parallel
//! generic TGS integrator with hooks-based extension points that historically
//! were public. As no downstream ever adopted the extension mechanism
//! (survey: 0 refs across `ALICE-Bamboo`, `ALICE-LOL`,
//! `ALICE-Kinematics`, `text-to-print-ios`), v1.0 API surface freeze
//! (Item B) chose Option C: pub(crate) the entire subsystem. It can be
//! re-exposed via a semver-minor bump if concrete demand emerges.

// Reserved TGS extension mechanism — pub(crate) since v0.14.0-preview.8.
// Broken intra-doc links to now-private items are expected until callers
// rewrite the references; suppression is scoped to this module.
#![allow(dead_code)]
#![allow(rustdoc::broken_intra_doc_links)]

use crate::error::PhysicsError;
use crate::math::Fix128;
use std::collections::HashMap;

// ---------------------------------------------------------------------------
// Trait shape
// ---------------------------------------------------------------------------

/// Minimum body surface the sub-stepping solver needs. Adapters over
/// concrete rigid-body types implement this on the outside.
pub(crate) trait BodyLike {
    /// Stable identity used for warm-start indexing and Kalman-style
    /// caches. Must be stable across a full physics tick.
    fn stable_id(&self) -> u64;
    /// `true` for bodies that participate in velocity/position updates.
    /// Static or kinematic bodies return `false` and are skipped by the
    /// island builder as sinks (they connect neighbours but do not
    /// spread).
    fn is_dynamic(&self) -> bool;
}

/// A pairwise contact between two bodies.
pub(crate) trait ContactLike {
    /// Index into the body slice for one side of the contact.
    fn body_a(&self) -> usize;
    /// Index into the body slice for the other side.
    fn body_b(&self) -> usize;
    /// Stable identity of the contact point. Used to look up the
    /// previously applied impulse for warm-starting; must survive
    /// re-detection across frames when the same feature pair is in
    /// contact (features can be encoded via hashing of feature IDs).
    fn stable_id(&self) -> u64;
}

/// A bilateral joint (distance, revolute, prismatic …) between two
/// bodies. Only the coupling for island detection is required here.
pub(crate) trait JointLike {
    /// Index of the first body attached to this joint.
    fn body_a(&self) -> usize;
    /// Index of the second body attached to this joint.
    fn body_b(&self) -> usize;
}

// ---------------------------------------------------------------------------
// Impulse cache (warm-start memory)
// ---------------------------------------------------------------------------

/// Impulse applied on a contact during a frame, broken down into the
/// contact-normal component and the two tangential (friction)
/// components. All values are in the contact frame.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub(crate) struct CachedImpulse {
    /// Normal impulse magnitude (non-negative in a healthy solve).
    pub(crate) normal: Fix128,
    /// First tangential impulse magnitude.
    pub(crate) tangent1: Fix128,
    /// Second tangential impulse magnitude.
    pub(crate) tangent2: Fix128,
}

/// Per-contact impulse memory carried across frames.
///
/// The cache is keyed by [`ContactLike::stable_id`]. Entries that are
/// not touched during a physics step are dropped by [`Self::sweep`] so
/// that stale IDs do not grow the map indefinitely.
#[derive(Debug, Default, Clone)]
pub(crate) struct ImpulseCache {
    pub(crate) entries: HashMap<u64, CachedImpulse>,
    // Bit set of IDs touched during the current tick.
    pub(crate) live: HashMap<u64, ()>,
    // Warm-starting hit/miss counters (Turn D: observability).
    pub(crate) hits: u64,
    pub(crate) misses: u64,
}

/// Snapshot of [`ImpulseCache`] warm-starting effectiveness. Callers
/// should read this at the end of each frame to gauge whether
/// contact IDs are stable across frames (high hit rate) or if the
/// scene is churning (low hit rate).
#[derive(Debug, Clone, Copy, Default)]
pub(crate) struct ImpulseCacheStats {
    /// Number of successful warm-start lookups (contact ID was cached).
    pub(crate) hits: u64,
    /// Number of cache misses (new or evicted contact ID).
    pub(crate) misses: u64,
}

impl ImpulseCacheStats {
    /// Ratio in `[0.0, 1.0]`. Returns `0.0` when no lookups have been
    /// performed yet.
    #[must_use]
    pub(crate) fn hit_rate(&self) -> f64 {
        let total = self.hits + self.misses;
        if total == 0 {
            0.0
        } else {
            self.hits as f64 / total as f64
        }
    }
}

impl ImpulseCache {
    /// Creates an empty cache.
    #[must_use]
    pub(crate) fn new() -> Self {
        Self::default()
    }

    /// Retrieves the previously applied impulse for `contact_id`, or
    /// zero if the contact is new. The read also marks the entry as
    /// alive for the current tick so that [`Self::sweep`] preserves it.
    #[must_use]
    pub(crate) fn take(&mut self, contact_id: u64) -> CachedImpulse {
        self.live.insert(contact_id, ());
        match self.entries.get(&contact_id).copied() {
            Some(v) => {
                self.hits += 1;
                v
            }
            None => {
                self.misses += 1;
                CachedImpulse::default()
            }
        }
    }

    /// Snapshot of warm-starting effectiveness. Combine with
    /// [`Self::reset_stats`] between measurement windows.
    #[must_use]
    pub(crate) fn stats(&self) -> ImpulseCacheStats {
        ImpulseCacheStats {
            hits: self.hits,
            misses: self.misses,
        }
    }

    /// Clears the hit / miss counters without touching the cached
    /// impulses. Useful when the caller wants to isolate the hit rate
    /// over a specific window (e.g. after warm-up frames).
    pub(crate) fn reset_stats(&mut self) {
        self.hits = 0;
        self.misses = 0;
    }

    /// Stores the applied impulse for `contact_id`. The entry is kept
    /// only as long as some subsequent [`Self::take`] call touches it
    /// before the next [`Self::sweep`]; setter calls do not implicitly
    /// mark the entry as alive so that stale contacts are pruned by
    /// the very next tick's sweep.
    pub(crate) fn set(&mut self, contact_id: u64, imp: CachedImpulse) {
        self.entries.insert(contact_id, imp);
    }

    /// Removes any entries that were not touched via [`Self::take`] or
    /// [`Self::set`] during the current tick. Call this once per
    /// physics step after all contacts have been visited.
    pub(crate) fn sweep(&mut self) {
        self.entries.retain(|k, _| self.live.contains_key(k));
        self.live.clear();
    }
}

// ---------------------------------------------------------------------------
// Union-find (path compression + rank union)
// ---------------------------------------------------------------------------

/// Disjoint-set data structure with path compression and union by
/// rank. Used to group bodies into islands.
#[derive(Debug, Clone)]
pub(crate) struct UnionFind {
    parent: Vec<usize>,
    rank: Vec<u8>,
}

impl UnionFind {
    /// Creates a forest of `n` singleton sets.
    #[must_use]
    pub(crate) fn new(n: usize) -> Self {
        Self {
            parent: (0..n).collect(),
            rank: vec![0; n],
        }
    }

    /// Returns the representative of the set containing `i`. Amortised
    /// α(n) via path compression.
    pub(crate) fn find(&mut self, i: usize) -> usize {
        let mut root = i;
        while self.parent[root] != root {
            root = self.parent[root];
        }
        // Path compression: point every visited node directly at root.
        let mut cur = i;
        while self.parent[cur] != root {
            let next = self.parent[cur];
            self.parent[cur] = root;
            cur = next;
        }
        root
    }

    /// Unions the sets containing `i` and `j`. Returns `true` when a
    /// merge actually happened.
    pub(crate) fn union(&mut self, i: usize, j: usize) -> bool {
        let ri = self.find(i);
        let rj = self.find(j);
        if ri == rj {
            return false;
        }
        // Union by rank: hang the shorter tree under the taller one.
        if self.rank[ri] < self.rank[rj] {
            self.parent[ri] = rj;
        } else if self.rank[ri] > self.rank[rj] {
            self.parent[rj] = ri;
        } else {
            self.parent[rj] = ri;
            self.rank[ri] += 1;
        }
        true
    }
}

// ---------------------------------------------------------------------------
// Island detection
// ---------------------------------------------------------------------------

/// A connected component of bodies plus the contacts and joints that
/// bind them. The three vectors are sorted ascending by index so that
/// downstream dispatch (e.g. `rayon::par_iter`) walks the same order
/// on every run, preserving determinism.
#[derive(Debug, Clone, Default, PartialEq, Eq)]
pub(crate) struct Island {
    /// World-index list of bodies belonging to this island, sorted ascending.
    pub(crate) bodies: Vec<usize>,
    /// World-index list of contacts inside this island, sorted ascending.
    pub(crate) contacts: Vec<usize>,
    /// World-index list of joints inside this island, sorted ascending.
    pub(crate) joints: Vec<usize>,
}

/// Groups bodies into islands using contacts and joints as edges.
/// Static bodies (`is_dynamic == false`) act as separators: they are
/// listed in every island that touches them but do not fuse islands
/// together.
///
/// # Errors
/// Returns [`PhysicsError::InvalidConstraint`] when any contact or joint
/// references a body index outside of `bodies`. No panic: an FFI host
/// built with `panic = "abort"` must never be taken down by a malformed
/// constraint list.
pub(crate) fn build_islands<B: BodyLike, C: ContactLike, J: JointLike>(
    bodies: &[B],
    contacts: &[C],
    joints: &[J],
) -> Result<Vec<Island>, PhysicsError> {
    let n = bodies.len();
    let mut uf = UnionFind::new(n);

    // Union dynamic pairs. Static bodies stay in their own set so they
    // do not glue moving stacks together across the world.
    for c in contacts {
        let (a, b) = (c.body_a(), c.body_b());
        if a >= n || b >= n {
            return Err(PhysicsError::InvalidConstraint {
                reason: "contact references body out of bounds",
            });
        }
        if bodies[a].is_dynamic() && bodies[b].is_dynamic() {
            uf.union(a, b);
        }
    }
    for j in joints {
        let (a, b) = (j.body_a(), j.body_b());
        if a >= n || b >= n {
            return Err(PhysicsError::InvalidConstraint {
                reason: "joint references body out of bounds",
            });
        }
        if bodies[a].is_dynamic() && bodies[b].is_dynamic() {
            uf.union(a, b);
        }
    }

    // Bucket bodies by representative. Static bodies are added to
    // every island that references them below.
    let mut buckets: HashMap<usize, Island> = HashMap::new();
    for (i, body) in bodies.iter().enumerate() {
        if body.is_dynamic() {
            let root = uf.find(i);
            buckets.entry(root).or_default().bodies.push(i);
        }
    }

    // Attach contacts and joints, and copy static neighbours into
    // whichever island touches them so that the constraint has both
    // sides available during solve.
    for (idx, c) in contacts.iter().enumerate() {
        let (a, b) = (c.body_a(), c.body_b());
        let root = if bodies[a].is_dynamic() {
            uf.find(a)
        } else if bodies[b].is_dynamic() {
            uf.find(b)
        } else {
            continue; // static-static contact: nothing to solve.
        };
        let island = buckets.entry(root).or_default();
        island.contacts.push(idx);
        if !bodies[a].is_dynamic() && !island.bodies.contains(&a) {
            island.bodies.push(a);
        }
        if !bodies[b].is_dynamic() && !island.bodies.contains(&b) {
            island.bodies.push(b);
        }
    }
    for (idx, j) in joints.iter().enumerate() {
        let (a, b) = (j.body_a(), j.body_b());
        let root = if bodies[a].is_dynamic() {
            uf.find(a)
        } else if bodies[b].is_dynamic() {
            uf.find(b)
        } else {
            continue;
        };
        let island = buckets.entry(root).or_default();
        island.joints.push(idx);
        if !bodies[a].is_dynamic() && !island.bodies.contains(&a) {
            island.bodies.push(a);
        }
        if !bodies[b].is_dynamic() && !island.bodies.contains(&b) {
            island.bodies.push(b);
        }
    }

    // Sort each vector so consumers walk in the same order on every
    // machine (determinism), then collect islands in a canonical order.
    let mut islands: Vec<Island> = buckets.into_values().collect();
    for island in &mut islands {
        island.bodies.sort_unstable();
        island.contacts.sort_unstable();
        island.joints.sort_unstable();
    }
    // Full lexicographic key: two islands can share their smallest body
    // index (a static body touching two otherwise separate islands), and
    // `buckets` is a HashMap, so a first-body-only key would leave the tie
    // order to the hasher.
    islands.sort_by(|x, y| {
        x.bodies
            .cmp(&y.bodies)
            .then_with(|| x.contacts.cmp(&y.contacts))
            .then_with(|| x.joints.cmp(&y.joints))
    });
    Ok(islands)
}

// ---------------------------------------------------------------------------
// Sub-stepping TGS configuration + driver
// ---------------------------------------------------------------------------

/// Sub-stepping TGS parameters. The defaults match the "small
/// substeps, few iterations" recipe recommended by recent rigid-body
/// literature.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(crate) struct TgsConfig {
    /// Number of equal-sized sub-steps per physics tick.
    pub(crate) substeps: u32,
    /// Velocity iterations per sub-step.
    pub(crate) velocity_iters: u32,
    /// Positional relaxation iterations per sub-step.
    pub(crate) position_iters: u32,
    /// When `true`, the impulse cache is consulted at the start of
    /// each velocity phase to seed the constraint impulses.
    pub(crate) warmstart: bool,
}

impl Default for TgsConfig {
    fn default() -> Self {
        Self {
            substeps: 4,
            velocity_iters: 4,
            position_iters: 2,
            warmstart: true,
        }
    }
}

// ---------------------------------------------------------------------------
// Adaptive sub-stepping (Turn D)
// ---------------------------------------------------------------------------

/// Configuration for closed-form adaptive sub-stepping.
///
/// The sub-step count for a physics tick is decided from the fastest
/// body: `substeps = smallest n such that n * max_translation_per_step
/// >= v_max * dt`, clamped into `[min_substeps, max_substeps]`.
/// Callers should assign the return value of
/// [`adaptive_substeps_for`] to [`TgsConfig::substeps`] before
/// dispatching `tgs_step`.
///
/// Determinism: the computation is a pure function of `bodies + dt +
/// cfg`, uses only Fix128 arithmetic (no floating-point comparisons),
/// and iterates in canonical index order — safe under lockstep /
/// rollback netcode.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(crate) struct AdaptiveSubStepConfig {
    /// Upper bound on translation per sub-step (world units per
    /// sub-step). When `v_max * dt <= max_translation_per_step`,
    /// `substeps = min_substeps`.
    pub(crate) max_translation_per_step: Fix128,
    /// Hard cap on sub-step count for the current tick.
    pub(crate) max_substeps: u32,
    /// Minimum sub-step count (typically `1`).
    pub(crate) min_substeps: u32,
}

impl Default for AdaptiveSubStepConfig {
    fn default() -> Self {
        Self {
            max_translation_per_step: Fix128::from_ratio(1, 10), // 0.1 world units
            max_substeps: 16,
            min_substeps: 1,
        }
    }
}

/// Body trait providing the velocity metric required by
/// [`adaptive_substeps_for`]. The L∞ (max absolute per-axis
/// component) norm is used instead of L2 to avoid Fix128 square roots
/// and keep the closed-form deterministic across CORDIC LUT
/// tolerances. Return `Fix128::ZERO` for
/// static / sleeping bodies so they do not contribute to `v_max`.
pub(crate) trait HasVelocity {
    /// L∞ velocity in world units per second.
    fn velocity_l_inf(&self) -> Fix128;
}

/// Returns the number of sub-steps required so that the fastest body
/// travels at most `cfg.max_translation_per_step` per sub-step during
/// `dt`.
///
/// This is a pure function of `bodies + dt + cfg` (no hidden global
/// state, no floating-point comparisons). Iteration is index-ordered
/// so that different scheduling policies never observe a different
/// `v_max` when the same bodies are present — safe under rayon
/// dispatch.
#[must_use]
pub(crate) fn adaptive_substeps_for<B: HasVelocity>(
    bodies: &[B],
    dt: Fix128,
    cfg: &AdaptiveSubStepConfig,
) -> u32 {
    // 1. v_max = max L∞ velocity across bodies.
    let mut v_max = Fix128::ZERO;
    for b in bodies {
        let v = b.velocity_l_inf();
        if v > v_max {
            v_max = v;
        }
    }
    // 2. Translation per full tick = v_max * dt.
    let travel = v_max * dt;
    // 3. substeps = smallest n such that n * max_translation_per_step
    //    >= travel, clamped into [min_substeps, max_substeps]. We do
    //    the search by repeated addition rather than division so the
    //    result is bit-identical regardless of Fix128 division rounding.
    let min = cfg.min_substeps.max(1);
    let max = cfg.max_substeps.max(min);
    if travel <= cfg.max_translation_per_step {
        return min;
    }
    let mut n: u32 = min;
    while n < max && Fix128::from_int(i64::from(n)) * cfg.max_translation_per_step < travel {
        n += 1;
    }
    n
}

/// CCD-optimised adaptive sub-stepping: shrinks
/// `max_translation_per_step` to a fraction of the smallest collider
/// radius so that fast bodies never advance more than
/// `radius * safety_factor` per sub-step. Combined with speculative
/// contacts + TOI, this prevents tunneling through thin walls.
///
/// The effective per-step cap is the minimum of
/// `cfg.max_translation_per_step` and
/// `smallest_collider_radius * ccd_safety_factor`, ensuring the CCD
/// constraint always dominates the vanilla adaptive metric.
///
/// Determinism: identical guarantees to [`adaptive_substeps_for`]
/// (pure function of inputs, index-ordered iteration, no Fix128
/// division, no floating-point comparison).
#[must_use]
pub(crate) fn adaptive_substeps_for_ccd<B: HasVelocity>(
    bodies: &[B],
    dt: Fix128,
    smallest_collider_radius: Fix128,
    ccd_safety_factor: Fix128,
    cfg: &AdaptiveSubStepConfig,
) -> u32 {
    let ccd_step = smallest_collider_radius * ccd_safety_factor;
    let effective_step = if ccd_step < cfg.max_translation_per_step {
        ccd_step
    } else {
        cfg.max_translation_per_step
    };
    let effective_cfg = AdaptiveSubStepConfig {
        max_translation_per_step: effective_step,
        max_substeps: cfg.max_substeps,
        min_substeps: cfg.min_substeps,
    };
    adaptive_substeps_for(bodies, dt, &effective_cfg)
}

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

    struct DummyBody(Fix128);
    impl HasVelocity for DummyBody {
        fn velocity_l_inf(&self) -> Fix128 {
            self.0
        }
    }

    fn cfg() -> AdaptiveSubStepConfig {
        AdaptiveSubStepConfig {
            max_translation_per_step: Fix128::from_ratio(1, 10),
            max_substeps: 8,
            min_substeps: 1,
        }
    }

    #[test]
    fn zero_velocity_returns_min_substeps() {
        let bodies = [DummyBody(Fix128::ZERO), DummyBody(Fix128::ZERO)];
        let dt = Fix128::from_ratio(1, 60);
        assert_eq!(adaptive_substeps_for(&bodies, dt, &cfg()), 1);
    }

    #[test]
    fn empty_body_slice_returns_min_substeps() {
        let bodies: [DummyBody; 0] = [];
        let dt = Fix128::from_ratio(1, 60);
        assert_eq!(adaptive_substeps_for(&bodies, dt, &cfg()), 1);
    }

    #[test]
    fn slow_body_below_threshold_returns_min_substeps() {
        // v = 1 unit/s, dt = 1/60 s → travel = 1/60 ≈ 0.0167 < 0.1
        let bodies = [DummyBody(Fix128::from_int(1))];
        let dt = Fix128::from_ratio(1, 60);
        assert_eq!(adaptive_substeps_for(&bodies, dt, &cfg()), 1);
    }

    #[test]
    fn fast_body_scales_substeps_up() {
        // v = 60 unit/s, dt = 1/60 s → travel = 1.0
        //   step size = 0.1 → need 10 substeps, capped at 8.
        let bodies = [DummyBody(Fix128::from_int(60))];
        let dt = Fix128::from_ratio(1, 60);
        let cfg = cfg();
        let n = adaptive_substeps_for(&bodies, dt, &cfg);
        assert!(
            n >= 2 && n <= cfg.max_substeps,
            "expected non-trivial substep count within cap, got {n}"
        );
    }

    #[test]
    fn monotonic_in_max_velocity() {
        let slow = [DummyBody(Fix128::from_int(6))];
        let fast = [DummyBody(Fix128::from_int(60))];
        let dt = Fix128::from_ratio(1, 60);
        let c = cfg();
        let a = adaptive_substeps_for(&slow, dt, &c);
        let b = adaptive_substeps_for(&fast, dt, &c);
        assert!(
            b >= a,
            "faster body must not require fewer substeps (a={a}, b={b})"
        );
    }

    /// CCD variant: when the smallest collider radius forces a tighter
    /// per-step cap than the base config, the CCD path returns at
    /// least as many substeps as the vanilla path.
    #[test]
    fn ccd_variant_dominates_vanilla_for_small_radius() {
        let bodies = [DummyBody(Fix128::from_int(60))];
        let dt = Fix128::from_ratio(1, 60);
        let base = cfg();
        let vanilla = adaptive_substeps_for(&bodies, dt, &base);
        let ccd = adaptive_substeps_for_ccd(
            &bodies,
            dt,
            Fix128::from_ratio(1, 100), // 0.01 world unit radius
            Fix128::from_ratio(1, 2),   // safety factor 0.5
            &base,
        );
        assert!(
            ccd >= vanilla,
            "CCD variant with tighter cap must not return fewer substeps (vanilla={vanilla}, ccd={ccd})"
        );
    }

    /// CCD variant: when the collider is large, the CCD cap does not
    /// tighten the vanilla path and both return the same substep count.
    #[test]
    fn ccd_variant_matches_vanilla_for_large_radius() {
        let bodies = [DummyBody(Fix128::from_int(60))];
        let dt = Fix128::from_ratio(1, 60);
        let base = cfg();
        let vanilla = adaptive_substeps_for(&bodies, dt, &base);
        let ccd = adaptive_substeps_for_ccd(
            &bodies,
            dt,
            Fix128::from_int(100),    // 100 world unit radius (huge)
            Fix128::from_ratio(1, 2), // safety factor 0.5
            &base,
        );
        assert_eq!(
            vanilla, ccd,
            "large collider must not tighten the vanilla adaptive cap"
        );
    }

    /// CCD variant: zero velocity always returns `min_substeps`,
    /// regardless of collider radius.
    #[test]
    fn ccd_variant_zero_velocity_returns_min() {
        let bodies = [DummyBody(Fix128::ZERO), DummyBody(Fix128::ZERO)];
        let dt = Fix128::from_ratio(1, 60);
        let base = cfg();
        let n = adaptive_substeps_for_ccd(
            &bodies,
            dt,
            Fix128::from_ratio(1, 1000),
            Fix128::from_ratio(1, 2),
            &base,
        );
        assert_eq!(n, base.min_substeps.max(1));
    }
}

/// Callbacks that drive one velocity or position iteration inside the
/// sub-stepping loop. Consumers wire these up to their concrete solver
/// so that the traversal (substeps, warm-start, sweep) stays here
/// while the per-constraint math stays outside.
pub(crate) trait TgsHooks {
    /// Called once at the beginning of every sub-step. Typical
    /// implementations apply gravity and integrate velocities forward
    /// by `sub_dt`.
    fn begin_substep(&mut self, sub_dt: Fix128);

    /// Called once per velocity iteration inside a sub-step. The
    /// implementation walks the island's contacts and joints and
    /// applies impulses.
    fn velocity_iteration(&mut self, sub_dt: Fix128);

    /// Called once per position iteration inside a sub-step. The
    /// implementation walks the island's contacts and joints and
    /// applies positional correction.
    fn position_iteration(&mut self, sub_dt: Fix128);

    /// Called once at the end of every sub-step. Typical
    /// implementations integrate positions using the just-solved
    /// velocities.
    fn end_substep(&mut self, sub_dt: Fix128);
}

/// Runs the sub-stepping loop for a single island. Splits the frame
/// `dt` into `cfg.substeps` equal sub-steps and, for each sub-step,
/// runs `velocity_iters` velocity iterations followed by
/// `position_iters` positional iterations. Warm-starting decisions are
/// left to the hook implementation (the module publishes the
/// [`ImpulseCache`] type that hooks are expected to consult when
/// `cfg.warmstart` is `true`).
///
/// `cfg.substeps == 0` is a no-op (nothing to integrate); before 1.2.0 it
/// panicked, which is an input-driven panic in a lockstep solver.
pub(crate) fn tgs_step<H: TgsHooks>(hooks: &mut H, cfg: &TgsConfig, dt: Fix128) {
    if cfg.substeps == 0 {
        return;
    }
    let inv = Fix128::from_f32(1.0 / cfg.substeps as f32);
    let sub_dt = dt * inv;
    for _ in 0..cfg.substeps {
        hooks.begin_substep(sub_dt);
        for _ in 0..cfg.velocity_iters {
            hooks.velocity_iteration(sub_dt);
        }
        for _ in 0..cfg.position_iters {
            hooks.position_iteration(sub_dt);
        }
        hooks.end_substep(sub_dt);
    }
}

// ---------------------------------------------------------------------------
// Adapter to the existing solver types
// ---------------------------------------------------------------------------
//
// The wrapper below lets this module operate on the crate's concrete
// [`DistanceConstraint`] without a hard coupling to its internal layout.
// The world adapts bodies and contacts with its own `body_to_tgs` /
// `contact_to_tgs` wrappers in `solver.rs`.

use crate::solver::DistanceConstraint;

/// Borrowed view of a [`DistanceConstraint`] as a bilateral joint.
pub(crate) struct DistanceRef<'a> {
    /// Reference to the underlying distance constraint state.
    pub(crate) joint: &'a DistanceConstraint,
}

impl JointLike for DistanceRef<'_> {
    fn body_a(&self) -> usize {
        self.joint.body_a
    }
    fn body_b(&self) -> usize {
        self.joint.body_b
    }
}

// ---------------------------------------------------------------------------
// Per-island dispatch (rayon, feature-gated)
// ---------------------------------------------------------------------------

/// Serially dispatches `f` over each island in canonical order.
///
/// Available even without the `parallel` feature so that consumers can
/// share a single call-site regardless of build configuration.
pub(crate) fn dispatch_islands<F>(islands: &[Island], mut f: F)
where
    F: FnMut(&Island),
{
    for island in islands {
        f(island);
    }
}

/// Dispatches `f` over each island in parallel via `rayon`, pairing each
/// island with its own slot of caller-provided per-island state `aux`
/// (e.g. a warm-start [`ImpulseCache`]), and collects one `R` per island
/// **in canonical island order**.
///
/// Islands are independent by construction (see [`build_islands`]), so
/// no inter-island synchronisation is required beyond each island owning
/// its own `aux` slot. Determinism of the aggregate `Vec<R>` is preserved
/// because `rayon`'s `par_iter().zip().map().collect()` over indexed
/// (slice-backed) iterators always reassembles results in source order,
/// independent of which thread executed which island.
///
/// # Panics
/// Panics when `aux.len() != islands.len()`.
///
/// Available only with the `parallel` feature.
#[cfg(feature = "parallel")]
pub(crate) fn par_dispatch_islands<T, F, R>(islands: &[Island], aux: &mut [T], f: F) -> Vec<R>
where
    F: Fn(&Island, &mut T) -> R + Send + Sync,
    T: Send,
    R: Send,
{
    assert_eq!(
        islands.len(),
        aux.len(),
        "one aux slot per island is required"
    );
    use rayon::prelude::*;
    islands
        .par_iter()
        .zip(aux.par_iter_mut())
        .map(|(isl, a)| f(isl, a))
        .collect()
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

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

    // --- Mock BodyLike / ContactLike / JointLike --------------------------

    struct MockBody {
        id: u64,
        dynamic: bool,
    }
    impl BodyLike for MockBody {
        fn stable_id(&self) -> u64 {
            self.id
        }
        fn is_dynamic(&self) -> bool {
            self.dynamic
        }
    }

    struct MockContact {
        id: u64,
        a: usize,
        b: usize,
    }
    impl ContactLike for MockContact {
        fn body_a(&self) -> usize {
            self.a
        }
        fn body_b(&self) -> usize {
            self.b
        }
        fn stable_id(&self) -> u64 {
            self.id
        }
    }

    struct MockJoint {
        a: usize,
        b: usize,
    }
    impl JointLike for MockJoint {
        fn body_a(&self) -> usize {
            self.a
        }
        fn body_b(&self) -> usize {
            self.b
        }
    }

    // --- ImpulseCache -----------------------------------------------------

    #[test]
    fn impulse_cache_returns_zero_for_new_contact() {
        let mut cache = ImpulseCache::new();
        let got = cache.take(42);
        assert_eq!(got, CachedImpulse::default());
    }

    /// Degenerate `sweep` inputs: an empty cache stays empty and does not
    /// panic, and a cache where every entry was touched this tick (nothing
    /// stale) is unchanged by `sweep`.
    #[test]
    fn impulse_cache_sweep_degenerate_empty_and_nothing_stale() {
        let mut empty = ImpulseCache::new();
        empty.sweep();
        assert!(
            empty.entries.is_empty(),
            "sweeping an empty cache stays empty"
        );

        let mut all_live = ImpulseCache::new();
        let imp = CachedImpulse {
            normal: Fix128::from_int(2),
            tangent1: Fix128::ZERO,
            tangent2: Fix128::ZERO,
        };
        all_live.set(1, imp);
        all_live.set(2, imp);
        let _ = all_live.take(1);
        let _ = all_live.take(2);
        all_live.sweep();
        assert_eq!(
            all_live.entries.len(),
            2,
            "nothing stale: both entries survive"
        );
        assert_eq!(all_live.stored(1), imp);
        assert_eq!(all_live.stored(2), imp);
    }

    #[test]
    fn impulse_cache_round_trips_stored_impulse() {
        let mut cache = ImpulseCache::new();
        let imp = CachedImpulse {
            normal: Fix128::from_f32(1.5),
            tangent1: Fix128::from_f32(-0.25),
            tangent2: Fix128::from_f32(0.0),
        };
        cache.set(7, imp);
        assert_eq!(cache.take(7), imp);
        assert_eq!(cache.entries.len(), 1);
    }

    #[test]
    fn impulse_cache_sweep_drops_untouched_entries() {
        let mut cache = ImpulseCache::new();
        cache.set(1, CachedImpulse::default());
        cache.set(2, CachedImpulse::default());
        // Touch only #1 this tick.
        let _ = cache.take(1);
        cache.sweep();
        assert_eq!(cache.entries.len(), 1);
        assert!(cache.entries_contains(1));
        assert!(!cache.entries_contains(2));
    }

    // Small test-only helper so we do not expose the internal map.
    impl ImpulseCache {
        fn entries_contains(&self, id: u64) -> bool {
            self.entries.contains_key(&id)
        }

        /// Stored impulse without marking it alive (default when absent).
        fn stored(&self, id: u64) -> CachedImpulse {
            self.entries.get(&id).copied().unwrap_or_default()
        }
    }

    // --- UnionFind --------------------------------------------------------

    #[test]
    fn union_find_singletons_are_distinct() {
        let mut uf = UnionFind::new(5);
        for i in 0..5 {
            assert_eq!(uf.find(i), i);
        }
    }

    #[test]
    fn union_find_merges_and_deduplicates() {
        let mut uf = UnionFind::new(4);
        assert!(uf.union(0, 1));
        assert!(uf.union(1, 2));
        assert!(!uf.union(0, 2), "already in same set");
        assert_eq!(uf.find(0), uf.find(2));
        assert_ne!(uf.find(0), uf.find(3));
    }

    // --- Island detection -------------------------------------------------

    #[test]
    fn islands_empty_world_returns_no_islands() {
        let bodies: [MockBody; 0] = [];
        let contacts: [MockContact; 0] = [];
        let joints: [MockJoint; 0] = [];
        assert!(build_islands(&bodies, &contacts, &joints)
            .expect("valid island inputs")
            .is_empty());
    }

    #[test]
    fn islands_out_of_bounds_contact_is_an_error_not_a_panic() {
        let bodies = [MockBody {
            id: 0,
            dynamic: true,
        }];
        let contacts = [MockContact { id: 7, a: 0, b: 1 }];
        let joints: [MockJoint; 0] = [];
        let err = build_islands(&bodies, &contacts, &joints).unwrap_err();
        assert_eq!(
            err,
            PhysicsError::InvalidConstraint {
                reason: "contact references body out of bounds"
            }
        );
    }

    #[test]
    fn islands_out_of_bounds_joint_is_an_error_not_a_panic() {
        let bodies = [MockBody {
            id: 0,
            dynamic: true,
        }];
        let contacts: [MockContact; 0] = [];
        let joints = [MockJoint { a: 5, b: 0 }];
        let err = build_islands(&bodies, &contacts, &joints).unwrap_err();
        assert_eq!(
            err,
            PhysicsError::InvalidConstraint {
                reason: "joint references body out of bounds"
            }
        );
    }

    #[test]
    fn islands_two_disjoint_pairs_form_two_groups() {
        let bodies = [
            MockBody {
                id: 0,
                dynamic: true,
            },
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: true,
            },
            MockBody {
                id: 3,
                dynamic: true,
            },
        ];
        let contacts = [
            MockContact {
                id: 100,
                a: 0,
                b: 1,
            },
            MockContact {
                id: 101,
                a: 2,
                b: 3,
            },
        ];
        let islands =
            build_islands(&bodies, &contacts, &[] as &[MockJoint]).expect("valid island inputs");
        assert_eq!(islands.len(), 2);
        assert_eq!(islands[0].bodies, vec![0, 1]);
        assert_eq!(islands[1].bodies, vec![2, 3]);
    }

    #[test]
    fn islands_joint_bridges_two_pairs() {
        let bodies = [
            MockBody {
                id: 0,
                dynamic: true,
            },
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: true,
            },
        ];
        let contacts = [MockContact {
            id: 100,
            a: 0,
            b: 1,
        }];
        let joints = [MockJoint { a: 1, b: 2 }];
        let islands = build_islands(&bodies, &contacts, &joints).expect("valid island inputs");
        assert_eq!(islands.len(), 1);
        assert_eq!(islands[0].bodies, vec![0, 1, 2]);
        assert_eq!(islands[0].contacts, vec![0]);
        assert_eq!(islands[0].joints, vec![0]);
    }

    #[test]
    fn islands_static_body_does_not_fuse_two_stacks() {
        // ground (static) touches two independent dynamic stacks
        let bodies = [
            MockBody {
                id: 0,
                dynamic: false,
            }, // static ground
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: true,
            },
            MockBody {
                id: 3,
                dynamic: true,
            },
            MockBody {
                id: 4,
                dynamic: true,
            },
        ];
        let contacts = [
            MockContact {
                id: 100,
                a: 0,
                b: 1,
            }, // stack A resting on ground
            MockContact {
                id: 101,
                a: 1,
                b: 2,
            }, // stack A internal
            MockContact {
                id: 200,
                a: 0,
                b: 3,
            }, // stack B resting on ground
            MockContact {
                id: 201,
                a: 3,
                b: 4,
            }, // stack B internal
        ];
        let islands =
            build_islands(&bodies, &contacts, &[] as &[MockJoint]).expect("valid island inputs");
        assert_eq!(islands.len(), 2, "static ground must not fuse islands");
        // Each island should contain the static ground as a member (so
        // that the solver has access to both sides of the contact).
        assert!(islands[0].bodies.contains(&0));
        assert!(islands[1].bodies.contains(&0));
    }

    #[test]
    fn islands_are_deterministic_across_runs() {
        let bodies = [
            MockBody {
                id: 0,
                dynamic: true,
            },
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: true,
            },
        ];
        let contacts = [
            MockContact {
                id: 100,
                a: 2,
                b: 0,
            },
            MockContact {
                id: 101,
                a: 1,
                b: 2,
            },
        ];
        let a =
            build_islands(&bodies, &contacts, &[] as &[MockJoint]).expect("valid island inputs");
        let b =
            build_islands(&bodies, &contacts, &[] as &[MockJoint]).expect("valid island inputs");
        assert_eq!(a, b);
    }

    // --- TgsConfig / driver ----------------------------------------------

    #[test]
    fn tgs_config_defaults_are_reasonable() {
        let cfg = TgsConfig::default();
        assert!(cfg.substeps >= 1);
        assert!(cfg.velocity_iters >= 1);
        assert!(cfg.position_iters >= 1);
        assert!(cfg.warmstart);
    }

    struct HookCounter {
        begin: u32,
        vel: u32,
        pos: u32,
        end: u32,
    }
    impl TgsHooks for HookCounter {
        fn begin_substep(&mut self, _sub_dt: Fix128) {
            self.begin += 1;
        }
        fn velocity_iteration(&mut self, _sub_dt: Fix128) {
            self.vel += 1;
        }
        fn position_iteration(&mut self, _sub_dt: Fix128) {
            self.pos += 1;
        }
        fn end_substep(&mut self, _sub_dt: Fix128) {
            self.end += 1;
        }
    }

    // --- Adapter --------------------------------------------------------

    // --- Dispatch -------------------------------------------------------

    #[test]
    fn dispatch_islands_visits_every_island_once() {
        let islands = vec![
            Island {
                bodies: vec![0, 1],
                contacts: vec![0],
                joints: vec![],
            },
            Island {
                bodies: vec![2, 3],
                contacts: vec![1],
                joints: vec![],
            },
        ];
        let mut visited = Vec::new();
        dispatch_islands(&islands, |isl| {
            visited.push(isl.bodies[0]);
        });
        assert_eq!(visited, vec![0, 2]);
    }

    /// Degenerate inputs for `dispatch_islands`: an empty slice calls `f`
    /// zero times, and a single-island slice calls it exactly once.
    #[test]
    fn dispatch_islands_degenerate_empty_and_single() {
        let none: [Island; 0] = [];
        let mut calls = 0u32;
        dispatch_islands(&none, |_| calls += 1);
        assert_eq!(calls, 0, "empty island list dispatches zero times");

        let one = [Island {
            bodies: vec![5],
            contacts: vec![],
            joints: vec![],
        }];
        let mut seen = Vec::new();
        dispatch_islands(&one, |isl| seen.push(isl.bodies.clone()));
        assert_eq!(seen, vec![vec![5]], "single island dispatches exactly once");
    }

    #[cfg(feature = "parallel")]
    #[test]
    fn par_dispatch_islands_visits_every_island() {
        let islands: Vec<Island> = (0..16)
            .map(|i| Island {
                bodies: vec![i, i + 100],
                contacts: vec![],
                joints: vec![],
            })
            .collect();
        let mut aux = vec![0u32; islands.len()];
        // Map-reduce: `results[k]` is island `k`'s first body index, so a
        // result equal to `0..16` proves both full coverage (16 entries)
        // and canonical-order collection (no shuffling across threads).
        let results: Vec<usize> = par_dispatch_islands(&islands, &mut aux, |isl, slot| {
            *slot += 1;
            isl.bodies[0]
        });
        assert_eq!(results, (0..16).collect::<Vec<_>>());
        assert!(
            aux.iter().all(|&v| v == 1),
            "each aux slot touched exactly once"
        );
    }

    #[cfg(feature = "parallel")]
    #[test]
    #[should_panic(expected = "one aux slot per island is required")]
    fn par_dispatch_islands_panics_on_aux_length_mismatch() {
        let islands = vec![Island::default(), Island::default()];
        let mut aux = vec![0u32; 1];
        let _: Vec<usize> = par_dispatch_islands(&islands, &mut aux, |_, _| 0);
    }

    #[test]
    fn tgs_step_visits_hooks_the_expected_number_of_times() {
        let cfg = TgsConfig {
            substeps: 3,
            velocity_iters: 2,
            position_iters: 1,
            warmstart: false,
        };
        let mut h = HookCounter {
            begin: 0,
            vel: 0,
            pos: 0,
            end: 0,
        };
        tgs_step(&mut h, &cfg, Fix128::from_f32(1.0 / 60.0));
        assert_eq!(h.begin, 3);
        assert_eq!(h.vel, 6); // 3 * 2
        assert_eq!(h.pos, 3); // 3 * 1
        assert_eq!(h.end, 3);
    }

    // ---- mutation-score tests (2026-09-15) ----------------------------

    #[test]
    fn union_find_union_reports_merge_and_uses_rank() {
        let mut uf = UnionFind::new(6);
        assert!(uf.union(0, 1), "初回 merge は true");
        assert!(!uf.union(0, 1), "同一集合は false");
        assert!(!uf.union(1, 0));
        assert!(uf.union(2, 3));
        assert!(uf.union(0, 2), "集合同士の merge");
        assert_eq!(uf.find(3), uf.find(1));
        assert_ne!(uf.find(4), uf.find(0));
        // 4 と 5 は独立、5 を 4 に繋いでから全体に
        assert!(uf.union(4, 5));
        assert!(uf.union(5, 3));
        let root = uf.find(0);
        for i in 0..6 {
            assert_eq!(uf.find(i), root, "{i}");
        }
        // path compression: find 後は parent が root 直結
        assert_eq!(uf.parent[5], root);
        // rank: 深い木の下に浅い木がぶら下がる (rank 1 の木 {0,1} に単独 6 を merge → root は変わらない)
        let mut r = UnionFind::new(3);
        r.union(0, 1);
        let root01 = r.find(0);
        r.union(2, 0);
        assert_eq!(r.find(2), root01, "rank の高い側が root のまま");
        assert_eq!(r.rank[root01], 1);
        // 同 rank 同士の merge で rank が増える
        let mut q = UnionFind::new(4);
        q.union(0, 1);
        q.union(2, 3);
        q.union(0, 2);
        let qr = q.find(0);
        assert_eq!(q.rank[qr], 2);
    }

    #[test]
    fn impulse_cache_counts_hits_and_misses_and_reports_hit_rate() {
        let mut c = ImpulseCache::new();
        assert!(c.entries.is_empty());
        // 未登録 → miss、default
        assert_eq!(c.take(7), CachedImpulse::default());
        let s = c.stats();
        assert_eq!((s.hits, s.misses), (0, 1));
        assert!((s.hit_rate() - 0.0).abs() < 1e-12);
        let imp = CachedImpulse {
            normal: Fix128::from_int(3),
            tangent1: Fix128::from_int(-1),
            tangent2: Fix128::from_ratio(1, 2),
        };
        c.set(7, imp);
        assert_eq!(c.entries.len(), 1);
        assert_eq!(c.take(7), imp);
        assert_eq!(c.take(7), imp);
        assert_eq!(c.take(9), CachedImpulse::default());
        let s = c.stats();
        assert_eq!((s.hits, s.misses), (2, 2));
        assert!((s.hit_rate() - 0.5).abs() < 1e-12);
        c.reset_stats();
        let s = c.stats();
        assert_eq!((s.hits, s.misses), (0, 0));
        assert_eq!(s.hit_rate(), 0.0, "total 0 は 0.0");
        // hit_rate = hits / (hits + misses): 3 hit 1 miss → 0.75
        let st = ImpulseCacheStats { hits: 3, misses: 1 };
        assert!((st.hit_rate() - 0.75).abs() < 1e-12);
        // sweep: 今 tick に take していない entry は落ちる
        c.set(100, imp);
        c.set(200, imp);
        let _ = c.take(100);
        c.sweep();
        assert_eq!(c.stored(100), imp);
        assert_eq!(c.stored(200), CachedImpulse::default());
        assert_eq!(
            c.stored(7),
            imp,
            "7 is live in this tick (taken before sweep)"
        );
        // 次 tick: 何も take せず sweep → 全 entry が落ちる (live は sweep で clear 済)
        c.sweep();
        assert!(c.entries.is_empty());
    }

    #[test]
    fn islands_attach_static_neighbours_and_sort_canonically() {
        // bodies: 0 dyn, 1 dyn, 2 static, 3 dyn (孤立), 4 dyn
        let bodies = [
            MockBody {
                id: 10,
                dynamic: true,
            },
            MockBody {
                id: 11,
                dynamic: true,
            },
            MockBody {
                id: 12,
                dynamic: false,
            },
            MockBody {
                id: 13,
                dynamic: true,
            },
            MockBody {
                id: 14,
                dynamic: true,
            },
        ];
        // contact 0: 1-0 (dyn-dyn、逆順)、contact 1: 4-2 (dyn-static)、contact 2: 2-2 (static-static → 無視)
        let contacts = [
            MockContact { id: 1, a: 1, b: 0 },
            MockContact { id: 2, a: 4, b: 2 },
            MockContact { id: 3, a: 2, b: 2 },
        ];
        let joints: [MockJoint; 0] = [];
        let islands = build_islands(&bodies, &contacts, &joints).expect("valid");
        // island は bodies.first() 昇順: {0,1} / {2,4} / {3}
        assert_eq!(islands.len(), 3);
        assert_eq!(islands[0].bodies, vec![0, 1]);
        assert_eq!(islands[0].contacts, vec![0]);
        assert_eq!(
            islands[1].bodies,
            vec![2, 4],
            "static 2 attached to island of 4, sorted"
        );
        assert_eq!(islands[1].contacts, vec![1]);
        assert_eq!(islands[2].bodies, vec![3]);
        assert!(islands[2].contacts.is_empty());
        // static-static contact はどの island にも入らない
        assert!(islands.iter().all(|i| !i.contacts.contains(&2)));
        // joint は dyn 同士を merge し、static とは添付のみ
        let joints2 = [MockJoint { a: 3, b: 1 }, MockJoint { a: 2, b: 3 }];
        let with_joints = build_islands(&bodies, &contacts, &joints2).expect("valid");
        assert_eq!(with_joints.len(), 2);
        assert_eq!(with_joints[0].bodies, vec![0, 1, 2, 3]);
        assert_eq!(with_joints[0].joints, vec![0, 1]);
        assert_eq!(
            with_joints[1].bodies,
            vec![2, 4],
            "static body is attached to both islands"
        );
    }

    #[test]
    fn adaptive_substeps_clamp_and_repeated_addition_search() {
        struct V(Fix128);
        impl HasVelocity for V {
            fn velocity_l_inf(&self) -> Fix128 {
                self.0
            }
        }
        let cfg = AdaptiveSubStepConfig {
            max_translation_per_step: Fix128::from_ratio(1, 4),
            max_substeps: 6,
            min_substeps: 2,
        };
        let dt = Fix128::ONE;
        // travel <= 1/4 → min (2)
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::from_ratio(1, 4))], dt, &cfg),
            2
        );
        // travel 1 → n = 4 (4 * 1/4 >= 1)、travel 1.1 → 5、travel 100 → clamp 6
        assert_eq!(adaptive_substeps_for(&[V(Fix128::ONE)], dt, &cfg), 4);
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::from_ratio(11, 10))], dt, &cfg),
            5
        );
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::from_int(100))], dt, &cfg),
            6
        );
        // v_max は最大値 (順序非依存)
        assert_eq!(
            adaptive_substeps_for(
                &[V(Fix128::ZERO), V(Fix128::ONE), V(Fix128::from_ratio(1, 2))],
                dt,
                &cfg
            ),
            4
        );
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::ONE), V(Fix128::ZERO)], dt, &cfg),
            4
        );
        // dt が効く: v 2、dt 1/2 → travel 1 → 4
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::from_int(2))], Fix128::from_ratio(1, 2), &cfg),
            4
        );
        // min 0 は 1 に、max < min は min に clamp
        let odd = AdaptiveSubStepConfig {
            max_translation_per_step: Fix128::ONE,
            max_substeps: 1,
            min_substeps: 0,
        };
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::from_int(50))], dt, &odd),
            1
        );
        let inv = AdaptiveSubStepConfig {
            max_translation_per_step: Fix128::ONE,
            max_substeps: 2,
            min_substeps: 5,
        };
        assert_eq!(
            adaptive_substeps_for(&[V(Fix128::from_int(50))], dt, &inv),
            5
        );
        // ccd: effective step = min(radius * safety, max_translation)
        // radius 1/8 × safety 1 = 1/8 < 1/4 → travel 1 なら 8 → clamp 6
        assert_eq!(
            adaptive_substeps_for_ccd(
                &[V(Fix128::ONE)],
                dt,
                Fix128::from_ratio(1, 8),
                Fix128::ONE,
                &cfg
            ),
            6
        );
        // radius 1 × safety 1 = 1 > 1/4 → 1/4 のまま → 4
        assert_eq!(
            adaptive_substeps_for_ccd(&[V(Fix128::ONE)], dt, Fix128::ONE, Fix128::ONE, &cfg),
            4
        );
        // safety が効く: radius 1 × safety 1/8 → 1/8 → travel 1/2 で 4
        assert_eq!(
            adaptive_substeps_for_ccd(
                &[V(Fix128::from_ratio(1, 2))],
                dt,
                Fix128::ONE,
                Fix128::from_ratio(1, 8),
                &cfg
            ),
            4
        );
    }

    #[test]
    fn union_find_higher_rank_root_absorbs_lower_without_rank_growth() {
        let mut uf = UnionFind::new(4);
        uf.union(0, 1); // root rank 1
        let root = uf.find(0);
        assert!(uf.union(0, 3), "rank 1 側 (ri) が rank 0 (rj) を吸収");
        assert_eq!(uf.find(3), root);
        assert_eq!(uf.rank[root], 1, "rank は増えない");
        assert!(uf.union(2, 1), "逆向き (ri rank 0、rj rank 1)");
        assert_eq!(uf.find(2), root);
        assert_eq!(uf.rank[root], 1);
    }

    #[test]
    fn islands_static_body_touching_two_stacks_keeps_them_separate() {
        let bodies = [
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: false,
            },
            MockBody {
                id: 3,
                dynamic: true,
            },
        ];
        // 0-1 (dyn-static) と 1-2 (static-dyn): static 1 は両方に添付されるが 0 と 2 は別 island
        let contacts = [
            MockContact { id: 1, a: 0, b: 1 },
            MockContact { id: 2, a: 1, b: 2 },
        ];
        let joints: [MockJoint; 0] = [];
        let islands = build_islands(&bodies, &contacts, &joints).expect("valid");
        assert_eq!(islands.len(), 2);
        assert_eq!(islands[0].bodies, vec![0, 1]);
        assert_eq!(islands[0].contacts, vec![0]);
        assert_eq!(islands[1].bodies, vec![1, 2]);
        assert_eq!(islands[1].contacts, vec![1]);
        // joint 版も同じ
        let j2 = [MockJoint { a: 0, b: 1 }, MockJoint { a: 2, b: 1 }];
        let noc: [MockContact; 0] = [];
        let via_joint = build_islands(&bodies, &noc, &j2).expect("valid");
        assert_eq!(via_joint.len(), 2);
        assert_eq!(via_joint[0].joints, vec![0]);
        assert_eq!(via_joint[1].joints, vec![1]);
    }

    // ---- mutation-score tests batch 7 (2026-09-15、cargo-mutants missed 分) ----

    /// union by rank の `rank[ri] > rank[rj]` 分岐: ri が深い時は rj を ri の下に
    /// 吊るし rank は増えない (`<` 変異だと同 rank 分岐に落ちて rank が 2 になる)
    #[test]
    fn union_find_deeper_left_tree_absorbs_without_rank_growth() {
        let mut uf = UnionFind::new(5);
        assert!(uf.union(0, 1)); // {0,1} rank 1、root 0
        assert_eq!(uf.find(0), 0);
        assert_eq!(uf.rank[0], 1);
        // ri = 0 (rank 1) > rj = 2 (rank 0)
        assert!(uf.union(0, 2));
        assert_eq!(uf.parent[2], 0, "浅い木 2 が深い木 0 の直下");
        assert_eq!(uf.rank[0], 1, "rank 不変");
        assert_eq!(uf.rank[2], 0);
        // 逆向き: ri = 3 (rank 0) < rj = 0 (rank 1) → 3 が 0 の下
        assert!(uf.union(3, 0));
        assert_eq!(uf.parent[3], 0);
        assert_eq!(uf.rank[0], 1);
        // 同 rank: rank 1 同士の merge で 2 に
        let mut q = UnionFind::new(4);
        q.union(0, 1);
        q.union(2, 3);
        assert!(q.union(1, 3));
        let qr = q.find(0);
        assert_eq!(q.rank[qr], 2);
    }

    /// static body は同一 island 内で複数 contact に触れても 1 回だけ列挙される
    /// (`!dyn && !contains` の `||` 変異は既収録の static を再 push して重複する)
    #[test]
    fn islands_static_body_listed_once_across_multiple_contacts() {
        let bodies = [
            MockBody {
                id: 0,
                dynamic: false,
            },
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: true,
            },
        ];
        let joints: [MockJoint; 0] = [];
        // static が a 側に 2 回
        let as_a = [
            MockContact { id: 1, a: 0, b: 1 },
            MockContact { id: 2, a: 0, b: 2 },
            MockContact { id: 3, a: 1, b: 2 },
        ];
        let islands = build_islands(&bodies, &as_a, &joints).expect("valid");
        assert_eq!(islands.len(), 1);
        assert_eq!(islands[0].bodies, vec![0, 1, 2]);
        assert_eq!(islands[0].contacts, vec![0, 1, 2]);
        // static が b 側に 2 回
        let as_b = [
            MockContact { id: 1, a: 1, b: 0 },
            MockContact { id: 2, a: 2, b: 0 },
            MockContact { id: 3, a: 1, b: 2 },
        ];
        let islands = build_islands(&bodies, &as_b, &joints).expect("valid");
        assert_eq!(islands.len(), 1);
        assert_eq!(islands[0].bodies, vec![0, 1, 2]);
        assert_eq!(islands[0].contacts, vec![0, 1, 2]);
    }

    /// Island order when several islands share their smallest body index: a
    /// static ground (body 0) touched by three separate dynamic bodies gives
    /// three islands whose first body is 0. The order must be the full
    /// lexicographic order of the body lists, whatever the HashMap yields.
    #[test]
    fn islands_sharing_a_static_first_body_are_ordered_lexicographically() {
        let mut bodies = vec![MockBody {
            id: 0,
            dynamic: false,
        }];
        for id in 1..=8u64 {
            bodies.push(MockBody { id, dynamic: true });
        }
        let joints: [MockJoint; 0] = [];
        // ground touches 8, 3 and 5 (deliberately not ascending)
        let contacts = [
            MockContact { id: 1, a: 0, b: 8 },
            MockContact { id: 2, a: 3, b: 0 },
            MockContact { id: 3, a: 0, b: 5 },
        ];
        for _ in 0..16 {
            let islands = build_islands(&bodies, &contacts, &joints).expect("valid");
            // isolated dynamic bodies (1, 2, 4, 6, 7) each form a singleton
            // island after the three ground islands
            let order: Vec<Vec<usize>> = islands.iter().map(|i| i.bodies.clone()).collect();
            assert_eq!(
                order,
                vec![
                    vec![0, 3],
                    vec![0, 5],
                    vec![0, 8],
                    vec![1],
                    vec![2],
                    vec![4],
                    vec![6],
                    vec![7]
                ]
            );
            assert_eq!(islands[0].contacts, vec![1]);
            assert_eq!(islands[1].contacts, vec![2]);
            assert_eq!(islands[2].contacts, vec![0]);
        }
    }

    /// joint 経由の static 添付: 1 回だけ列挙 (`||` 変異 → 重複) かつ必ず添付される
    /// (`!` 削除変異 → static b が island に入らない)
    #[test]
    fn islands_static_body_attached_once_via_joints() {
        let bodies = [
            MockBody {
                id: 0,
                dynamic: false,
            },
            MockBody {
                id: 1,
                dynamic: true,
            },
            MockBody {
                id: 2,
                dynamic: true,
            },
        ];
        let contacts: [MockContact; 0] = [];
        // static が a 側に 2 回
        let as_a = [
            MockJoint { a: 0, b: 1 },
            MockJoint { a: 0, b: 2 },
            MockJoint { a: 1, b: 2 },
        ];
        let islands = build_islands(&bodies, &contacts, &as_a).expect("valid");
        assert_eq!(islands.len(), 1);
        assert_eq!(islands[0].bodies, vec![0, 1, 2]);
        assert_eq!(islands[0].joints, vec![0, 1, 2]);
        // static が b 側に 2 回
        let as_b = [
            MockJoint { a: 1, b: 0 },
            MockJoint { a: 2, b: 0 },
            MockJoint { a: 1, b: 2 },
        ];
        let islands = build_islands(&bodies, &contacts, &as_b).expect("valid");
        assert_eq!(islands.len(), 1);
        assert_eq!(islands[0].bodies, vec![0, 1, 2]);
        assert_eq!(islands[0].joints, vec![0, 1, 2]);
        // 単一 joint (dyn a、static b): static b が添付される
        let single = [MockJoint { a: 1, b: 0 }];
        let islands = build_islands(&bodies, &contacts, &single).expect("valid");
        assert_eq!(islands.len(), 2);
        assert_eq!(islands[0].bodies, vec![0, 1]);
        assert_eq!(islands[0].joints, vec![0]);
        assert_eq!(islands[1].bodies, vec![2]);
        assert!(islands[1].joints.is_empty());
        // dyn-static joint は 2 つの dyn を融合しない (`&&` → `||` 変異は union してしまう)
        // (両 island の bodies.first() が static 0 で同点 → 出力順は HashMap 依存、bodies 全体で比較)
        let bridge = [MockJoint { a: 1, b: 0 }, MockJoint { a: 0, b: 2 }];
        let mut islands = build_islands(&bodies, &contacts, &bridge).expect("valid");
        islands.sort_by(|x, y| x.bodies.cmp(&y.bodies));
        assert_eq!(islands.len(), 2);
        assert_eq!(islands[0].bodies, vec![0, 1]);
        assert_eq!(islands[0].joints, vec![0]);
        assert_eq!(islands[1].bodies, vec![0, 2]);
        assert_eq!(islands[1].joints, vec![1]);
    }

    /// adapter accessor が下層 constraint の index をそのまま返す
    /// (`→ 0` / `→ 1` 変異を index 2 / 3 / 6 / 8 で検出)
    #[test]
    fn adapter_refs_forward_body_indices_and_ids() {
        use crate::math::Vec3Fix;
        let joint = DistanceConstraint::new(2, 3, Vec3Fix::ZERO, Vec3Fix::ZERO, Fix128::ONE);
        let jref = DistanceRef { joint: &joint };
        assert_eq!(JointLike::body_a(&jref), 2);
        assert_eq!(JointLike::body_b(&jref), 3);
        let joint2 = DistanceConstraint::new(6, 8, Vec3Fix::ZERO, Vec3Fix::ZERO, Fix128::ONE);
        let jref2 = DistanceRef { joint: &joint2 };
        assert_eq!(JointLike::body_a(&jref2), 6);
        assert_eq!(JointLike::body_b(&jref2), 8);
        // build_islands 経由でも index が正しく伝わる (body 2-3 が結合)
        let bodies: Vec<MockBody> = (0..4)
            .map(|i| MockBody {
                id: i,
                dynamic: true,
            })
            .collect();
        let contact = MockContact { id: 5, a: 2, b: 3 };
        let islands = build_islands(&bodies, &[contact], &[jref]).expect("valid");
        assert_eq!(islands.len(), 3);
        assert_eq!(islands[2].bodies, vec![2, 3]);
        assert_eq!(islands[2].contacts, vec![0]);
        assert_eq!(islands[2].joints, vec![0]);
    }

    /// `par_dispatch_islands` は全 island に対して closure を 1 回ずつ呼び、
    /// 結果を island の canonical order で返す (`→ ()` / 順序破壊変異を検出)
    #[cfg(feature = "parallel")]
    #[test]
    fn par_dispatch_islands_runs_closure_for_each_island_exactly_once() {
        let islands: Vec<Island> = (0..7)
            .map(|i| Island {
                bodies: vec![i * 2, i * 2 + 1],
                contacts: vec![i],
                joints: vec![],
            })
            .collect();
        let mut aux = vec![0u32; islands.len()];
        let results: Vec<usize> = par_dispatch_islands(&islands, &mut aux, |isl, slot| {
            *slot += 1;
            isl.contacts[0]
        });
        assert_eq!(
            results,
            (0..7).collect::<Vec<_>>(),
            "order preserved, each island visited exactly once"
        );
        assert!(
            aux.iter().all(|&v| v == 1),
            "each aux slot mutated exactly once (no double-dispatch, no skip)"
        );

        // 空 slice は 0 回、空 aux + 空 result。
        let mut none_aux: Vec<u32> = Vec::new();
        let none: Vec<usize> = par_dispatch_islands(&[], &mut none_aux, |_, slot| {
            *slot += 1;
            0
        });
        assert!(none.is_empty());
    }

    /// `AdaptiveSubStepConfig::default()`: 0.1 world units per sub-step,
    /// clamp `[1, 16]`. A body at `v` world units/s over `dt = 1/60` needs
    /// `ceil(v / 6)` sub-steps, and a static scene needs exactly 1.
    #[test]
    fn adaptive_substep_default_gives_ceil_of_travel_over_tenth() {
        let cfg = AdaptiveSubStepConfig::default();
        assert_eq!((cfg.min_substeps, cfg.max_substeps), (1, 16));
        assert!((cfg.max_translation_per_step.to_f64() - 0.1).abs() < 1e-12);
        struct V(Fix128);
        impl HasVelocity for V {
            fn velocity_l_inf(&self) -> Fix128 {
                self.0
            }
        }
        let dt = Fix128::from_ratio(1, 60);
        let n = |v: i64| adaptive_substeps_for(&[V(Fix128::from_int(v))], dt, &cfg);
        assert_eq!(n(0), 1, "static scene");
        assert_eq!(n(6), 1, "travel exactly 0.1");
        assert_eq!(n(7), 2, "travel 0.1167 -> 2 sub-steps");
        assert_eq!(n(60), 10, "travel 1.0 -> 10 sub-steps");
        assert_eq!(n(96), 16, "travel 1.6 -> 16 (cap)");
        assert_eq!(n(10_000), 16, "clamped at max_substeps");
    }
}