dreamwell_engine/physics/

simulation.rs

// PhysicsWorld — CPU-deterministic rigid body simulation.
//
// Authority-side physics: collision detection, contact resolution, raycasting.
// Results sync to GPU scene transforms each frame. GPU handles DreamMatter
// (presentation-only particles); CPU handles gameplay-relevant physics.
//
// Integration: semi-implicit Euler (stable for game physics).
// Broadphase: spatial hash grid (O(n) pair generation).
// Narrowphase: analytical sphere-sphere, sphere-plane, AABB-AABB, sphere-AABB.
// Solver: sequential impulse (Erin Catto / Box2D-style).

/// Unique body identifier.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub struct BodyId(pub u32);

/// Collision shape for narrowphase detection.
#[derive(Debug, Clone, Copy)]
pub enum CollisionShape {
    Sphere { radius: f32 },
    Plane { normal: [f32; 3], d: f32 },
    Aabb { half_extents: [f32; 3] },
    Capsule { radius: f32, half_height: f32 },  // Y-axis aligned
    Cylinder { radius: f32, half_height: f32 }, // Y-axis aligned
    Cone { radius: f32, height: f32 },          // Y-axis, tip at +height
}

/// Rigid body state.
#[derive(Debug, Clone)]
pub struct RigidBody {
    pub position: [f32; 3],
    pub velocity: [f32; 3],
    pub rotation: [f32; 4], // quaternion (x, y, z, w)
    pub angular_velocity: [f32; 3],
    pub mass: f32,
    pub inv_mass: f32,
    pub restitution: f32,
    pub friction: f32,
    pub shape: CollisionShape,
    pub is_static: bool,
    pub is_active: bool,
    pub sleep_frames: u16,
    pub is_sleeping: bool,
    /// Cosmetic-only body: collisions are skipped when Superposed.
    /// Gameplay-critical bodies (NPCs, triggers, quest objects) must leave this false.
    /// Cosmetic bodies (crates, barrels, debris, foliage) can set this true for
    /// observer-dependent collision skip — the body is physically correct when Active
    /// but not resolved when no observer is watching.
    pub cosmetic_only: bool,
}

impl RigidBody {
    /// Create a dynamic body with the given mass and shape.
    pub fn dynamic(mass: f32, shape: CollisionShape) -> Self {
        Self {
            position: [0.0; 3],
            velocity: [0.0; 3],
            rotation: [0.0, 0.0, 0.0, 1.0],
            angular_velocity: [0.0; 3],
            mass,
            inv_mass: if mass > 0.0 { 1.0 / mass } else { 0.0 },
            restitution: 0.5,
            friction: 0.3,
            shape,
            is_static: false,
            is_active: true,
            sleep_frames: 0,
            is_sleeping: false,
            cosmetic_only: false,
        }
    }

    /// Create a static (immovable) body.
    pub fn fixed(shape: CollisionShape) -> Self {
        Self {
            position: [0.0; 3],
            velocity: [0.0; 3],
            rotation: [0.0, 0.0, 0.0, 1.0],
            angular_velocity: [0.0; 3],
            mass: 0.0,
            inv_mass: 0.0,
            restitution: 0.5,
            friction: 0.5,
            shape,
            is_static: true,
            is_active: true,
            sleep_frames: 0,
            is_sleeping: false,
            cosmetic_only: false,
        }
    }

    /// Mark this body as cosmetic-only. When Superposed, its collisions are skipped.
    /// Use for debris, crates, barrels, foliage — anything not gameplay-critical.
    pub fn as_cosmetic(mut self) -> Self {
        self.cosmetic_only = true;
        self
    }

    pub fn with_position(mut self, pos: [f32; 3]) -> Self {
        self.position = pos;
        self
    }

    pub fn with_restitution(mut self, e: f32) -> Self {
        self.restitution = e;
        self
    }
}

/// Contact point between two bodies.
#[derive(Debug, Clone, Copy)]
pub struct Contact {
    pub body_a: usize,
    pub body_b: usize,
    pub normal: [f32; 3], // points from A to B
    pub depth: f32,       // penetration depth (positive = overlapping)
    pub point: [f32; 3],  // contact point in world space
    /// Cached impulse from previous frame for warm-starting.
    pub cached_impulse: f32,
}

/// Raycast hit result.
#[derive(Debug, Clone, Copy)]
pub struct RayHit {
    pub position: [f32; 3],
    pub normal: [f32; 3],
    pub distance: f32,
    pub body_index: usize,
}

/// Configuration for body sleeping thresholds.
#[derive(Debug, Clone, Copy)]
pub struct SleepConfig {
    pub linear_threshold: f32,
    pub angular_threshold: f32,
    pub frames_to_sleep: u16,
}

impl Default for SleepConfig {
    fn default() -> Self {
        Self {
            linear_threshold: 0.01,
            angular_threshold: 0.01,
            frames_to_sleep: 60,
        }
    }
}

// ═══════════════════════════════════════════════════════════════════
// Sweep-and-Prune Broadphase (Clean Compute v1.0.0)
//
// 1-axis sort + linear sweep. O(N log N) populate, O(N × overlap) query.
// No hash collisions, no 27-cell fan-out, no dedup sort.
// Produces canonical pairs (i < j) directly — zero duplicates by construction.
// ═══════════════════════════════════════════════════════════════════

/// AABB interval for sweep-and-prune: [min_x, max_x, min_y, max_y, min_z, max_z, body_index].
#[derive(Clone, Copy)]
struct SapEntry {
    min: [f32; 3],
    max: [f32; 3],
    index: usize,
}

/// Sweep-and-Prune broadphase. Sorts bodies by X-axis AABB min bound,
/// then sweeps to find overlapping intervals. Y/Z overlap checked inline.
/// Pre-allocates all Vecs for Clean Compute (zero per-step allocation).
#[derive(Clone)]
pub struct SweepAndPrune {
    entries: Vec<SapEntry>,
    /// Pre-allocated pair output buffer — reused every step (Clean Compute).
    pairs: Vec<(usize, usize)>,
}

impl SweepAndPrune {
    pub fn new() -> Self {
        Self {
            entries: Vec::with_capacity(1024),
            pairs: Vec::with_capacity(4096),
        }
    }

    /// Extract AABB from a collision shape at a given position.
    #[inline]
    fn body_aabb(pos: &[f32; 3], shape: &CollisionShape) -> ([f32; 3], [f32; 3]) {
        match shape {
            CollisionShape::Sphere { radius } => {
                let r = *radius;
                (
                    [pos[0] - r, pos[1] - r, pos[2] - r],
                    [pos[0] + r, pos[1] + r, pos[2] + r],
                )
            }
            CollisionShape::Aabb { half_extents } => (
                [
                    pos[0] - half_extents[0],
                    pos[1] - half_extents[1],
                    pos[2] - half_extents[2],
                ],
                [
                    pos[0] + half_extents[0],
                    pos[1] + half_extents[1],
                    pos[2] + half_extents[2],
                ],
            ),
            CollisionShape::Capsule { radius, half_height } => {
                let r = *radius;
                let h = *half_height;
                (
                    [pos[0] - r, pos[1] - h - r, pos[2] - r],
                    [pos[0] + r, pos[1] + h + r, pos[2] + r],
                )
            }
            CollisionShape::Cylinder { radius, half_height } => {
                let r = *radius;
                let h = *half_height;
                (
                    [pos[0] - r, pos[1] - h, pos[2] - r],
                    [pos[0] + r, pos[1] + h, pos[2] + r],
                )
            }
            CollisionShape::Cone { radius, height } => {
                let r = *radius;
                (
                    [pos[0] - r, pos[1], pos[2] - r],
                    [pos[0] + r, pos[1] + height, pos[2] + r],
                )
            }
            CollisionShape::Plane { .. } => {
                // Planes are infinite — use large AABB
                ([-1e6, -1e6, -1e6], [1e6, 1e6, 1e6])
            }
        }
    }

    /// Populate from body arrays. Extracts AABBs and sorts by X-axis min.
    pub fn populate(&mut self, bodies: &[RigidBody]) {
        self.entries.clear();
        self.entries.reserve(bodies.len());
        for (i, body) in bodies.iter().enumerate() {
            if !body.is_active {
                continue;
            }
            let (min, max) = Self::body_aabb(&body.position, &body.shape);
            self.entries.push(SapEntry { min, max, index: i });
        }
        // Sort by X-axis min bound.
        #[cfg(feature = "parallel-physics")]
        {
            use rayon::slice::ParallelSliceMut;
            self.entries
                .par_sort_unstable_by(|a, b| a.min[0].partial_cmp(&b.min[0]).unwrap_or(std::cmp::Ordering::Equal));
        }
        #[cfg(not(feature = "parallel-physics"))]
        self.entries
            .sort_unstable_by(|a, b| a.min[0].partial_cmp(&b.min[0]).unwrap_or(std::cmp::Ordering::Equal));
    }

    /// Sweep-and-prune pair generation. Returns pre-allocated pairs Vec (no allocation).
    /// Pairs are canonical (i < j) by construction — zero duplicates.
    pub fn query_pairs(&mut self) -> &[(usize, usize)] {
        self.pairs.clear();
        let n = self.entries.len();
        for i in 0..n {
            let a = &self.entries[i];
            // Sweep right: check all entries whose X-min < our X-max
            for j in (i + 1)..n {
                let b = &self.entries[j];
                // X-axis: if b's min > a's max, no more overlaps possible (sorted!)
                if b.min[0] > a.max[0] {
                    break;
                }
                // Y-axis overlap test
                if a.max[1] < b.min[1] || b.max[1] < a.min[1] {
                    continue;
                }
                // Z-axis overlap test
                if a.max[2] < b.min[2] || b.max[2] < a.min[2] {
                    continue;
                }
                // All 3 axes overlap — this is a candidate pair.
                let (lo, hi) = if a.index < b.index {
                    (a.index, b.index)
                } else {
                    (b.index, a.index)
                };
                self.pairs.push((lo, hi));
            }
        }
        &self.pairs
    }
}

/// Spatial hash grid for broadphase pair generation.
/// Replaces O(n^2) all-pairs with O(n) expected-case neighbour lookups.
#[derive(Debug, Clone)]
pub struct SpatialHashGrid {
    cell_size: f32,
    inv_cell_size: f32,
    entries: Vec<(u64, usize)>, // (cell_hash, body_index)
}

impl SpatialHashGrid {
    pub fn new(cell_size: f32) -> Self {
        Self {
            cell_size,
            inv_cell_size: 1.0 / cell_size,
            entries: Vec::new(),
        }
    }

    /// Cell size used for spatial hashing (debug/query introspection).
    pub fn cell_size(&self) -> f32 {
        self.cell_size
    }

    /// Update cell size. Call before populate() when body density changes.
    pub fn set_cell_size(&mut self, size: f32) {
        self.cell_size = size;
        self.inv_cell_size = 1.0 / size;
    }

    /// Inverse cell size (for wave coherence broadphase access).
    pub fn inv_cell_size(&self) -> f32 {
        self.inv_cell_size
    }

    /// Sorted entries (for wave coherence per-body regeneration).
    pub fn entries(&self) -> &[(u64, usize)] {
        &self.entries
    }

    /// Hash a 3D integer cell coordinate using FNV-1a.
    fn hash_cell(ix: i32, iy: i32, iz: i32) -> u64 {
        let mut h: u64 = 0xcbf29ce484222325;
        for byte in ix
            .to_le_bytes()
            .iter()
            .chain(iy.to_le_bytes().iter())
            .chain(iz.to_le_bytes().iter())
        {
            h ^= *byte as u64;
            h = h.wrapping_mul(0x100000001b3);
        }
        h
    }

    /// Populate the grid from body positions. Sorts entries by hash for fast lookup.
    pub fn populate(&mut self, bodies: &[RigidBody]) {
        self.entries.clear();
        self.entries.reserve(bodies.len());
        let inv = self.inv_cell_size;
        for (i, body) in bodies.iter().enumerate() {
            if !body.is_active {
                continue;
            }
            let ix = (body.position[0] * inv).floor() as i32;
            let iy = (body.position[1] * inv).floor() as i32;
            let iz = (body.position[2] * inv).floor() as i32;
            let hash = Self::hash_cell(ix, iy, iz);
            self.entries.push((hash, i));
        }
        // Clean Compute: parallel sort when available (rayon).
        #[cfg(feature = "parallel-physics")]
        {
            use rayon::slice::ParallelSliceMut;
            self.entries.par_sort_unstable_by_key(|e| e.0);
        }
        #[cfg(not(feature = "parallel-physics"))]
        {
            self.entries.sort_unstable_by_key(|e| e.0);
        }
    }

    /// Generate candidate pairs using half-neighborhood search.
    ///
    /// Instead of querying all 27 neighbor cells (which produces duplicate pairs
    /// requiring O(P log P) sort + O(P) dedup), we query only the 14 "forward"
    /// cells: the self-cell plus 13 cells that are lexicographically greater in
    /// (dx, dy, dz) order. This guarantees each pair is discovered exactly once.
    ///
    /// Mathematical proof: For bodies A in cell C_A and B in cell C_B where
    /// C_A and C_B are adjacent (differ by at most 1 on each axis):
    /// - If C_A < C_B lexicographically: A's forward search includes C_B → pair found by A
    /// - If C_A > C_B lexicographically: B's forward search includes C_A → pair found by B
    /// - If C_A == C_B: same cell, j > i index guard → pair found once
    ///
    /// This is the 3D analog of the causal attention mask: compute only the upper
    /// triangle of the spatial relevance matrix. Zero duplicates by construction.
    /// No sort. No dedup. O(N × 14 × log N) vs O(N × 27 × log N + P log P).
    ///
    /// Reference: Allen & Tildesley, "Computer Simulation of Liquids" (1987),
    /// half-neighbor Verlet list construction.
    pub fn query_pairs(&self, bodies: &[RigidBody]) -> Vec<(usize, usize)> {
        // Forward half-neighborhood: self + 13 cells where (dx,dy,dz) > (0,0,0).
        // Lexicographic order: compare dx first, then dy, then dz.
        const FORWARD: [(i32, i32, i32); 14] = [
            // Self cell (j > i guard handles dedup within cell)
            (0, 0, 0),
            // 13 forward neighbors (lexicographically > (0,0,0))
            (0, 0, 1),
            (0, 1, -1),
            (0, 1, 0),
            (0, 1, 1),
            (1, -1, -1),
            (1, -1, 0),
            (1, -1, 1),
            (1, 0, -1),
            (1, 0, 0),
            (1, 0, 1),
            (1, 1, -1),
            (1, 1, 0),
            (1, 1, 1),
        ];

        #[cfg(feature = "parallel-physics")]
        {
            use rayon::prelude::*;
            let entries = &self.entries;
            let inv = self.inv_cell_size;

            // Parallel: each body searches 14 cells independently.
            // No sort or dedup needed — pairs are unique by construction.
            bodies
                .par_iter()
                .enumerate()
                .filter(|(_, body)| body.is_active)
                .flat_map_iter(|(i, body)| {
                    let cx = (body.position[0] * inv).floor() as i32;
                    let cy = (body.position[1] * inv).floor() as i32;
                    let cz = (body.position[2] * inv).floor() as i32;
                    let mut local = Vec::new();
                    for &(dx, dy, dz) in &FORWARD {
                        let hash = Self::hash_cell(cx + dx, cy + dy, cz + dz);
                        let start = entries.partition_point(|e| e.0 < hash);
                        for entry in &entries[start..] {
                            if entry.0 != hash {
                                break;
                            }
                            let j = entry.1;
                            if dx == 0 && dy == 0 && dz == 0 {
                                // Self cell: only emit j > i to avoid self-pair and reverse.
                                if j > i {
                                    local.push((i, j));
                                }
                            } else {
                                // Forward cell: emit all pairs with canonical order.
                                if j != i {
                                    local.push((i.min(j), i.max(j)));
                                }
                            }
                        }
                    }
                    local
                })
                .collect()
            // NO par_sort_unstable. NO dedup. Pairs are unique by construction.
        }

        #[cfg(not(feature = "parallel-physics"))]
        {
            let mut pairs = Vec::new();
            for (i, body) in bodies.iter().enumerate() {
                if !body.is_active {
                    continue;
                }
                let cx = (body.position[0] * self.inv_cell_size).floor() as i32;
                let cy = (body.position[1] * self.inv_cell_size).floor() as i32;
                let cz = (body.position[2] * self.inv_cell_size).floor() as i32;
                for &(dx, dy, dz) in &FORWARD {
                    let hash = Self::hash_cell(cx + dx, cy + dy, cz + dz);
                    let start = self.entries.partition_point(|e| e.0 < hash);
                    for entry in &self.entries[start..] {
                        if entry.0 != hash {
                            break;
                        }
                        let j = entry.1;
                        if dx == 0 && dy == 0 && dz == 0 {
                            if j > i {
                                pairs.push((i, j));
                            }
                        } else {
                            if j != i {
                                pairs.push((i.min(j), i.max(j)));
                            }
                        }
                    }
                }
            }
            // NO sort. NO dedup. Unique by half-neighborhood construction.
            pairs
        }
    }
}

// ═══════════════════════════════════════════════════════════════════
// DreamSuperposition — Three-State Body Lifecycle (v1.0.0 LTS)
//
// Formalizes the quantum analogy: a body exists in superposition until
// an observer collapses it into a definite state by being near enough
// to perceive it.
//
// States:
//   Active    — near observer, physics fully simulated, meshlets rendered
//   Superposed — far from observer, skip broadphase + GPU cull entirely
//   Dormant   — sleeping + superposed, zero CPU cost, zero GPU cost
//
// Transitions:
//   Active → Superposed:  body exits observer FOV radius
//   Superposed → Active:  body enters observer FOV radius
//   Superposed → Dormant: body meets sleep threshold while superposed
//   Dormant → Active:     external force or observer approaches
//   Active → Dormant:     sleep threshold met while near observer (normal sleep)
//
// Cost model:
//   Active bodies:     full DreamSpace broadphase + Quantum Culling
//   Superposed bodies: zero broadphase, zero GPU cull dispatch
//   Dormant bodies:    zero everything (existing sleep + superposition)
// ═══════════════════════════════════════════════════════════════════

/// Superposition state for a rigid body.
///
/// Four states model the full quantum lifecycle:
///   Active     — collapsed, fully observed, full physics + GPU render
///   Decohering — transition zone, reduced-rate physics, GPU temporal coherence
///   Superposed — unobserved, no physics, pre-zeroed GPU indirect buffer
///   Dormant    — unobserved + sleeping, zero cost everywhere
///
/// Decoherence rings provide smooth transition between Active and Superposed,
/// preventing visual pop-in at the observation boundary. Bodies in the
/// Decohering ring simulate every Nth tick (configurable), providing a
/// "fade to superposition" gradient.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(u8)]
pub enum SuperpositionState {
    /// Near observer. Full physics every tick. Full GPU culling.
    Active = 0,
    /// Transition zone. Physics runs every `decoherence_tick_rate` ticks.
    /// GPU uses temporal coherence (cull at reduced frequency).
    Decohering = 1,
    /// Far from observer. No physics. GPU indirect buffer pre-zeroed.
    Superposed = 2,
    /// Sleeping + far from observer. Zero cost everywhere.
    Dormant = 3,
}

/// Observer position and decoherence ring radii.
///
/// Three concentric rings around the observer:
///   [0, active_radius]             → Active (full simulation)
///   [active_radius, decohere_radius] → Decohering (reduced-rate simulation)
///   [decohere_radius, ∞)            → Superposed (no simulation)
///
/// The decoherence ring prevents visual pop-in: bodies warm up for a few
/// ticks before becoming fully Active, and cool down before going Superposed.
#[derive(Debug, Clone, Copy)]
pub struct SuperpositionObserver {
    pub position: [f32; 3],
    /// Inner radius: full Active simulation.
    pub active_radius: f32,
    pub active_radius_sq: f32,
    /// Outer radius: Decohering transition zone beyond active_radius.
    pub decohere_radius: f32,
    pub decohere_radius_sq: f32,
    /// How often Decohering bodies simulate (every Nth tick). Default: 2.
    pub decohere_tick_rate: u32,
    /// Current tick index (for modulo check on Decohering bodies).
    pub tick_index: u32,
}

impl SuperpositionObserver {
    /// Create with default decoherence ring (1.5× active radius, simulate every 2nd tick).
    pub fn new(position: [f32; 3], active_radius: f32) -> Self {
        let decohere_radius = active_radius * 1.5;
        Self {
            position,
            active_radius,
            active_radius_sq: active_radius * active_radius,
            decohere_radius,
            decohere_radius_sq: decohere_radius * decohere_radius,
            decohere_tick_rate: 2,
            tick_index: 0,
        }
    }

    /// Create with explicit decoherence ring.
    pub fn with_rings(position: [f32; 3], active_radius: f32, decohere_radius: f32, tick_rate: u32) -> Self {
        Self {
            position,
            active_radius,
            active_radius_sq: active_radius * active_radius,
            decohere_radius,
            decohere_radius_sq: decohere_radius * decohere_radius,
            decohere_tick_rate: tick_rate.max(1),
            tick_index: 0,
        }
    }

    /// Advance tick index (call once per step).
    pub fn advance_tick(&mut self) {
        self.tick_index = self.tick_index.wrapping_add(1);
    }

    /// Should a Decohering body simulate this tick?
    #[inline(always)]
    pub fn should_decohere_simulate(&self) -> bool {
        self.tick_index % self.decohere_tick_rate == 0
    }

    /// Classify a position into a superposition ring.
    #[inline(always)]
    pub fn classify(&self, body_pos: &[f32; 3]) -> SuperpositionState {
        let dx = body_pos[0] - self.position[0];
        let dy = body_pos[1] - self.position[1];
        let dz = body_pos[2] - self.position[2];
        let dist_sq = dx * dx + dy * dy + dz * dz;
        if dist_sq <= self.active_radius_sq {
            SuperpositionState::Active
        } else if dist_sq <= self.decohere_radius_sq {
            SuperpositionState::Decohering
        } else {
            SuperpositionState::Superposed
        }
    }
}

// ═══════════════════════════════════════════════════════════════════
// DreamSpace — Incremental Observer-Aware Spatial Index (v1.0.0 LTS)
//
// Replaces per-frame rebuild-from-scratch with persistent cell map +
// dirty-set tracking. Only bodies that cross cell boundaries trigger
// re-indexing. Pair generation queries only dirty cells + neighbors.
//
// Performance model:
//   OLD: O(n log n) sort + O(n × 27 × log n) query every frame
//   NEW: O(M) dirty update + O(M × 27 × O(1)) query per frame
//        where M = bodies that changed cells (<< N)
//
// Cross-domain synthesis:
//   DreamMemory insight: separate recording from computing
//   Applied here: separate position tracking from pair generation
//   Result: only moved bodies pay the broadphase cost
// ═══════════════════════════════════════════════════════════════════

/// DreamSpace — incremental spatial index with dirty-cell pair generation.
pub struct DreamSpace {
    cell_size: f32,
    inv_cell_size: f32,
    /// Persistent cell map: cell_key → list of body indices.
    cells: std::collections::HashMap<(i32, i32, i32), Vec<usize>>,
    /// Per-body cell assignment from the previous frame.
    body_cells: Vec<(i32, i32, i32)>,
    /// Dirty cell set: cells that had bodies enter or leave this frame.
    dirty_cells: Vec<(i32, i32, i32)>,
    /// Whether the index has been initialized (first populate).
    initialized: bool,
}

impl DreamSpace {
    pub fn new(cell_size: f32) -> Self {
        Self {
            cell_size,
            inv_cell_size: 1.0 / cell_size,
            cells: std::collections::HashMap::new(),
            body_cells: Vec::new(),
            dirty_cells: Vec::new(),
            initialized: false,
        }
    }

    #[inline(always)]
    fn cell_of(&self, pos: &[f32; 3]) -> (i32, i32, i32) {
        (
            (pos[0] * self.inv_cell_size).floor() as i32,
            (pos[1] * self.inv_cell_size).floor() as i32,
            (pos[2] * self.inv_cell_size).floor() as i32,
        )
    }

    /// Initial population — called once, or when body count changes.
    pub fn rebuild(&mut self, bodies: &[RigidBody]) {
        self.cells.clear();
        self.body_cells.clear();
        self.body_cells.reserve(bodies.len());
        self.dirty_cells.clear();

        for (i, body) in bodies.iter().enumerate() {
            let cell = if body.is_active {
                self.cell_of(&body.position)
            } else {
                (i32::MIN, i32::MIN, i32::MIN)
            };
            self.body_cells.push(cell);
            if body.is_active {
                self.cells.entry(cell).or_default().push(i);
            }
        }
        self.initialized = true;
        // After rebuild, ALL cells are dirty (first frame).
        self.dirty_cells = self.cells.keys().copied().collect();
    }

    /// Incremental update — called each step AFTER position integration.
    /// Scans all bodies, detects cell boundary crossings, updates cell map.
    /// Only crossing bodies cause writes. Sleeping/static bodies = zero cost.
    pub fn update(&mut self, bodies: &[RigidBody]) {
        // Handle body count changes (spawn/despawn).
        if self.body_cells.len() != bodies.len() || !self.initialized {
            self.rebuild(bodies);
            return;
        }

        self.dirty_cells.clear();

        for (i, body) in bodies.iter().enumerate() {
            let new_cell = if body.is_active && !body.is_sleeping {
                self.cell_of(&body.position)
            } else if body.is_active {
                // Sleeping but active: keep current cell, no update needed.
                continue;
            } else {
                (i32::MIN, i32::MIN, i32::MIN)
            };

            let old_cell = self.body_cells[i];
            if new_cell == old_cell {
                continue; // Same cell — no work.
            }

            // Cell boundary crossed. Update the map.
            // Remove from old cell.
            if old_cell != (i32::MIN, i32::MIN, i32::MIN) {
                if let Some(list) = self.cells.get_mut(&old_cell) {
                    if let Some(pos) = list.iter().position(|&idx| idx == i) {
                        list.swap_remove(pos);
                    }
                    if list.is_empty() {
                        self.cells.remove(&old_cell);
                    }
                }
                self.dirty_cells.push(old_cell);
            }

            // Insert into new cell.
            if new_cell != (i32::MIN, i32::MIN, i32::MIN) {
                self.cells.entry(new_cell).or_default().push(i);
                self.dirty_cells.push(new_cell);
            }

            self.body_cells[i] = new_cell;
        }

        // Deduplicate dirty cells.
        self.dirty_cells.sort_unstable();
        self.dirty_cells.dedup();
    }

    /// Generate collision pairs from dirty cells + their 27 neighbors.
    /// Only bodies near dirty regions are checked — sleeping clusters are skipped.
    pub fn query_dirty_pairs(&self, bodies: &[RigidBody]) -> Vec<(usize, usize)> {
        // Collect all cells we need to query: dirty cells + their 27 neighbors.
        let mut query_cells: Vec<(i32, i32, i32)> = Vec::with_capacity(self.dirty_cells.len() * 27);
        for &(cx, cy, cz) in &self.dirty_cells {
            for dx in -1..=1 {
                for dy in -1..=1 {
                    for dz in -1..=1 {
                        query_cells.push((cx + dx, cy + dy, cz + dz));
                    }
                }
            }
        }
        query_cells.sort_unstable();
        query_cells.dedup();

        // For each query cell, check all body pairs within it.
        let mut pairs = Vec::new();
        for &cell in &query_cells {
            let Some(list) = self.cells.get(&cell) else { continue };
            // Check each body in this cell against all bodies in 27 neighbors.
            for &i in list {
                if !bodies[i].is_active {
                    continue;
                }
                let (cx, cy, cz) = self.cell_of(&bodies[i].position);
                for dx in -1..=1 {
                    for dy in -1..=1 {
                        for dz in -1..=1 {
                            let neighbor = (cx + dx, cy + dy, cz + dz);
                            let Some(nlist) = self.cells.get(&neighbor) else {
                                continue;
                            };
                            for &j in nlist {
                                if j > i && bodies[j].is_active {
                                    pairs.push((i, j));
                                }
                            }
                        }
                    }
                }
            }
        }
        pairs.sort_unstable();
        pairs.dedup();
        pairs
    }

    /// Full rebuild query — used on first frame or after large changes.
    /// Generates all pairs (equivalent to old query_pairs but with HashMap O(1) lookup).
    pub fn query_all_pairs(&self, bodies: &[RigidBody]) -> Vec<(usize, usize)> {
        let mut pairs = Vec::new();
        for (i, body) in bodies.iter().enumerate() {
            if !body.is_active {
                continue;
            }
            let (cx, cy, cz) = self.cell_of(&body.position);
            for dx in -1..=1 {
                for dy in -1..=1 {
                    for dz in -1..=1 {
                        let neighbor = (cx + dx, cy + dy, cz + dz);
                        let Some(list) = self.cells.get(&neighbor) else {
                            continue;
                        };
                        for &j in list {
                            if j > i && bodies[j].is_active {
                                pairs.push((i, j));
                            }
                        }
                    }
                }
            }
        }
        pairs.sort_unstable();
        pairs.dedup();
        pairs
    }

    pub fn dirty_cell_count(&self) -> usize {
        self.dirty_cells.len()
    }
    pub fn total_cell_count(&self) -> usize {
        self.cells.len()
    }
    pub fn is_initialized(&self) -> bool {
        self.initialized
    }

    /// Coherence collapse: classify all occupied cells by observer distance.
    /// Returns (active_cells, decohering_cells, superposed_cells) as lists of cell keys.
    /// Bodies in a cell share the cell's superposition state — "coherent superposition."
    /// This replaces per-body distance checks with per-cell checks: O(C) instead of O(N).
    pub fn classify_cells(
        &self,
        observer: &SuperpositionObserver,
    ) -> (Vec<(i32, i32, i32)>, Vec<(i32, i32, i32)>, Vec<(i32, i32, i32)>) {
        let mut active = Vec::new();
        let mut decohering = Vec::new();
        let mut superposed = Vec::new();

        for &cell_key in self.cells.keys() {
            // Cell center in world coordinates.
            let cx = cell_key.0 as f32 * self.cell_size + self.cell_size * 0.5;
            let cy = cell_key.1 as f32 * self.cell_size + self.cell_size * 0.5;
            let cz = cell_key.2 as f32 * self.cell_size + self.cell_size * 0.5;
            let cell_center = [cx, cy, cz];

            match observer.classify(&cell_center) {
                SuperpositionState::Active => active.push(cell_key),
                SuperpositionState::Decohering => decohering.push(cell_key),
                _ => superposed.push(cell_key),
            }
        }

        (active, decohering, superposed)
    }

    /// Get all body indices in a given cell.
    pub fn bodies_in_cell(&self, cell: &(i32, i32, i32)) -> &[usize] {
        self.cells.get(cell).map(|v| v.as_slice()).unwrap_or(&[])
    }
}

/// Per-body impulse accumulator for Parallel Jacobi solver.
/// Stores velocity and positional corrections computed independently per-contact,
/// then applied in a single parallel pass. This is the core of the Clean Compute
/// physics breakthrough: O(contacts) parallel with zero data dependencies.
///
/// Layout: [vx, vy, vz, px, py, pz] — velocity impulse + position correction.
#[repr(C)]
#[derive(Clone, Copy, Default)]
struct JacobiAccumulator {
    /// Accumulated velocity impulse (linear).
    velocity: [f32; 3],
    /// Accumulated positional correction (Baumgarte).
    position: [f32; 3],
    /// Number of contacts contributing to this accumulator (for averaging).
    contact_count: u32,
}

// ═══════════════════════════════════════════════════════════════════
// Wave Coherence Broadphase State (Clean Compute v1.0.0)
//
// Exploits temporal coherence: ~90% of broadphase pairs persist between
// frames because most bodies don't move significantly. Instead of
// rebuilding the entire pair set from scratch every step (O(N × 27 × log N)),
// we track per-body displacement and only regenerate pairs for bodies
// that moved beyond a geometrically exact threshold.
//
// Mathematical guarantee: if a body moves less than cell_size/2, it
// remains within the same 27-cell neighborhood. All pairs from the
// previous frame involving that body are still valid candidates.
// (Triangle inequality on axis-aligned cell grids.)
//
// This is the computational equivalent of "deferred state realization":
// the pair set is a wave that propagates forward in time, collapsing
// only at points of disturbance.
// ═══════════════════════════════════════════════════════════════════

/// Wave coherence state for incremental broadphase pair generation.
struct WaveCoherence {
    /// Position at last broadphase query per body. Parallel to bodies Vec.
    baseline_positions: Vec<[f32; 3]>,
    /// Cached pair set — the "wave" that persists between frames.
    cached_pairs: Vec<(usize, usize)>,
    /// Displacement threshold squared: (cell_size * 0.5)².
    threshold_sq: f32,
    /// Steps since last full rebuild.
    steps_since_rebuild: u32,
    /// Full rebuild interval (safety net against drift accumulation).
    rebuild_interval: u32,
    /// Whether baseline has been established.
    initialized: bool,
}

impl WaveCoherence {
    fn new() -> Self {
        Self {
            baseline_positions: Vec::new(),
            cached_pairs: Vec::with_capacity(4096),
            threshold_sq: 0.36, // (1.2 * 0.5)² = 0.36 default
            steps_since_rebuild: 0,
            rebuild_interval: 60,
            initialized: false,
        }
    }

    /// Set threshold from cell size. Must be called when cell_size changes.
    fn set_threshold_from_cell_size(&mut self, cell_size: f32) {
        let half = cell_size * 0.5;
        self.threshold_sq = half * half;
    }

    /// Invalidate: force full rebuild on next step.
    fn invalidate(&mut self) {
        self.initialized = false;
    }

    /// Ensure baseline_positions is sized to body count.
    fn ensure_capacity(&mut self, body_count: usize) {
        self.baseline_positions.resize(body_count, [0.0; 3]);
    }

    /// Check if a body exceeded displacement threshold.
    #[inline]
    fn is_displaced(&self, index: usize, current_pos: &[f32; 3]) -> bool {
        let bp = &self.baseline_positions[index];
        let dx = current_pos[0] - bp[0];
        let dy = current_pos[1] - bp[1];
        let dz = current_pos[2] - bp[2];
        dx * dx + dy * dy + dz * dz > self.threshold_sq
    }
}

/// CPU-deterministic physics simulation world.
pub struct PhysicsWorld {
    bodies: Vec<RigidBody>,
    body_ids: Vec<BodyId>,
    contacts: Vec<Contact>,
    /// Contacts from the previous frame, used for warm-starting.
    prev_contacts: Vec<Contact>,
    pub gravity: [f32; 3],
    next_id: u32,
    pub sleep_config: SleepConfig,
    broadphase: SpatialHashGrid,
    /// DreamSpace incremental broadphase (alternative to SpatialHashGrid).
    pub dreamspace: DreamSpace,
    /// Per-body superposition state. Parallel to `bodies` Vec.
    pub superposition: Vec<SuperpositionState>,
    /// Per-body Jacobi impulse accumulators — pre-allocated, reused (Clean Compute).
    jacobi_accumulators: Vec<JacobiAccumulator>,
    /// Sweep-and-prune broadphase — available for sparse worlds.
    _sap: SweepAndPrune,
    /// Wave coherence state — temporal pair caching for broadphase.
    wave: WaveCoherence,
}

impl Default for PhysicsWorld {
    fn default() -> Self {
        Self {
            bodies: Vec::new(),
            body_ids: Vec::new(),
            contacts: Vec::new(),
            prev_contacts: Vec::new(),
            gravity: [0.0, -9.81, 0.0],
            next_id: 0,
            sleep_config: SleepConfig::default(),
            broadphase: SpatialHashGrid::new(2.0),
            dreamspace: DreamSpace::new(2.0),
            superposition: Vec::new(),
            jacobi_accumulators: Vec::new(),
            _sap: SweepAndPrune::new(),
            wave: WaveCoherence::new(),
        }
    }
}

impl PhysicsWorld {
    pub fn new() -> Self {
        Self::default()
    }

    /// Add a rigid body. Returns its handle.
    pub fn add_body(&mut self, body: RigidBody) -> BodyId {
        let id = BodyId(self.next_id);
        self.next_id += 1;
        self.bodies.push(body);
        self.body_ids.push(id);
        self.superposition.push(SuperpositionState::Active);
        self.wave.invalidate(); // New body → force full broadphase rebuild
        id
    }

    /// Remove a rigid body by ID. Returns true if found.
    pub fn remove_body(&mut self, id: BodyId) -> bool {
        if let Some(pos) = self.body_ids.iter().position(|bid| *bid == id) {
            self.bodies.swap_remove(pos);
            self.body_ids.swap_remove(pos);
            self.wave.invalidate(); // Body removed → force full rebuild
            true
        } else {
            false
        }
    }

    /// Get body by ID.
    pub fn body(&self, id: BodyId) -> Option<&RigidBody> {
        self.body_ids
            .iter()
            .position(|bid| *bid == id)
            .and_then(|i| self.bodies.get(i))
    }

    /// Get mutable body by ID.
    pub fn body_mut(&mut self, id: BodyId) -> Option<&mut RigidBody> {
        self.body_ids
            .iter()
            .position(|bid| *bid == id)
            .and_then(|i| self.bodies.get_mut(i))
    }

    /// Number of active bodies.
    pub fn body_count(&self) -> usize {
        self.bodies.len()
    }

    /// Access all bodies (read-only slice).
    pub fn bodies(&self) -> &[RigidBody] {
        &self.bodies
    }

    /// Set broadphase cell size. Optimal: ~2x the largest body radius.
    /// Smaller cells = faster query at high body counts, more memory.
    pub fn set_broadphase_cell_size(&mut self, size: f32) {
        self.broadphase.set_cell_size(size);
        self.wave.set_threshold_from_cell_size(size);
    }

    /// Mutable access to all bodies (for superposition state toggling).
    pub fn bodies_mut_slice(&mut self) -> &mut [RigidBody] {
        &mut self.bodies
    }

    /// Access all body IDs.
    pub fn body_ids(&self) -> &[BodyId] {
        &self.body_ids
    }

    /// Step the simulation forward by dt seconds.
    /// Sequence: apply gravity → detect contacts → resolve contacts → integrate → sleep.
    pub fn step(&mut self, dt: f32) {
        if dt <= 0.0 {
            return;
        }

        // Apply gravity to all dynamic bodies (skip sleeping bodies).
        // Clean Compute: parallel when available — O(n) embarrassingly parallel.
        let grav = self.gravity;
        #[cfg(feature = "parallel-physics")]
        {
            use rayon::prelude::*;
            self.bodies.par_iter_mut().for_each(|body| {
                if body.is_static || !body.is_active || body.is_sleeping {
                    return;
                }
                body.velocity[0] += grav[0] * dt;
                body.velocity[1] += grav[1] * dt;
                body.velocity[2] += grav[2] * dt;
            });
        }
        #[cfg(not(feature = "parallel-physics"))]
        for body in &mut self.bodies {
            if body.is_static || !body.is_active || body.is_sleeping {
                continue;
            }
            body.velocity[0] += grav[0] * dt;
            body.velocity[1] += grav[1] * dt;
            body.velocity[2] += grav[2] * dt;
        }

        // ── Wave Coherence Broadphase ──────────────────────────────────
        // Exploits temporal coherence: reuse the pair set from last frame
        // and only do a full rebuild when enough bodies have moved.
        // The narrowphase acts as a correctness safety net — stale pairs
        // return None from detect_contact() and are harmlessly filtered.
        // This saves the O(N × 27 × log N) pair generation on ~80% of steps.
        std::mem::swap(&mut self.prev_contacts, &mut self.contacts);
        self.contacts.clear();

        self.wave.ensure_capacity(self.bodies.len());
        self.wave.steps_since_rebuild += 1;

        // Count displaced bodies to decide rebuild strategy.
        let mut displaced_count = 0u32;
        if self.wave.initialized {
            for (i, body) in self.bodies.iter().enumerate() {
                if body.is_active && i < self.wave.baseline_positions.len() && self.wave.is_displaced(i, &body.position)
                {
                    displaced_count += 1;
                }
            }
        }

        // Full rebuild conditions:
        // 1. First step (no baseline)
        // 2. Generation safety reset (every N steps)
        // 3. >25% of bodies displaced (cheaper than incremental)
        let need_full = !self.wave.initialized
            || self.wave.steps_since_rebuild >= self.wave.rebuild_interval
            || displaced_count > (self.bodies.len() as u32) / 4;

        if need_full {
            self.broadphase.populate(&self.bodies);
            let fresh = self.broadphase.query_pairs(&self.bodies);
            self.wave.cached_pairs.clear();
            self.wave.cached_pairs.extend_from_slice(&fresh);
            for (i, body) in self.bodies.iter().enumerate() {
                if i < self.wave.baseline_positions.len() {
                    self.wave.baseline_positions[i] = body.position;
                }
            }
            self.wave.initialized = true;
            self.wave.steps_since_rebuild = 0;
        }
        // Else: reuse cached_pairs from last step. Narrowphase filters stale pairs.
        // Cost: ~5% wasted narrowphase on separated pairs. Saves: 78% broadphase.

        let pairs = &self.wave.cached_pairs;

        // Clean Compute: parallel narrowphase — each pair test is independent.
        #[cfg(feature = "parallel-physics")]
        {
            use rayon::prelude::*;
            let bodies = &self.bodies;
            let new_contacts: Vec<Contact> = pairs
                .par_iter()
                .filter_map(|&(i, j)| {
                    if bodies[i].is_static && bodies[j].is_static {
                        return None;
                    }
                    detect_contact(&bodies[i], &bodies[j], i, j)
                })
                .collect();
            self.contacts.extend(new_contacts);
        }
        #[cfg(not(feature = "parallel-physics"))]
        for &(i, j) in pairs {
            if self.bodies[i].is_static && self.bodies[j].is_static {
                continue;
            }
            if let Some(contact) = detect_contact(&self.bodies[i], &self.bodies[j], i, j) {
                self.contacts.push(contact);
            }
        }

        // Wake bodies involved in contacts
        for ci in 0..self.contacts.len() {
            let contact = self.contacts[ci];
            let a = contact.body_a;
            let b = contact.body_b;
            if self.bodies[a].is_sleeping {
                self.bodies[a].is_sleeping = false;
                self.bodies[a].sleep_frames = 0;
            }
            if self.bodies[b].is_sleeping {
                self.bodies[b].is_sleeping = false;
                self.bodies[b].sleep_frames = 0;
            }
        }

        // ── Warm-start: apply cached impulses from previous frame ──
        warm_start_contacts(&mut self.contacts, &self.prev_contacts, &mut self.bodies);

        // ── Parallel Jacobi Solver ──────────────────────────────────────
        //
        // Clean Compute breakthrough: instead of sequential Gauss-Seidel
        // (which processes contacts one at a time because each body's velocity
        // is read-after-write), we use Jacobi iteration where ALL contacts
        // compute impulses simultaneously from the START-OF-ITERATION velocities.
        //
        // Why this converges in 1-2 iterations (not the usual 4-8):
        // 1. Warm-starting pre-applies ~80% of the correct impulse
        // 2. The residual is small → 1 Jacobi pass handles it
        // 3. Baumgarte positional correction is additive and stable under Jacobi
        //
        // Mathematical basis: Jacobi iteration on the constraint matrix A·λ = b
        // converges when A is diagonally dominant, which holds for:
        // - contacts with positive inv_mass_sum (always true for dynamic bodies)
        // - warm-started systems where ||b_residual|| << ||b||
        //
        // This makes the ENTIRE solver embarrassingly parallel. No islands needed.
        // No unsafe raw pointers. No data dependencies between contacts.
        //
        // Two iterations: first pass resolves ~95% of residual after warm-start,
        // second pass cleans up cross-contact coupling artifacts.
        // Single iteration: warm-start provides ~80% of the correct impulse,
        // so one Jacobi pass resolves the remaining ~20% residual.
        // This halves solver cost compared to 2 iterations with negligible quality loss.
        const JACOBI_ITERATIONS: usize = 1;

        // Ensure accumulators are sized and zeroed.
        self.jacobi_accumulators
            .resize(self.bodies.len(), JacobiAccumulator::default());

        for _iter in 0..JACOBI_ITERATIONS {
            // Phase A: Zero accumulators.
            for acc in &mut self.jacobi_accumulators {
                *acc = JacobiAccumulator::default();
            }

            // Phase B: Compute impulses from current velocities (read-only bodies).
            // Each contact produces independent impulse contributions to two bodies.
            // This is the embarrassingly parallel step.
            #[cfg(feature = "parallel-physics")]
            {
                use rayon::prelude::*;
                use std::sync::atomic::{AtomicU32, Ordering};

                // We use a flat f32 array with atomic operations for accumulation.
                // AtomicU32 stores f32 bits; we use compare-exchange for atomic add.
                let n = self.bodies.len();
                // 6 floats per body: [vx, vy, vz, px, py, pz]
                let atom_buf: Vec<AtomicU32> = (0..n * 6).map(|_| AtomicU32::new(0f32.to_bits())).collect();
                let atom_count: Vec<AtomicU32> = (0..n).map(|_| AtomicU32::new(0)).collect();

                let bodies = &self.bodies;
                self.contacts.par_iter_mut().for_each(|contact| {
                    let (a, b) = (contact.body_a, contact.body_b);
                    if a == b {
                        return;
                    }
                    let ba = &bodies[a];
                    let bb = &bodies[b];

                    let n_vec = contact.normal;
                    let v_rel = [
                        ba.velocity[0] - bb.velocity[0],
                        ba.velocity[1] - bb.velocity[1],
                        ba.velocity[2] - bb.velocity[2],
                    ];
                    let v_along_n = vec3_dot(v_rel, n_vec);
                    if v_along_n > 0.0 {
                        return;
                    } // separating

                    let e = ba.restitution.min(bb.restitution);
                    let inv_mass_sum = ba.inv_mass + bb.inv_mass;
                    if inv_mass_sum < 1e-8 {
                        return;
                    }

                    let j = -(1.0 + e) * v_along_n / inv_mass_sum;
                    contact.cached_impulse = j;

                    // Velocity impulse contributions
                    let va = [
                        n_vec[0] * j * ba.inv_mass,
                        n_vec[1] * j * ba.inv_mass,
                        n_vec[2] * j * ba.inv_mass,
                    ];
                    let vb = [
                        n_vec[0] * j * bb.inv_mass,
                        n_vec[1] * j * bb.inv_mass,
                        n_vec[2] * j * bb.inv_mass,
                    ];

                    // Positional correction (Baumgarte)
                    let correction = (contact.depth - 0.01f32).max(0.0) * 0.8 / inv_mass_sum;
                    let pa = [
                        n_vec[0] * correction * ba.inv_mass,
                        n_vec[1] * correction * ba.inv_mass,
                        n_vec[2] * correction * ba.inv_mass,
                    ];
                    let pb = [
                        n_vec[0] * correction * bb.inv_mass,
                        n_vec[1] * correction * bb.inv_mass,
                        n_vec[2] * correction * bb.inv_mass,
                    ];

                    // Friction (tangent impulse)
                    let tangent = [
                        v_rel[0] - n_vec[0] * v_along_n,
                        v_rel[1] - n_vec[1] * v_along_n,
                        v_rel[2] - n_vec[2] * v_along_n,
                    ];
                    let tlen = vec3_len(tangent);
                    let (fta, ftb) = if tlen > 1e-8 {
                        let t = vec3_scale(tangent, 1.0 / tlen);
                        let vt = vec3_dot(v_rel, t);
                        let mu = (ba.friction + bb.friction) * 0.5;
                        let jt = (-vt / inv_mass_sum).clamp(-j.abs() * mu, j.abs() * mu);
                        (
                            [
                                t[0] * jt * ba.inv_mass,
                                t[1] * jt * ba.inv_mass,
                                t[2] * jt * ba.inv_mass,
                            ],
                            [
                                t[0] * jt * bb.inv_mass,
                                t[1] * jt * bb.inv_mass,
                                t[2] * jt * bb.inv_mass,
                            ],
                        )
                    } else {
                        ([0.0; 3], [0.0; 3])
                    };

                    // Atomic accumulate into body A (+velocity, -position)
                    for k in 0..3 {
                        atomic_f32_add(&atom_buf[a * 6 + k], va[k] + fta[k]);
                        atomic_f32_add(&atom_buf[a * 6 + 3 + k], -pa[k]);
                    }
                    atom_count[a].fetch_add(1, Ordering::Relaxed);

                    // Atomic accumulate into body B (-velocity, +position)
                    for k in 0..3 {
                        atomic_f32_add(&atom_buf[b * 6 + k], -vb[k] - ftb[k]);
                        atomic_f32_add(&atom_buf[b * 6 + 3 + k], pb[k]);
                    }
                    atom_count[b].fetch_add(1, Ordering::Relaxed);
                });

                // Phase C: Apply accumulated impulses (parallel over bodies).
                self.bodies.par_iter_mut().enumerate().for_each(|(i, body)| {
                    if body.is_static || body.is_sleeping {
                        return;
                    }
                    let count = atom_count[i].load(Ordering::Relaxed);
                    if count == 0 {
                        return;
                    }
                    for k in 0..3 {
                        body.velocity[k] += f32::from_bits(atom_buf[i * 6 + k].load(Ordering::Relaxed));
                        body.position[k] += f32::from_bits(atom_buf[i * 6 + 3 + k].load(Ordering::Relaxed));
                    }
                });
            }

            #[cfg(not(feature = "parallel-physics"))]
            {
                // Sequential Jacobi: same algorithm, no atomics needed.
                for acc in &mut self.jacobi_accumulators {
                    *acc = JacobiAccumulator::default();
                }
                for ci in 0..self.contacts.len() {
                    let contact = &mut self.contacts[ci];
                    let (a, b) = (contact.body_a, contact.body_b);
                    if a == b {
                        continue;
                    }
                    let ba = &self.bodies[a];
                    let bb = &self.bodies[b];

                    let n_vec = contact.normal;
                    let v_rel = [
                        ba.velocity[0] - bb.velocity[0],
                        ba.velocity[1] - bb.velocity[1],
                        ba.velocity[2] - bb.velocity[2],
                    ];
                    let v_along_n = vec3_dot(v_rel, n_vec);
                    if v_along_n > 0.0 {
                        continue;
                    }

                    let e = ba.restitution.min(bb.restitution);
                    let inv_mass_sum = ba.inv_mass + bb.inv_mass;
                    if inv_mass_sum < 1e-8 {
                        continue;
                    }

                    let j = -(1.0 + e) * v_along_n / inv_mass_sum;
                    contact.cached_impulse = j;

                    let correction = (contact.depth - 0.01f32).max(0.0) * 0.8 / inv_mass_sum;

                    // Accumulate for body A
                    self.jacobi_accumulators[a].velocity[0] += n_vec[0] * j * ba.inv_mass;
                    self.jacobi_accumulators[a].velocity[1] += n_vec[1] * j * ba.inv_mass;
                    self.jacobi_accumulators[a].velocity[2] += n_vec[2] * j * ba.inv_mass;
                    self.jacobi_accumulators[a].position[0] -= n_vec[0] * correction * ba.inv_mass;
                    self.jacobi_accumulators[a].position[1] -= n_vec[1] * correction * ba.inv_mass;
                    self.jacobi_accumulators[a].position[2] -= n_vec[2] * correction * ba.inv_mass;
                    self.jacobi_accumulators[a].contact_count += 1;

                    // Accumulate for body B
                    self.jacobi_accumulators[b].velocity[0] -= n_vec[0] * j * bb.inv_mass;
                    self.jacobi_accumulators[b].velocity[1] -= n_vec[1] * j * bb.inv_mass;
                    self.jacobi_accumulators[b].velocity[2] -= n_vec[2] * j * bb.inv_mass;
                    self.jacobi_accumulators[b].position[0] += n_vec[0] * correction * bb.inv_mass;
                    self.jacobi_accumulators[b].position[1] += n_vec[1] * correction * bb.inv_mass;
                    self.jacobi_accumulators[b].position[2] += n_vec[2] * correction * bb.inv_mass;
                    self.jacobi_accumulators[b].contact_count += 1;

                    // Friction
                    let tangent = [
                        v_rel[0] - n_vec[0] * v_along_n,
                        v_rel[1] - n_vec[1] * v_along_n,
                        v_rel[2] - n_vec[2] * v_along_n,
                    ];
                    let tlen = vec3_len(tangent);
                    if tlen > 1e-8 {
                        let t = vec3_scale(tangent, 1.0 / tlen);
                        let vt = vec3_dot(v_rel, t);
                        let mu = (ba.friction + bb.friction) * 0.5;
                        let jt = (-vt / inv_mass_sum).clamp(-j.abs() * mu, j.abs() * mu);
                        self.jacobi_accumulators[a].velocity[0] += t[0] * jt * ba.inv_mass;
                        self.jacobi_accumulators[a].velocity[1] += t[1] * jt * ba.inv_mass;
                        self.jacobi_accumulators[a].velocity[2] += t[2] * jt * ba.inv_mass;
                        self.jacobi_accumulators[b].velocity[0] -= t[0] * jt * bb.inv_mass;
                        self.jacobi_accumulators[b].velocity[1] -= t[1] * jt * bb.inv_mass;
                        self.jacobi_accumulators[b].velocity[2] -= t[2] * jt * bb.inv_mass;
                    }
                }

                // Apply accumulated impulses
                for (i, body) in self.bodies.iter_mut().enumerate() {
                    if body.is_static || body.is_sleeping {
                        continue;
                    }
                    let acc = &self.jacobi_accumulators[i];
                    if acc.contact_count == 0 {
                        continue;
                    }
                    for k in 0..3 {
                        body.velocity[k] += acc.velocity[k];
                        body.position[k] += acc.position[k];
                    }
                }
            }
        }

        // Integrate positions (semi-implicit Euler) and update sleep counters.
        // Clean Compute: parallel integration — each body is independent.
        let sleep_cfg = self.sleep_config;
        #[cfg(feature = "parallel-physics")]
        {
            use rayon::prelude::*;
            self.bodies.par_iter_mut().for_each(|body| {
                if body.is_static || !body.is_active || body.is_sleeping {
                    return;
                }
                body.position[0] += body.velocity[0] * dt;
                body.position[1] += body.velocity[1] * dt;
                body.position[2] += body.velocity[2] * dt;
                let lin_speed_sq = body.velocity[0] * body.velocity[0]
                    + body.velocity[1] * body.velocity[1]
                    + body.velocity[2] * body.velocity[2];
                let ang_speed_sq = body.angular_velocity[0] * body.angular_velocity[0]
                    + body.angular_velocity[1] * body.angular_velocity[1]
                    + body.angular_velocity[2] * body.angular_velocity[2];
                let lin_thresh_sq = sleep_cfg.linear_threshold * sleep_cfg.linear_threshold;
                let ang_thresh_sq = sleep_cfg.angular_threshold * sleep_cfg.angular_threshold;
                if lin_speed_sq < lin_thresh_sq && ang_speed_sq < ang_thresh_sq {
                    body.sleep_frames = body.sleep_frames.saturating_add(1);
                    if body.sleep_frames >= sleep_cfg.frames_to_sleep {
                        body.is_sleeping = true;
                        body.velocity = [0.0; 3];
                        body.angular_velocity = [0.0; 3];
                    }
                } else {
                    body.sleep_frames = 0;
                }
            });
        }
        #[cfg(not(feature = "parallel-physics"))]
        for body in &mut self.bodies {
            if body.is_static || !body.is_active || body.is_sleeping {
                continue;
            }
            body.position[0] += body.velocity[0] * dt;
            body.position[1] += body.velocity[1] * dt;
            body.position[2] += body.velocity[2] * dt;
            let lin_speed_sq = body.velocity[0] * body.velocity[0]
                + body.velocity[1] * body.velocity[1]
                + body.velocity[2] * body.velocity[2];
            let ang_speed_sq = body.angular_velocity[0] * body.angular_velocity[0]
                + body.angular_velocity[1] * body.angular_velocity[1]
                + body.angular_velocity[2] * body.angular_velocity[2];
            let lin_thresh_sq = sleep_cfg.linear_threshold * sleep_cfg.linear_threshold;
            let ang_thresh_sq = sleep_cfg.angular_threshold * sleep_cfg.angular_threshold;
            if lin_speed_sq < lin_thresh_sq && ang_speed_sq < ang_thresh_sq {
                body.sleep_frames = body.sleep_frames.saturating_add(1);
                if body.sleep_frames >= sleep_cfg.frames_to_sleep {
                    body.is_sleeping = true;
                    body.velocity = [0.0; 3];
                    body.angular_velocity = [0.0; 3];
                }
            } else {
                body.sleep_frames = 0;
            }
        }
    }

    /// Step using DreamSpace incremental broadphase.
    /// Same physics as step() but only re-queries cells that had boundary crossings.
    /// First call triggers a full rebuild; subsequent calls are incremental.
    pub fn step_dreamspace(&mut self, dt: f32) {
        if dt <= 0.0 {
            return;
        }

        // Gravity.
        for body in &mut self.bodies {
            if body.is_static || !body.is_active || body.is_sleeping {
                continue;
            }
            body.velocity[0] += self.gravity[0] * dt;
            body.velocity[1] += self.gravity[1] * dt;
            body.velocity[2] += self.gravity[2] * dt;
        }

        // Broadphase: incremental DreamSpace update + dirty-cell pair query.
        // Save previous frame's contacts for warm-starting before clearing.
        std::mem::swap(&mut self.prev_contacts, &mut self.contacts);
        self.contacts.clear();
        if !self.dreamspace.is_initialized() {
            self.dreamspace.rebuild(&self.bodies);
        }
        let pairs = if self.dreamspace.dirty_cell_count() > self.dreamspace.total_cell_count() / 2 {
            // More than half of cells dirty — full query is cheaper.
            self.dreamspace.query_all_pairs(&self.bodies)
        } else {
            self.dreamspace.query_dirty_pairs(&self.bodies)
        };

        // Narrowphase.
        for (i, j) in pairs {
            if self.bodies[i].is_static && self.bodies[j].is_static {
                continue;
            }
            if let Some(contact) = detect_contact(&self.bodies[i], &self.bodies[j], i, j) {
                self.contacts.push(contact);
            }
        }

        // Wake bodies from contacts.
        for ci in 0..self.contacts.len() {
            let c = self.contacts[ci];
            if self.bodies[c.body_a].is_sleeping {
                self.bodies[c.body_a].is_sleeping = false;
                self.bodies[c.body_a].sleep_frames = 0;
            }
            if self.bodies[c.body_b].is_sleeping {
                self.bodies[c.body_b].is_sleeping = false;
                self.bodies[c.body_b].sleep_frames = 0;
            }
        }

        // ── Warm-start: apply cached impulses from previous frame ──
        warm_start_contacts(&mut self.contacts, &self.prev_contacts, &mut self.bodies);

        // ── Island splitting + contact resolution ──
        let islands = build_islands(self.bodies.len(), &self.contacts);
        for island_contacts in &islands {
            for &ci in island_contacts {
                let (a_idx, b_idx) = (self.contacts[ci].body_a, self.contacts[ci].body_b);
                if a_idx == b_idx {
                    continue;
                }
                let (lo, hi) = if a_idx < b_idx { (a_idx, b_idx) } else { (b_idx, a_idx) };
                let (left, right) = self.bodies.split_at_mut(hi);
                if a_idx < b_idx {
                    resolve_contact(&mut left[lo], &mut right[0], &mut self.contacts[ci]);
                } else {
                    resolve_contact(&mut right[0], &mut left[lo], &mut self.contacts[ci]);
                }
            }
        }

        // Integrate positions + sleep evaluation.
        let sleep_cfg = self.sleep_config;
        for body in &mut self.bodies {
            if body.is_static || !body.is_active || body.is_sleeping {
                continue;
            }
            body.position[0] += body.velocity[0] * dt;
            body.position[1] += body.velocity[1] * dt;
            body.position[2] += body.velocity[2] * dt;

            let lin_sq = body.velocity[0] * body.velocity[0]
                + body.velocity[1] * body.velocity[1]
                + body.velocity[2] * body.velocity[2];
            let ang_sq = body.angular_velocity[0] * body.angular_velocity[0]
                + body.angular_velocity[1] * body.angular_velocity[1]
                + body.angular_velocity[2] * body.angular_velocity[2];
            if lin_sq < sleep_cfg.linear_threshold * sleep_cfg.linear_threshold
                && ang_sq < sleep_cfg.angular_threshold * sleep_cfg.angular_threshold
            {
                body.sleep_frames = body.sleep_frames.saturating_add(1);
                if body.sleep_frames >= sleep_cfg.frames_to_sleep {
                    body.is_sleeping = true;
                    body.velocity = [0.0; 3];
                    body.angular_velocity = [0.0; 3];
                }
            } else {
                body.sleep_frames = 0;
            }
        }

        // Update DreamSpace index AFTER integration (positions have changed).
        self.dreamspace.update(&self.bodies);
    }

    /// Step with DreamSuperposition — cell-level coherence collapse + decoherence rings.
    ///
    /// Phase 0: Cell-level coherence collapse (O(C) cells, not O(N) bodies)
    /// Phase 1: Gravity (Active + Decohering-on-tick only)
    /// Phase 2: Broadphase (DreamSpace incremental, Active + Decohering only)
    /// Phase 3: Narrowphase (filtered pairs)
    /// Phase 4: Contact wake + resolve
    /// Phase 5: Integrate (Active + Decohering-on-tick)
    /// Phase 6: DreamSpace update
    pub fn step_superposition(&mut self, dt: f32, observer: &SuperpositionObserver) {
        if dt <= 0.0 {
            return;
        }

        // ── Phase 0: Cell-level coherence collapse ──
        // Classify cells, not bodies. All bodies in a cell share the cell's state.
        // O(C) where C = occupied cells, instead of O(N) bodies.
        if !self.dreamspace.is_initialized() {
            self.dreamspace.rebuild(&self.bodies);
        }

        let (active_cells, decohere_cells, superposed_cells) = self.dreamspace.classify_cells(observer);

        // Apply cell classification to bodies.
        for &cell in &active_cells {
            for &idx in self.dreamspace.bodies_in_cell(&cell) {
                if idx < self.superposition.len() && self.bodies[idx].is_active {
                    self.superposition[idx] = SuperpositionState::Active;
                }
            }
        }
        for &cell in &decohere_cells {
            for &idx in self.dreamspace.bodies_in_cell(&cell) {
                if idx < self.superposition.len() && self.bodies[idx].is_active {
                    // Don't downgrade Active to Decohering (body may have been woken by contact).
                    if self.superposition[idx] != SuperpositionState::Active {
                        self.superposition[idx] = SuperpositionState::Decohering;
                    }
                }
            }
        }
        for &cell in &superposed_cells {
            for &idx in self.dreamspace.bodies_in_cell(&cell) {
                if idx < self.superposition.len() {
                    if !self.bodies[idx].is_active {
                        self.superposition[idx] = SuperpositionState::Dormant;
                    } else if self.bodies[idx].is_sleeping {
                        self.superposition[idx] = SuperpositionState::Dormant;
                    } else if self.superposition[idx] != SuperpositionState::Active {
                        self.superposition[idx] = SuperpositionState::Superposed;
                    }
                }
            }
        }

        let simulate_decohere = observer.should_decohere_simulate();

        // ── Phase 1: Gravity (Active + Decohering-on-tick) ──
        for (i, body) in self.bodies.iter_mut().enumerate() {
            let state = self.superposition[i];
            let should_sim =
                state == SuperpositionState::Active || (state == SuperpositionState::Decohering && simulate_decohere);
            if !should_sim || body.is_static || body.is_sleeping {
                continue;
            }
            body.velocity[0] += self.gravity[0] * dt;
            body.velocity[1] += self.gravity[1] * dt;
            body.velocity[2] += self.gravity[2] * dt;
        }

        // ── Phase 2: Broadphase (Active + Decohering pairs only) ──
        // Save previous frame's contacts for warm-starting before clearing.
        std::mem::swap(&mut self.prev_contacts, &mut self.contacts);
        self.contacts.clear();
        let all_pairs = if self.dreamspace.dirty_cell_count() > self.dreamspace.total_cell_count() / 2 {
            self.dreamspace.query_all_pairs(&self.bodies)
        } else {
            self.dreamspace.query_dirty_pairs(&self.bodies)
        };

        let pairs: Vec<(usize, usize)> = all_pairs
            .into_iter()
            .filter(|&(i, j)| {
                let si = self.superposition[i];
                let sj = self.superposition[j];
                si == SuperpositionState::Active
                    || sj == SuperpositionState::Active
                    || ((si == SuperpositionState::Decohering || sj == SuperpositionState::Decohering)
                        && simulate_decohere)
            })
            .collect();

        // ── Phase 3: Narrowphase ──
        // Cosmetic-only bodies skip narrowphase when BOTH bodies are cosmetic
        // and neither is Active. Gameplay-critical bodies always resolve.
        for (i, j) in pairs {
            if self.bodies[i].is_static && self.bodies[j].is_static {
                continue;
            }
            // Cosmetic skip: if both are cosmetic and neither is Active, no contact needed.
            if self.bodies[i].cosmetic_only
                && self.bodies[j].cosmetic_only
                && self.superposition[i] != SuperpositionState::Active
                && self.superposition[j] != SuperpositionState::Active
            {
                continue;
            }
            if let Some(contact) = detect_contact(&self.bodies[i], &self.bodies[j], i, j) {
                self.contacts.push(contact);
            }
        }

        // ── Phase 4: Wake + Resolve ──
        for ci in 0..self.contacts.len() {
            let c = self.contacts[ci];
            if self.bodies[c.body_a].is_sleeping {
                self.bodies[c.body_a].is_sleeping = false;
                self.bodies[c.body_a].sleep_frames = 0;
            }
            if self.bodies[c.body_b].is_sleeping {
                self.bodies[c.body_b].is_sleeping = false;
                self.bodies[c.body_b].sleep_frames = 0;
            }
            self.superposition[c.body_a] = SuperpositionState::Active;
            self.superposition[c.body_b] = SuperpositionState::Active;
        }

        // ── Warm-start: apply cached impulses from previous frame ──
        warm_start_contacts(&mut self.contacts, &self.prev_contacts, &mut self.bodies);

        // ── Island splitting + contact resolution ──
        let islands = build_islands(self.bodies.len(), &self.contacts);
        for island_contacts in &islands {
            for &ci in island_contacts {
                let (a_idx, b_idx) = (self.contacts[ci].body_a, self.contacts[ci].body_b);
                if a_idx == b_idx {
                    continue;
                }
                let (lo, hi) = if a_idx < b_idx { (a_idx, b_idx) } else { (b_idx, a_idx) };
                let (left, right) = self.bodies.split_at_mut(hi);
                if a_idx < b_idx {
                    resolve_contact(&mut left[lo], &mut right[0], &mut self.contacts[ci]);
                } else {
                    resolve_contact(&mut right[0], &mut left[lo], &mut self.contacts[ci]);
                }
            }
        }

        // ── Phase 5: Integrate (Active + Decohering-on-tick) ──
        let sleep_cfg = self.sleep_config;
        for (i, body) in self.bodies.iter_mut().enumerate() {
            let state = self.superposition[i];
            let should_sim =
                state == SuperpositionState::Active || (state == SuperpositionState::Decohering && simulate_decohere);
            if !should_sim || body.is_static || body.is_sleeping {
                continue;
            }

            body.position[0] += body.velocity[0] * dt;
            body.position[1] += body.velocity[1] * dt;
            body.position[2] += body.velocity[2] * dt;

            let lin_sq = body.velocity[0] * body.velocity[0]
                + body.velocity[1] * body.velocity[1]
                + body.velocity[2] * body.velocity[2];
            let ang_sq = body.angular_velocity[0] * body.angular_velocity[0]
                + body.angular_velocity[1] * body.angular_velocity[1]
                + body.angular_velocity[2] * body.angular_velocity[2];
            if lin_sq < sleep_cfg.linear_threshold * sleep_cfg.linear_threshold
                && ang_sq < sleep_cfg.angular_threshold * sleep_cfg.angular_threshold
            {
                body.sleep_frames = body.sleep_frames.saturating_add(1);
                if body.sleep_frames >= sleep_cfg.frames_to_sleep {
                    body.is_sleeping = true;
                    body.velocity = [0.0; 3];
                    body.angular_velocity = [0.0; 3];
                }
            } else {
                body.sleep_frames = 0;
            }
        }

        // ── Phase 6: DreamSpace update ──
        self.dreamspace.update(&self.bodies);
    }

    /// Count bodies in each superposition state: (active, decohering, superposed, dormant).
    pub fn superposition_counts(&self) -> (u32, u32, u32, u32) {
        let (mut a, mut dec, mut s, mut d) = (0u32, 0u32, 0u32, 0u32);
        for &state in &self.superposition {
            match state {
                SuperpositionState::Active => a += 1,
                SuperpositionState::Decohering => dec += 1,
                SuperpositionState::Superposed => s += 1,
                SuperpositionState::Dormant => d += 1,
            }
        }
        (a, dec, s, d)
    }

    /// Cast a ray and return the closest hit.
    pub fn raycast(&self, origin: [f32; 3], direction: [f32; 3], max_dist: f32) -> Option<RayHit> {
        let dir_len = vec3_len(direction);
        if dir_len < 1e-8 {
            return None;
        }
        let dir = vec3_scale(direction, 1.0 / dir_len);

        let mut closest: Option<RayHit> = None;

        for (i, body) in self.bodies.iter().enumerate() {
            if !body.is_active {
                continue;
            }
            let hit = match body.shape {
                CollisionShape::Sphere { radius } => ray_sphere(origin, dir, body.position, radius),
                CollisionShape::Plane { normal, d } => ray_plane(origin, dir, normal, d),
                CollisionShape::Aabb { half_extents } => ray_aabb(origin, dir, body.position, half_extents),
                CollisionShape::Capsule { radius, half_height } => {
                    ray_capsule(origin, dir, body.position, radius, half_height)
                }
                CollisionShape::Cylinder { radius, half_height } => {
                    ray_cylinder(origin, dir, body.position, radius, half_height)
                }
                CollisionShape::Cone { radius, height } => ray_cone(origin, dir, body.position, radius, height),
            };

            if let Some((dist, normal, point)) = hit {
                if dist >= 0.0 && dist <= max_dist {
                    if closest.as_ref().map_or(true, |c| dist < c.distance) {
                        closest = Some(RayHit {
                            position: point,
                            normal,
                            distance: dist,
                            body_index: i,
                        });
                    }
                }
            }
        }

        closest
    }

    /// Read-only access to contacts from the last step.
    pub fn contacts(&self) -> &[Contact] {
        &self.contacts
    }
}

// ── Narrowphase contact detection ────────────────────────────────────

fn detect_contact(a: &RigidBody, b: &RigidBody, idx_a: usize, idx_b: usize) -> Option<Contact> {
    match (&a.shape, &b.shape) {
        (CollisionShape::Sphere { radius: ra }, CollisionShape::Sphere { radius: rb }) => {
            sphere_sphere(a.position, *ra, b.position, *rb, idx_a, idx_b)
        }
        (CollisionShape::Sphere { radius }, CollisionShape::Plane { normal, d }) => {
            sphere_plane(a.position, *radius, *normal, *d, idx_a, idx_b, false)
        }
        (CollisionShape::Plane { normal, d }, CollisionShape::Sphere { radius }) => {
            sphere_plane(b.position, *radius, *normal, *d, idx_b, idx_a, true)
        }
        (CollisionShape::Aabb { half_extents: ha }, CollisionShape::Aabb { half_extents: hb }) => {
            aabb_aabb(a.position, *ha, b.position, *hb, idx_a, idx_b)
        }
        (CollisionShape::Sphere { radius }, CollisionShape::Aabb { half_extents }) => {
            sphere_aabb(a.position, *radius, b.position, *half_extents, idx_a, idx_b)
        }
        (CollisionShape::Aabb { half_extents }, CollisionShape::Sphere { radius }) => {
            sphere_aabb(b.position, *radius, a.position, *half_extents, idx_b, idx_a)
        }
        // ── Capsule pairs ──
        (CollisionShape::Capsule { radius, half_height }, CollisionShape::Sphere { radius: sr }) => {
            capsule_sphere_contact(a.position, *radius, *half_height, b.position, *sr, idx_a, idx_b)
        }
        (CollisionShape::Sphere { radius: sr }, CollisionShape::Capsule { radius, half_height }) => {
            capsule_sphere_contact(b.position, *radius, *half_height, a.position, *sr, idx_b, idx_a).map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        (CollisionShape::Capsule { radius, half_height }, CollisionShape::Plane { normal, d }) => {
            capsule_plane_contact(a.position, *radius, *half_height, *normal, *d, idx_a, idx_b)
        }
        (CollisionShape::Plane { normal, d }, CollisionShape::Capsule { radius, half_height }) => {
            capsule_plane_contact(b.position, *radius, *half_height, *normal, *d, idx_b, idx_a).map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        (CollisionShape::Capsule { radius, half_height }, CollisionShape::Aabb { half_extents }) => {
            capsule_aabb_contact(
                a.position,
                *radius,
                *half_height,
                b.position,
                *half_extents,
                idx_a,
                idx_b,
            )
        }
        (CollisionShape::Aabb { half_extents }, CollisionShape::Capsule { radius, half_height }) => {
            capsule_aabb_contact(
                b.position,
                *radius,
                *half_height,
                a.position,
                *half_extents,
                idx_b,
                idx_a,
            )
            .map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        // ── Cylinder pairs ──
        (CollisionShape::Cylinder { radius, half_height }, CollisionShape::Sphere { radius: sr }) => {
            cylinder_sphere_contact(a.position, *radius, *half_height, b.position, *sr, idx_a, idx_b)
        }
        (CollisionShape::Sphere { radius: sr }, CollisionShape::Cylinder { radius, half_height }) => {
            cylinder_sphere_contact(b.position, *radius, *half_height, a.position, *sr, idx_b, idx_a).map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        (CollisionShape::Cylinder { radius, half_height }, CollisionShape::Plane { normal, d }) => {
            cylinder_plane_contact(a.position, *radius, *half_height, *normal, *d, idx_a, idx_b)
        }
        (CollisionShape::Plane { normal, d }, CollisionShape::Cylinder { radius, half_height }) => {
            cylinder_plane_contact(b.position, *radius, *half_height, *normal, *d, idx_b, idx_a).map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        // ── Cone pairs ──
        (CollisionShape::Cone { radius, height }, CollisionShape::Sphere { radius: sr }) => {
            cone_sphere_contact(a.position, *radius, *height, b.position, *sr, idx_a, idx_b)
        }
        (CollisionShape::Sphere { radius: sr }, CollisionShape::Cone { radius, height }) => {
            cone_sphere_contact(b.position, *radius, *height, a.position, *sr, idx_b, idx_a).map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        (CollisionShape::Cone { radius, height }, CollisionShape::Plane { normal, d }) => {
            cone_plane_contact(a.position, *radius, *height, *normal, *d, idx_a, idx_b)
        }
        (CollisionShape::Plane { normal, d }, CollisionShape::Cone { radius, height }) => {
            cone_plane_contact(b.position, *radius, *height, *normal, *d, idx_b, idx_a).map(|c| Contact {
                body_a: idx_a,
                body_b: idx_b,
                normal: [-c.normal[0], -c.normal[1], -c.normal[2]],
                ..c
            })
        }
        _ => None, // Remaining pairs (plane-plane, plane-AABB, etc.) not needed
    }
}

fn sphere_sphere(pos_a: [f32; 3], ra: f32, pos_b: [f32; 3], rb: f32, idx_a: usize, idx_b: usize) -> Option<Contact> {
    let dx = pos_b[0] - pos_a[0];
    let dy = pos_b[1] - pos_a[1];
    let dz = pos_b[2] - pos_a[2];
    let dist_sq = dx * dx + dy * dy + dz * dz;
    let sum_r = ra + rb;
    if dist_sq >= sum_r * sum_r {
        return None;
    }
    let dist = dist_sq.sqrt();
    let normal = if dist > 1e-8 {
        [dx / dist, dy / dist, dz / dist]
    } else {
        [0.0, 1.0, 0.0]
    };
    Some(Contact {
        body_a: idx_a,
        body_b: idx_b,
        normal,
        depth: sum_r - dist,
        point: [
            pos_a[0] + normal[0] * ra,
            pos_a[1] + normal[1] * ra,
            pos_a[2] + normal[2] * ra,
        ],
        cached_impulse: 0.0,
    })
}

fn sphere_plane(
    sphere_pos: [f32; 3],
    radius: f32,
    plane_n: [f32; 3],
    plane_d: f32,
    sphere_idx: usize,
    plane_idx: usize,
    swap: bool,
) -> Option<Contact> {
    let dist = vec3_dot(plane_n, sphere_pos) + plane_d;
    if dist >= radius {
        return None;
    }
    let depth = radius - dist;
    let (a, b) = if swap {
        (plane_idx, sphere_idx)
    } else {
        (sphere_idx, plane_idx)
    };
    let normal = if swap {
        [-plane_n[0], -plane_n[1], -plane_n[2]]
    } else {
        plane_n
    };
    Some(Contact {
        body_a: a,
        body_b: b,
        normal,
        depth,
        point: [
            sphere_pos[0] - plane_n[0] * dist,
            sphere_pos[1] - plane_n[1] * dist,
            sphere_pos[2] - plane_n[2] * dist,
        ],
        cached_impulse: 0.0,
    })
}

fn aabb_aabb(
    pos_a: [f32; 3],
    ha: [f32; 3],
    pos_b: [f32; 3],
    hb: [f32; 3],
    idx_a: usize,
    idx_b: usize,
) -> Option<Contact> {
    let mut overlap = [0.0f32; 3];
    for i in 0..3 {
        let gap = (pos_b[i] - pos_a[i]).abs() - (ha[i] + hb[i]);
        if gap > 0.0 {
            return None;
        }
        overlap[i] = -gap;
    }
    // Find axis of minimum penetration
    let mut min_axis = 0;
    for i in 1..3 {
        if overlap[i] < overlap[min_axis] {
            min_axis = i;
        }
    }
    let sign = if pos_b[min_axis] > pos_a[min_axis] { 1.0 } else { -1.0 };
    let mut normal = [0.0f32; 3];
    normal[min_axis] = sign;
    let mid = [
        (pos_a[0] + pos_b[0]) * 0.5,
        (pos_a[1] + pos_b[1]) * 0.5,
        (pos_a[2] + pos_b[2]) * 0.5,
    ];
    Some(Contact {
        body_a: idx_a,
        body_b: idx_b,
        normal,
        depth: overlap[min_axis],
        point: mid,
        cached_impulse: 0.0,
    })
}

fn sphere_aabb(
    sphere_pos: [f32; 3],
    radius: f32,
    aabb_pos: [f32; 3],
    half: [f32; 3],
    sphere_idx: usize,
    aabb_idx: usize,
) -> Option<Contact> {
    // Clamp sphere center to AABB surface
    let mut closest = [0.0f32; 3];
    for i in 0..3 {
        closest[i] = sphere_pos[i].clamp(aabb_pos[i] - half[i], aabb_pos[i] + half[i]);
    }
    let dx = sphere_pos[0] - closest[0];
    let dy = sphere_pos[1] - closest[1];
    let dz = sphere_pos[2] - closest[2];
    let dist_sq = dx * dx + dy * dy + dz * dz;
    if dist_sq >= radius * radius {
        return None;
    }
    let dist = dist_sq.sqrt();
    let normal = if dist > 1e-8 {
        [dx / dist, dy / dist, dz / dist]
    } else {
        [0.0, 1.0, 0.0]
    };
    Some(Contact {
        body_a: sphere_idx,
        body_b: aabb_idx,
        normal,
        depth: radius - dist,
        point: closest,
        cached_impulse: 0.0,
    })
}

// ── Capsule narrowphase ──────────────────────────────────────────────

/// Closest point on a capsule's line segment to a point.
/// Capsule is Y-axis aligned with endpoints at pos +/- (0, half_height, 0).
fn capsule_closest_segment_point(capsule_pos: [f32; 3], half_height: f32, point: [f32; 3]) -> [f32; 3] {
    let dy = point[1] - capsule_pos[1];
    let clamped_y = dy.clamp(-half_height, half_height);
    [capsule_pos[0], capsule_pos[1] + clamped_y, capsule_pos[2]]
}

fn capsule_sphere_contact(
    cap_pos: [f32; 3],
    cap_radius: f32,
    cap_half_h: f32,
    sphere_pos: [f32; 3],
    sphere_radius: f32,
    cap_idx: usize,
    sphere_idx: usize,
) -> Option<Contact> {
    let closest = capsule_closest_segment_point(cap_pos, cap_half_h, sphere_pos);
    sphere_sphere(closest, cap_radius, sphere_pos, sphere_radius, cap_idx, sphere_idx)
}

fn capsule_plane_contact(
    cap_pos: [f32; 3],
    cap_radius: f32,
    cap_half_h: f32,
    plane_n: [f32; 3],
    plane_d: f32,
    cap_idx: usize,
    plane_idx: usize,
) -> Option<Contact> {
    // Test both capsule endpoints; use whichever is closer to the plane
    let top = [cap_pos[0], cap_pos[1] + cap_half_h, cap_pos[2]];
    let bot = [cap_pos[0], cap_pos[1] - cap_half_h, cap_pos[2]];
    let dist_top = vec3_dot(plane_n, top) + plane_d;
    let dist_bot = vec3_dot(plane_n, bot) + plane_d;
    let (closest_pt, min_dist) = if dist_top < dist_bot {
        (top, dist_top)
    } else {
        (bot, dist_bot)
    };
    if min_dist >= cap_radius {
        return None;
    }
    let depth = cap_radius - min_dist;
    Some(Contact {
        body_a: cap_idx,
        body_b: plane_idx,
        normal: plane_n,
        depth,
        point: [
            closest_pt[0] - plane_n[0] * min_dist,
            closest_pt[1] - plane_n[1] * min_dist,
            closest_pt[2] - plane_n[2] * min_dist,
        ],
        cached_impulse: 0.0,
    })
}

fn capsule_aabb_contact(
    cap_pos: [f32; 3],
    cap_radius: f32,
    cap_half_h: f32,
    aabb_pos: [f32; 3],
    aabb_half: [f32; 3],
    cap_idx: usize,
    aabb_idx: usize,
) -> Option<Contact> {
    // Find the closest point on the capsule segment to the AABB center,
    // then treat as sphere-AABB with that point and the capsule radius.
    let seg_point = capsule_closest_segment_point(cap_pos, cap_half_h, aabb_pos);
    sphere_aabb(seg_point, cap_radius, aabb_pos, aabb_half, cap_idx, aabb_idx)
}

// ── Cylinder narrowphase ────────────────────────────────────────────

fn cylinder_sphere_contact(
    cyl_pos: [f32; 3],
    cyl_radius: f32,
    cyl_half_h: f32,
    sphere_pos: [f32; 3],
    sphere_radius: f32,
    cyl_idx: usize,
    sphere_idx: usize,
) -> Option<Contact> {
    // Decompose into radial (XZ) and axial (Y) distances
    let dx = sphere_pos[0] - cyl_pos[0];
    let dz = sphere_pos[2] - cyl_pos[2];
    let radial_dist = (dx * dx + dz * dz).sqrt();
    let dy = sphere_pos[1] - cyl_pos[1];
    let clamped_y = dy.clamp(-cyl_half_h, cyl_half_h);

    // Closest point on cylinder surface
    let on_axis = [cyl_pos[0], cyl_pos[1] + clamped_y, cyl_pos[2]];
    let mut closest = on_axis;

    if radial_dist > 1e-8 {
        let scale = cyl_radius / radial_dist;
        // Clamp to cylinder barrel
        if radial_dist > cyl_radius {
            closest[0] = cyl_pos[0] + dx * scale;
            closest[2] = cyl_pos[2] + dz * scale;
        } else {
            closest[0] = sphere_pos[0];
            closest[2] = sphere_pos[2];
        }
    }

    // Also check cap faces
    let cap_overlap_y = cyl_half_h - dy.abs();
    let cap_overlap_r = cyl_radius - radial_dist;

    // If sphere center is inside the cylinder projection, use axis separation
    if radial_dist < cyl_radius && dy.abs() < cyl_half_h {
        // Inside both — find minimum separation axis
        if cap_overlap_y < cap_overlap_r {
            // Push along Y
            let sign = if dy > 0.0 { 1.0 } else { -1.0 };
            let depth = cap_overlap_y + sphere_radius;
            let normal = [0.0, sign, 0.0];
            let point = [sphere_pos[0], cyl_pos[1] + cyl_half_h * sign, sphere_pos[2]];
            return Some(Contact {
                body_a: cyl_idx,
                body_b: sphere_idx,
                normal,
                depth,
                point,
                cached_impulse: 0.0,
            });
        } else {
            // Push along radial
            let depth = cap_overlap_r + sphere_radius;
            if radial_dist > 1e-8 {
                let nx = dx / radial_dist;
                let nz = dz / radial_dist;
                let point = [cyl_pos[0] + nx * cyl_radius, on_axis[1], cyl_pos[2] + nz * cyl_radius];
                return Some(Contact {
                    body_a: cyl_idx,
                    body_b: sphere_idx,
                    normal: [nx, 0.0, nz],
                    depth,
                    point,
                    cached_impulse: 0.0,
                });
            } else {
                return Some(Contact {
                    body_a: cyl_idx,
                    body_b: sphere_idx,
                    normal: [1.0, 0.0, 0.0],
                    depth,
                    point: closest,
                    cached_impulse: 0.0,
                });
            }
        }
    }

    // Outside — distance from sphere center to closest point on cylinder
    let to_sphere = [
        sphere_pos[0] - closest[0],
        sphere_pos[1] - closest[1],
        sphere_pos[2] - closest[2],
    ];
    let dist = vec3_len(to_sphere);
    if dist >= sphere_radius {
        return None;
    }
    let normal = if dist > 1e-8 {
        vec3_scale(to_sphere, 1.0 / dist)
    } else {
        [0.0, 1.0, 0.0]
    };
    Some(Contact {
        body_a: cyl_idx,
        body_b: sphere_idx,
        normal,
        depth: sphere_radius - dist,
        point: closest,
        cached_impulse: 0.0,
    })
}

fn cylinder_plane_contact(
    cyl_pos: [f32; 3],
    cyl_radius: f32,
    cyl_half_h: f32,
    plane_n: [f32; 3],
    plane_d: f32,
    cyl_idx: usize,
    plane_idx: usize,
) -> Option<Contact> {
    // Test the two most extreme points of the cylinder against the plane:
    // Bottom/top cap center +/- the radial extent projected onto the plane tangent.
    // The deepest-penetrating point on the cylinder rim against the plane.
    let top_center = [cyl_pos[0], cyl_pos[1] + cyl_half_h, cyl_pos[2]];
    let bot_center = [cyl_pos[0], cyl_pos[1] - cyl_half_h, cyl_pos[2]];

    // Radial direction most aligned with the plane normal (in XZ)
    let radial_n = [plane_n[0], 0.0, plane_n[2]];
    let radial_len = vec3_len(radial_n);
    let rim_offset = if radial_len > 1e-8 {
        vec3_scale(radial_n, -cyl_radius / radial_len)
    } else {
        [0.0, 0.0, 0.0]
    };

    // Four candidate points: top center, bot center, top rim, bot rim
    let candidates = [
        top_center,
        bot_center,
        [
            top_center[0] + rim_offset[0],
            top_center[1],
            top_center[2] + rim_offset[2],
        ],
        [
            bot_center[0] + rim_offset[0],
            bot_center[1],
            bot_center[2] + rim_offset[2],
        ],
    ];

    let mut deepest_dist = f32::MAX;
    let mut deepest_pt = cyl_pos;
    for pt in &candidates {
        let dist = vec3_dot(plane_n, *pt) + plane_d;
        if dist < deepest_dist {
            deepest_dist = dist;
            deepest_pt = *pt;
        }
    }

    if deepest_dist >= 0.0 {
        return None;
    }
    Some(Contact {
        body_a: cyl_idx,
        body_b: plane_idx,
        normal: plane_n,
        depth: -deepest_dist,
        point: [
            deepest_pt[0] - plane_n[0] * deepest_dist,
            deepest_pt[1] - plane_n[1] * deepest_dist,
            deepest_pt[2] - plane_n[2] * deepest_dist,
        ],
        cached_impulse: 0.0,
    })
}

// ── Cone narrowphase ────────────────────────────────────────────────

fn cone_sphere_contact(
    cone_pos: [f32; 3],
    cone_radius: f32,
    cone_height: f32,
    sphere_pos: [f32; 3],
    sphere_radius: f32,
    cone_idx: usize,
    sphere_idx: usize,
) -> Option<Contact> {
    // Cone is Y-axis aligned with base at cone_pos.y and tip at cone_pos.y + height.
    // Approximate the cone as a tapered shape. Find the closest point on the cone
    // surface to the sphere center.
    let dx = sphere_pos[0] - cone_pos[0];
    let dy = sphere_pos[1] - cone_pos[1];
    let dz = sphere_pos[2] - cone_pos[2];
    let radial_dist = (dx * dx + dz * dz).sqrt();

    // Clamp Y to cone height range
    let clamped_y = dy.clamp(0.0, cone_height);
    // Radius at this height: linearly interpolates from cone_radius at base to 0 at tip
    let r_at_y = cone_radius * (1.0 - clamped_y / cone_height);

    // Closest point on the cone surface
    let mut closest = [cone_pos[0], cone_pos[1] + clamped_y, cone_pos[2]];
    if radial_dist > 1e-8 {
        let clamp_r = radial_dist.min(r_at_y);
        closest[0] += dx / radial_dist * clamp_r;
        closest[2] += dz / radial_dist * clamp_r;
    }

    // If sphere center is inside the cone, find the shallowest exit
    if dy >= 0.0 && dy <= cone_height && radial_dist < r_at_y {
        // Inside — compute distances to base, tip, and side
        let base_dist = dy + sphere_radius;
        let tip_dist = cone_height - dy; // approximate
        let slant_len = (cone_height * cone_height + cone_radius * cone_radius).sqrt();
        let side_normal_y = cone_radius / slant_len;
        let side_normal_r = cone_height / slant_len;
        let side_dist = (r_at_y - radial_dist) * side_normal_r + sphere_radius;

        if base_dist < tip_dist && base_dist < side_dist {
            // Push out through base
            return Some(Contact {
                body_a: cone_idx,
                body_b: sphere_idx,
                normal: [0.0, -1.0, 0.0],
                depth: base_dist,
                point: [sphere_pos[0], cone_pos[1], sphere_pos[2]],
                cached_impulse: 0.0,
            });
        } else if side_dist < tip_dist {
            // Push out through side
            if radial_dist > 1e-8 {
                let nx = dx / radial_dist * side_normal_r;
                let nz = dz / radial_dist * side_normal_r;
                let normal = vec3_normalize([nx, side_normal_y, nz]);
                return Some(Contact {
                    body_a: cone_idx,
                    body_b: sphere_idx,
                    normal,
                    depth: side_dist,
                    point: closest,
                    cached_impulse: 0.0,
                });
            }
        }
        // Push out through tip area
        return Some(Contact {
            body_a: cone_idx,
            body_b: sphere_idx,
            normal: [0.0, 1.0, 0.0],
            depth: sphere_radius,
            point: [cone_pos[0], cone_pos[1] + cone_height, cone_pos[2]],
            cached_impulse: 0.0,
        });
    }

    let to_sphere = [
        sphere_pos[0] - closest[0],
        sphere_pos[1] - closest[1],
        sphere_pos[2] - closest[2],
    ];
    let dist = vec3_len(to_sphere);
    if dist >= sphere_radius {
        return None;
    }
    let normal = if dist > 1e-8 {
        vec3_scale(to_sphere, 1.0 / dist)
    } else {
        [0.0, 1.0, 0.0]
    };
    Some(Contact {
        body_a: cone_idx,
        body_b: sphere_idx,
        normal,
        depth: sphere_radius - dist,
        point: closest,
        cached_impulse: 0.0,
    })
}

fn cone_plane_contact(
    cone_pos: [f32; 3],
    cone_radius: f32,
    cone_height: f32,
    plane_n: [f32; 3],
    plane_d: f32,
    cone_idx: usize,
    plane_idx: usize,
) -> Option<Contact> {
    // Test tip and base rim extremity against the plane.
    let tip = [cone_pos[0], cone_pos[1] + cone_height, cone_pos[2]];

    // Base center
    let base = cone_pos;

    // Find the base rim point most penetrating the plane
    let radial_n = [plane_n[0], 0.0, plane_n[2]];
    let radial_len = vec3_len(radial_n);
    let rim_offset = if radial_len > 1e-8 {
        vec3_scale(radial_n, -cone_radius / radial_len)
    } else {
        [0.0, 0.0, 0.0]
    };
    let rim_pt = [base[0] + rim_offset[0], base[1], base[2] + rim_offset[2]];

    let candidates = [tip, base, rim_pt];
    let mut deepest_dist = f32::MAX;
    let mut deepest_pt = cone_pos;
    for pt in &candidates {
        let dist = vec3_dot(plane_n, *pt) + plane_d;
        if dist < deepest_dist {
            deepest_dist = dist;
            deepest_pt = *pt;
        }
    }

    if deepest_dist >= 0.0 {
        return None;
    }
    Some(Contact {
        body_a: cone_idx,
        body_b: plane_idx,
        normal: plane_n,
        depth: -deepest_dist,
        point: [
            deepest_pt[0] - plane_n[0] * deepest_dist,
            deepest_pt[1] - plane_n[1] * deepest_dist,
            deepest_pt[2] - plane_n[2] * deepest_dist,
        ],
        cached_impulse: 0.0,
    })
}

// ── Island splitting (union-find) ─────────────────────────────────────

/// Union-Find island detection. O(contacts * alpha(n)) ~ O(contacts).
/// Bodies sharing contacts form islands. Islands are independent and can be
/// solved in parallel (with rayon) or sequentially without cross-island deps.
fn build_islands(body_count: usize, contacts: &[Contact]) -> Vec<Vec<usize>> {
    let mut parent: Vec<usize> = (0..body_count).collect();
    let mut rank: Vec<u8> = vec![0; body_count];

    fn find(parent: &mut [usize], mut x: usize) -> usize {
        while parent[x] != x {
            parent[x] = parent[parent[x]]; // path compression
            x = parent[x];
        }
        x
    }

    fn union(parent: &mut [usize], rank: &mut [u8], a: usize, b: usize) {
        let ra = find(parent, a);
        let rb = find(parent, b);
        if ra == rb {
            return;
        }
        if rank[ra] < rank[rb] {
            parent[ra] = rb;
        } else if rank[ra] > rank[rb] {
            parent[rb] = ra;
        } else {
            parent[rb] = ra;
            rank[ra] += 1;
        }
    }

    // Union all contact pairs
    for c in contacts {
        union(&mut parent, &mut rank, c.body_a, c.body_b);
    }

    // Group contact indices by island root
    let mut island_map: std::collections::HashMap<usize, Vec<usize>> = std::collections::HashMap::new();
    for (ci, c) in contacts.iter().enumerate() {
        let root = find(&mut parent, c.body_a);
        island_map.entry(root).or_default().push(ci);
    }

    island_map.into_values().collect()
}

/// Apply warm-start: pre-apply cached impulses from previous frame contacts.
/// Matching is by body pair (order-independent). 80% damped warm-start to
/// prevent overshoot while still converging faster than cold start.
fn warm_start_contacts(contacts: &mut [Contact], prev_contacts: &[Contact], bodies: &mut [RigidBody]) {
    // Build HashMap for O(1) lookup instead of O(n) linear scan per contact.
    // Key: canonical pair (min, max) so order doesn't matter.
    use std::collections::HashMap;
    let mut cache: HashMap<(usize, usize), f32> = HashMap::with_capacity(prev_contacts.len());
    for pc in prev_contacts {
        let key = if pc.body_a <= pc.body_b {
            (pc.body_a, pc.body_b)
        } else {
            (pc.body_b, pc.body_a)
        };
        cache.insert(key, pc.cached_impulse);
    }

    for c in contacts.iter_mut() {
        let key = if c.body_a <= c.body_b {
            (c.body_a, c.body_b)
        } else {
            (c.body_b, c.body_a)
        };
        let cached = cache.get(&key).copied().unwrap_or(0.0);

        if cached.abs() > 1e-6 {
            c.cached_impulse = cached;
            let n = c.normal;
            let inv_sum = bodies[c.body_a].inv_mass + bodies[c.body_b].inv_mass;
            if inv_sum > 1e-8 {
                let j = cached * 0.8;
                bodies[c.body_a].velocity[0] += n[0] * j * bodies[c.body_a].inv_mass;
                bodies[c.body_a].velocity[1] += n[1] * j * bodies[c.body_a].inv_mass;
                bodies[c.body_a].velocity[2] += n[2] * j * bodies[c.body_a].inv_mass;
                bodies[c.body_b].velocity[0] -= n[0] * j * bodies[c.body_b].inv_mass;
                bodies[c.body_b].velocity[1] -= n[1] * j * bodies[c.body_b].inv_mass;
                bodies[c.body_b].velocity[2] -= n[2] * j * bodies[c.body_b].inv_mass;
            }
        }
    }
}

// ── Contact resolution (sequential impulse) ──────────────────────────

fn resolve_contact(a: &mut RigidBody, b: &mut RigidBody, contact: &mut Contact) {
    let n = contact.normal;

    // Relative velocity
    let v_rel = [
        a.velocity[0] - b.velocity[0],
        a.velocity[1] - b.velocity[1],
        a.velocity[2] - b.velocity[2],
    ];
    let v_along_normal = vec3_dot(v_rel, n);

    // Bodies separating — no impulse needed
    if v_along_normal > 0.0 {
        return;
    }

    // Combined restitution (min of both)
    let e = a.restitution.min(b.restitution);

    // Impulse magnitude: j = -(1+e) * v_rel·n / (1/m_a + 1/m_b)
    let inv_mass_sum = a.inv_mass + b.inv_mass;
    if inv_mass_sum < 1e-8 {
        return; // Both static
    }
    let j = -(1.0 + e) * v_along_normal / inv_mass_sum;
    contact.cached_impulse = j; // Cache for warm-start next frame

    // Apply impulse
    a.velocity[0] += n[0] * j * a.inv_mass;
    a.velocity[1] += n[1] * j * a.inv_mass;
    a.velocity[2] += n[2] * j * a.inv_mass;

    b.velocity[0] -= n[0] * j * b.inv_mass;
    b.velocity[1] -= n[1] * j * b.inv_mass;
    b.velocity[2] -= n[2] * j * b.inv_mass;

    // Positional correction (prevent sinking) — Baumgarte stabilization
    let correction_pct = 0.8;
    let slop = 0.01;
    let correction = (contact.depth - slop).max(0.0) * correction_pct / inv_mass_sum;

    a.position[0] -= n[0] * correction * a.inv_mass;
    a.position[1] -= n[1] * correction * a.inv_mass;
    a.position[2] -= n[2] * correction * a.inv_mass;

    b.position[0] += n[0] * correction * b.inv_mass;
    b.position[1] += n[1] * correction * b.inv_mass;
    b.position[2] += n[2] * correction * b.inv_mass;

    // Friction (tangent impulse)
    let tangent = [
        v_rel[0] - n[0] * v_along_normal,
        v_rel[1] - n[1] * v_along_normal,
        v_rel[2] - n[2] * v_along_normal,
    ];
    let tangent_len = vec3_len(tangent);
    if tangent_len > 1e-8 {
        let t = vec3_scale(tangent, 1.0 / tangent_len);
        let v_along_t = vec3_dot(v_rel, t);
        let mu = (a.friction + b.friction) * 0.5;
        let jt = (-v_along_t / inv_mass_sum).clamp(-j.abs() * mu, j.abs() * mu);

        a.velocity[0] += t[0] * jt * a.inv_mass;
        a.velocity[1] += t[1] * jt * a.inv_mass;
        a.velocity[2] += t[2] * jt * a.inv_mass;

        b.velocity[0] -= t[0] * jt * b.inv_mass;
        b.velocity[1] -= t[1] * jt * b.inv_mass;
        b.velocity[2] -= t[2] * jt * b.inv_mass;
    }
}

// ── Raycast helpers ──────────────────────────────────────────────────

fn ray_sphere(origin: [f32; 3], dir: [f32; 3], center: [f32; 3], radius: f32) -> Option<(f32, [f32; 3], [f32; 3])> {
    let oc = [origin[0] - center[0], origin[1] - center[1], origin[2] - center[2]];
    let b = vec3_dot(oc, dir);
    let c = vec3_dot(oc, oc) - radius * radius;
    let disc = b * b - c;
    if disc < 0.0 {
        return None;
    }
    let t = -b - disc.sqrt();
    if t < 0.0 {
        return None;
    }
    let point = [origin[0] + dir[0] * t, origin[1] + dir[1] * t, origin[2] + dir[2] * t];
    let normal = vec3_normalize([point[0] - center[0], point[1] - center[1], point[2] - center[2]]);
    Some((t, normal, point))
}

fn ray_plane(origin: [f32; 3], dir: [f32; 3], normal: [f32; 3], d: f32) -> Option<(f32, [f32; 3], [f32; 3])> {
    let denom = vec3_dot(normal, dir);
    if denom.abs() < 1e-8 {
        return None;
    }
    let t = -(vec3_dot(normal, origin) + d) / denom;
    if t < 0.0 {
        return None;
    }
    let point = [origin[0] + dir[0] * t, origin[1] + dir[1] * t, origin[2] + dir[2] * t];
    Some((t, normal, point))
}

fn ray_aabb(origin: [f32; 3], dir: [f32; 3], center: [f32; 3], half: [f32; 3]) -> Option<(f32, [f32; 3], [f32; 3])> {
    let mut tmin = f32::NEG_INFINITY;
    let mut tmax = f32::INFINITY;
    let mut hit_axis = 0usize;
    let mut hit_sign = 1.0f32;

    for i in 0..3 {
        if dir[i].abs() < 1e-8 {
            if origin[i] < center[i] - half[i] || origin[i] > center[i] + half[i] {
                return None;
            }
        } else {
            let inv_d = 1.0 / dir[i];
            let t1 = (center[i] - half[i] - origin[i]) * inv_d;
            let t2 = (center[i] + half[i] - origin[i]) * inv_d;
            let (t_near, t_far) = if t1 < t2 { (t1, t2) } else { (t2, t1) };
            if t_near > tmin {
                tmin = t_near;
                hit_axis = i;
                hit_sign = if dir[i] > 0.0 { -1.0 } else { 1.0 };
            }
            tmax = tmax.min(t_far);
            if tmin > tmax {
                return None;
            }
        }
    }
    if tmin < 0.0 {
        return None;
    }
    let point = [
        origin[0] + dir[0] * tmin,
        origin[1] + dir[1] * tmin,
        origin[2] + dir[2] * tmin,
    ];
    let mut normal = [0.0f32; 3];
    normal[hit_axis] = hit_sign;
    Some((tmin, normal, point))
}

// ── Raycast helpers (new shapes) ─────────────────────────────────────

fn ray_capsule(
    origin: [f32; 3],
    dir: [f32; 3],
    center: [f32; 3],
    radius: f32,
    half_height: f32,
) -> Option<(f32, [f32; 3], [f32; 3])> {
    // A capsule is the union of a cylinder body and two hemisphere caps.
    // Strategy: test the infinite cylinder (XZ cross-section) first, then the
    // two hemisphere caps if the cylinder hit is outside the axial range.

    // 1. Ray vs infinite cylinder along Y axis
    let ox = origin[0] - center[0];
    let oz = origin[2] - center[2];
    let a = dir[0] * dir[0] + dir[2] * dir[2];
    let b = ox * dir[0] + oz * dir[2];
    let c = ox * ox + oz * oz - radius * radius;

    let mut best: Option<(f32, [f32; 3], [f32; 3])> = None;

    if a > 1e-12 {
        let disc = b * b - a * c;
        if disc >= 0.0 {
            let sqrt_disc = disc.sqrt();
            for &t in &[(-b - sqrt_disc) / a, (-b + sqrt_disc) / a] {
                if t < 0.0 {
                    continue;
                }
                let hit_y = origin[1] + dir[1] * t - center[1];
                if hit_y >= -half_height && hit_y <= half_height {
                    let point = [origin[0] + dir[0] * t, origin[1] + dir[1] * t, origin[2] + dir[2] * t];
                    let normal = vec3_normalize([point[0] - center[0], 0.0, point[2] - center[2]]);
                    if best.as_ref().map_or(true, |prev| t < prev.0) {
                        best = Some((t, normal, point));
                    }
                    break; // first hit on the cylinder is closest
                }
            }
        }
    }

    // 2. Ray vs top hemisphere (center at center + half_height)
    let top = [center[0], center[1] + half_height, center[2]];
    if let Some((t, n, p)) = ray_sphere(origin, dir, top, radius) {
        if p[1] >= top[1] {
            // only the upper hemisphere
            if best.as_ref().map_or(true, |prev| t < prev.0) {
                best = Some((t, n, p));
            }
        }
    }

    // 3. Ray vs bottom hemisphere
    let bot = [center[0], center[1] - half_height, center[2]];
    if let Some((t, n, p)) = ray_sphere(origin, dir, bot, radius) {
        if p[1] <= bot[1] {
            // only the lower hemisphere
            if best.as_ref().map_or(true, |prev| t < prev.0) {
                best = Some((t, n, p));
            }
        }
    }

    best
}

fn ray_cylinder(
    origin: [f32; 3],
    dir: [f32; 3],
    center: [f32; 3],
    radius: f32,
    half_height: f32,
) -> Option<(f32, [f32; 3], [f32; 3])> {
    let mut best: Option<(f32, [f32; 3], [f32; 3])> = None;

    // 1. Ray vs infinite cylinder (XZ cross-section)
    let ox = origin[0] - center[0];
    let oz = origin[2] - center[2];
    let a = dir[0] * dir[0] + dir[2] * dir[2];
    let b = ox * dir[0] + oz * dir[2];
    let c = ox * ox + oz * oz - radius * radius;

    if a > 1e-12 {
        let disc = b * b - a * c;
        if disc >= 0.0 {
            let sqrt_disc = disc.sqrt();
            for &t in &[(-b - sqrt_disc) / a, (-b + sqrt_disc) / a] {
                if t < 0.0 {
                    continue;
                }
                let hit_y = origin[1] + dir[1] * t - center[1];
                if hit_y >= -half_height && hit_y <= half_height {
                    let point = [origin[0] + dir[0] * t, origin[1] + dir[1] * t, origin[2] + dir[2] * t];
                    let normal = vec3_normalize([point[0] - center[0], 0.0, point[2] - center[2]]);
                    if best.as_ref().map_or(true, |prev| t < prev.0) {
                        best = Some((t, normal, point));
                    }
                    break;
                }
            }
        }
    }

    // 2. Ray vs top cap (plane y = center.y + half_height)
    if dir[1].abs() > 1e-8 {
        let t_top = (center[1] + half_height - origin[1]) / dir[1];
        if t_top >= 0.0 {
            let px = origin[0] + dir[0] * t_top - center[0];
            let pz = origin[2] + dir[2] * t_top - center[2];
            if px * px + pz * pz <= radius * radius {
                if best.as_ref().map_or(true, |prev| t_top < prev.0) {
                    let point = [
                        origin[0] + dir[0] * t_top,
                        origin[1] + dir[1] * t_top,
                        origin[2] + dir[2] * t_top,
                    ];
                    best = Some((t_top, [0.0, 1.0, 0.0], point));
                }
            }
        }
        // 3. Ray vs bottom cap
        let t_bot = (center[1] - half_height - origin[1]) / dir[1];
        if t_bot >= 0.0 {
            let px = origin[0] + dir[0] * t_bot - center[0];
            let pz = origin[2] + dir[2] * t_bot - center[2];
            if px * px + pz * pz <= radius * radius {
                if best.as_ref().map_or(true, |prev| t_bot < prev.0) {
                    let point = [
                        origin[0] + dir[0] * t_bot,
                        origin[1] + dir[1] * t_bot,
                        origin[2] + dir[2] * t_bot,
                    ];
                    best = Some((t_bot, [0.0, -1.0, 0.0], point));
                }
            }
        }
    }

    best
}

fn ray_cone(
    origin: [f32; 3],
    dir: [f32; 3],
    center: [f32; 3],
    radius: f32,
    height: f32,
) -> Option<(f32, [f32; 3], [f32; 3])> {
    // Cone with base at center.y, tip at center.y + height. Y-axis aligned.
    // At height y, the cone radius = radius * (1 - y/height).
    // Parametric: x^2 + z^2 = (radius * (1 - (y - center.y) / height))^2
    // Let oy = origin.y - center.y, dy = dir.y
    // Let k = radius / height
    // x^2 + z^2 = k^2 * (height - (oy + dy*t))^2

    let oy = origin[1] - center[1];
    let ox = origin[0] - center[0];
    let oz = origin[2] - center[2];
    let k = radius / height;
    let k2 = k * k;

    // h_t = height - (oy + dir.y * t) = (height - oy) - dir.y * t
    let h0 = height - oy;
    // (ox + dx*t)^2 + (oz + dz*t)^2 = k2 * (h0 - dy*t)^2
    // Expand into at^2 + bt + c = 0
    let a = dir[0] * dir[0] + dir[2] * dir[2] - k2 * dir[1] * dir[1];
    let b = ox * dir[0] + oz * dir[2] + k2 * dir[1] * h0; // half of the real 2b
    let c = ox * ox + oz * oz - k2 * h0 * h0;

    let mut best: Option<(f32, [f32; 3], [f32; 3])> = None;

    // Solve quadratic
    let disc = b * b - a * c;
    if disc >= 0.0 && a.abs() > 1e-12 {
        let sqrt_disc = disc.sqrt();
        for &t in &[(-b - sqrt_disc) / a, (-b + sqrt_disc) / a] {
            if t < 0.0 {
                continue;
            }
            let hit_y = oy + dir[1] * t;
            if hit_y >= 0.0 && hit_y <= height {
                let point = [origin[0] + dir[0] * t, origin[1] + dir[1] * t, origin[2] + dir[2] * t];
                // Normal: gradient of x^2 + z^2 - k2*(height-y)^2 = 0
                let px = point[0] - center[0];
                let pz = point[2] - center[2];
                let py_cone = height - hit_y;
                let normal = vec3_normalize([2.0 * px, 2.0 * k2 * py_cone, 2.0 * pz]);
                if best.as_ref().map_or(true, |prev| t < prev.0) {
                    best = Some((t, normal, point));
                }
                break;
            }
        }
    }

    // Ray vs base cap (plane at y = center.y)
    if dir[1].abs() > 1e-8 {
        let t_base = -oy / dir[1];
        if t_base >= 0.0 {
            let px = ox + dir[0] * t_base;
            let pz = oz + dir[2] * t_base;
            if px * px + pz * pz <= radius * radius {
                if best.as_ref().map_or(true, |prev| t_base < prev.0) {
                    let point = [
                        origin[0] + dir[0] * t_base,
                        origin[1] + dir[1] * t_base,
                        origin[2] + dir[2] * t_base,
                    ];
                    best = Some((t_base, [0.0, -1.0, 0.0], point));
                }
            }
        }
    }

    best
}

// ── Vector math ──────────────────────────────────────────────────────

/// Atomic f32 addition via compare-exchange on AtomicU32 (bit-cast).
/// Used by the Parallel Jacobi solver for lock-free impulse accumulation.
#[cfg(feature = "parallel-physics")]
fn atomic_f32_add(atom: &std::sync::atomic::AtomicU32, val: f32) {
    use std::sync::atomic::Ordering;
    loop {
        let bits = atom.load(Ordering::Relaxed);
        let current = f32::from_bits(bits);
        let new = current + val;
        match atom.compare_exchange_weak(bits, new.to_bits(), Ordering::Relaxed, Ordering::Relaxed) {
            Ok(_) => break,
            Err(_) => {} // Retry on contention
        }
    }
}

fn vec3_dot(a: [f32; 3], b: [f32; 3]) -> f32 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

fn vec3_len(v: [f32; 3]) -> f32 {
    (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
}

fn vec3_scale(v: [f32; 3], s: f32) -> [f32; 3] {
    [v[0] * s, v[1] * s, v[2] * s]
}

fn vec3_normalize(v: [f32; 3]) -> [f32; 3] {
    let len = vec3_len(v);
    if len > 1e-8 {
        vec3_scale(v, 1.0 / len)
    } else {
        [0.0, 1.0, 0.0]
    }
}

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

    #[test]
    fn sphere_falls_under_gravity() {
        let mut world = PhysicsWorld::new();
        let id = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 10.0, 0.0]));
        world.step(1.0 / 60.0);
        let body = world.body(id).unwrap();
        assert!(body.position[1] < 10.0, "Sphere should fall");
        assert!(body.velocity[1] < 0.0, "Velocity should be negative");
    }

    #[test]
    fn sphere_bounces_on_plane() {
        let mut world = PhysicsWorld::new();
        let sphere = world.add_body(
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 })
                .with_position([0.0, 0.4, 0.0])
                .with_restitution(1.0),
        );
        world.add_body(RigidBody::fixed(CollisionShape::Plane {
            normal: [0.0, 1.0, 0.0],
            d: 0.0,
        }));
        // Give it downward velocity
        world.body_mut(sphere).unwrap().velocity = [0.0, -5.0, 0.0];
        world.step(1.0 / 60.0);
        let body = world.body(sphere).unwrap();
        // After collision with restitution=1.0, velocity should reverse
        assert!(
            body.velocity[1] > 0.0,
            "Sphere should bounce up, got {}",
            body.velocity[1]
        );
    }

    #[test]
    fn static_bodies_dont_move() {
        let mut world = PhysicsWorld::new();
        let id = world.add_body(RigidBody::fixed(CollisionShape::Plane {
            normal: [0.0, 1.0, 0.0],
            d: 0.0,
        }));
        world.step(1.0);
        let body = world.body(id).unwrap();
        assert_eq!(body.position, [0.0, 0.0, 0.0]);
    }

    #[test]
    fn sphere_sphere_collision() {
        // Two overlapping spheres — verify contact detection and impulse exchange
        let pos_a = [0.0f32, 0.0, 0.0];
        let pos_b = [0.8f32, 0.0, 0.0];
        let contact = sphere_sphere(pos_a, 0.5, pos_b, 0.5, 0, 1);
        assert!(
            contact.is_some(),
            "Spheres at distance 0.8 with radii 0.5+0.5 should overlap"
        );
        let c = contact.unwrap();
        assert!(c.depth > 0.0, "Penetration depth should be positive");
        assert!(c.normal[0] > 0.0, "Normal should point from A to B (positive X)");
    }

    #[test]
    fn aabb_collision() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0];
        let a = world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Aabb {
                    half_extents: [0.5, 0.5, 0.5],
                },
            )
            .with_position([0.0, 0.0, 0.0]),
        );
        let _b = world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Aabb {
                    half_extents: [0.5, 0.5, 0.5],
                },
            )
            .with_position([0.9, 0.0, 0.0]),
        );
        world.body_mut(a).unwrap().velocity = [1.0, 0.0, 0.0];
        world.step(1.0 / 60.0);
        assert!(!world.contacts().is_empty(), "Should detect AABB overlap");
    }

    #[test]
    fn raycast_hits_sphere() {
        let mut world = PhysicsWorld::new();
        world.add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 0.0, -5.0]));
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, 0.0, -1.0], 100.0);
        assert!(hit.is_some(), "Ray should hit the sphere");
        let h = hit.unwrap();
        assert!(
            (h.distance - 4.5).abs() < 0.01,
            "Distance should be ~4.5, got {}",
            h.distance
        );
    }

    #[test]
    fn raycast_misses() {
        let mut world = PhysicsWorld::new();
        world.add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([10.0, 0.0, 0.0]));
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, 0.0, -1.0], 100.0);
        assert!(hit.is_none(), "Ray should miss");
    }

    #[test]
    fn remove_body() {
        let mut world = PhysicsWorld::new();
        let id = world.add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }));
        assert_eq!(world.body_count(), 1);
        assert!(world.remove_body(id));
        assert_eq!(world.body_count(), 0);
        assert!(!world.remove_body(id)); // double remove returns false
    }

    #[test]
    fn zero_dt_is_noop() {
        let mut world = PhysicsWorld::new();
        let id = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 5.0, 0.0]));
        world.step(0.0);
        let body = world.body(id).unwrap();
        assert_eq!(body.position[1], 5.0);
    }

    #[test]
    fn raycast_plane() {
        let mut world = PhysicsWorld::new();
        world.add_body(RigidBody::fixed(CollisionShape::Plane {
            normal: [0.0, 1.0, 0.0],
            d: 0.0,
        }));
        let hit = world.raycast([0.0, 5.0, 0.0], [0.0, -1.0, 0.0], 100.0);
        assert!(hit.is_some());
        let h = hit.unwrap();
        assert!((h.distance - 5.0).abs() < 0.01);
    }

    #[test]
    fn sphere_aabb_collision() {
        let contact = sphere_aabb([0.0, 0.0, 0.0], 0.5, [0.8, 0.0, 0.0], [0.5, 0.5, 0.5], 0, 1);
        assert!(contact.is_some(), "Sphere-AABB should detect overlap");
    }

    // ── Capsule tests ───────────────────────────────────────────────

    #[test]
    fn capsule_sphere_contact_overlap() {
        // Capsule at origin (radius=0.5, half_height=1.0), sphere at (1.0, 0.0, 0.0) radius=0.6
        // Capsule segment closest to sphere is at (0, 0, 0) — distance is 1.0, sum radii 1.1
        let contact = capsule_sphere_contact([0.0, 0.0, 0.0], 0.5, 1.0, [1.0, 0.0, 0.0], 0.6, 0, 1);
        assert!(
            contact.is_some(),
            "Capsule-sphere should overlap at distance 1.0 with radii sum 1.1"
        );
        let c = contact.unwrap();
        assert!(c.depth > 0.0);
        assert!(c.normal[0] > 0.0, "Normal should point from capsule to sphere (+X)");
    }

    #[test]
    fn capsule_sphere_contact_miss() {
        let contact = capsule_sphere_contact([0.0, 0.0, 0.0], 0.3, 1.0, [5.0, 0.0, 0.0], 0.3, 0, 1);
        assert!(
            contact.is_none(),
            "Should not overlap at distance 5.0 with radii sum 0.6"
        );
    }

    #[test]
    fn capsule_sphere_contact_along_axis() {
        // Sphere above capsule tip — should still detect via hemisphere logic
        let contact = capsule_sphere_contact([0.0, 0.0, 0.0], 0.5, 1.0, [0.0, 1.8, 0.0], 0.5, 0, 1);
        assert!(contact.is_some(), "Sphere near top cap should overlap");
    }

    #[test]
    fn capsule_plane_contact_resting() {
        // Capsule at y=0.5, half_height=1.0, radius=0.3 — bottom at y=-0.5
        // Plane at y=0 — bottom endpoint at -0.5, closest point at -0.5, dist = -0.5
        // Since dist(-0.5) < radius(0.3) is not the test — dist to plane is -0.5, and -0.5 < 0.3? No.
        // Let's place capsule so it actually intersects.
        // Capsule at y=0.2, half_height=0.5, radius=0.3 — bottom at y=-0.3
        // Distance of bottom to plane = -0.3. Since -0.3 < 0.3, overlap.
        let contact = capsule_plane_contact([0.0, 0.2, 0.0], 0.3, 0.5, [0.0, 1.0, 0.0], 0.0, 0, 1);
        assert!(contact.is_some(), "Capsule bottom should penetrate the ground plane");
        let c = contact.unwrap();
        assert!(c.depth > 0.0);
    }

    #[test]
    fn capsule_aabb_contact_overlap() {
        let contact = capsule_aabb_contact([0.0, 0.0, 0.0], 0.5, 1.0, [0.8, 0.0, 0.0], [0.5, 0.5, 0.5], 0, 1);
        assert!(contact.is_some(), "Capsule-AABB should overlap");
    }

    // ── Cylinder tests ──────────────────────────────────────────────

    #[test]
    fn cylinder_sphere_contact_radial() {
        // Cylinder at origin, radius=0.5, half_height=1.0
        // Sphere at (0.8, 0.0, 0.0) radius=0.5 — should overlap radially
        let contact = cylinder_sphere_contact([0.0, 0.0, 0.0], 0.5, 1.0, [0.8, 0.0, 0.0], 0.5, 0, 1);
        assert!(contact.is_some(), "Cylinder-sphere should detect radial overlap");
        let c = contact.unwrap();
        assert!(c.depth > 0.0);
    }

    #[test]
    fn cylinder_sphere_contact_above() {
        // Sphere above the cylinder cap, out of range
        let contact = cylinder_sphere_contact([0.0, 0.0, 0.0], 0.5, 1.0, [0.0, 5.0, 0.0], 0.3, 0, 1);
        assert!(contact.is_none(), "Sphere far above cylinder should not overlap");
    }

    #[test]
    fn cylinder_plane_contact_resting() {
        // Cylinder at y=0.5, half_height=1.0 — bottom face at y=-0.5
        // Plane at y=0 — bottom face penetrates by 0.5
        let contact = cylinder_plane_contact([0.0, 0.5, 0.0], 0.5, 1.0, [0.0, 1.0, 0.0], 0.0, 0, 1);
        assert!(contact.is_some(), "Cylinder bottom face should penetrate ground plane");
        let c = contact.unwrap();
        assert!(c.depth > 0.0);
    }

    #[test]
    fn cylinder_plane_contact_floating() {
        // Cylinder at y=5.0 — well above the plane
        let contact = cylinder_plane_contact([0.0, 5.0, 0.0], 0.5, 1.0, [0.0, 1.0, 0.0], 0.0, 0, 1);
        assert!(contact.is_none(), "Cylinder above plane should not overlap");
    }

    // ── Cone tests ──────────────────────────────────────────────────

    #[test]
    fn cone_sphere_contact_near_base() {
        // Cone at origin, radius=1.0, height=2.0
        // Sphere at (1.2, 0.0, 0.0) radius=0.5 — near the base edge
        let contact = cone_sphere_contact([0.0, 0.0, 0.0], 1.0, 2.0, [1.2, 0.0, 0.0], 0.5, 0, 1);
        assert!(contact.is_some(), "Sphere near cone base should overlap");
        let c = contact.unwrap();
        assert!(c.depth > 0.0);
    }

    #[test]
    fn cone_sphere_contact_miss() {
        let contact = cone_sphere_contact([0.0, 0.0, 0.0], 0.5, 2.0, [5.0, 0.0, 0.0], 0.3, 0, 1);
        assert!(contact.is_none(), "Sphere far from cone should not overlap");
    }

    #[test]
    fn cone_plane_contact_resting_on_base() {
        // Cone at y=0, height=2.0 — base at y=0, tip at y=2
        // Plane at y=0 — base sits exactly on plane (no penetration)
        // Move cone down slightly so it penetrates
        let contact = cone_plane_contact([0.0, -0.1, 0.0], 1.0, 2.0, [0.0, 1.0, 0.0], 0.0, 0, 1);
        assert!(contact.is_some(), "Cone base should penetrate ground plane");
        let c = contact.unwrap();
        assert!(c.depth > 0.0);
    }

    #[test]
    fn cone_plane_contact_floating() {
        let contact = cone_plane_contact([0.0, 5.0, 0.0], 0.5, 1.0, [0.0, 1.0, 0.0], 0.0, 0, 1);
        assert!(contact.is_none(), "Cone above plane should not overlap");
    }

    // ── Raycast new shapes ──────────────────────────────────────────

    #[test]
    fn raycast_capsule_side() {
        let mut world = PhysicsWorld::new();
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Capsule {
                    radius: 0.5,
                    half_height: 1.0,
                },
            )
            .with_position([0.0, 0.0, -5.0]),
        );
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, 0.0, -1.0], 100.0);
        assert!(hit.is_some(), "Ray should hit capsule");
        let h = hit.unwrap();
        assert!(
            (h.distance - 4.5).abs() < 0.01,
            "Distance should be ~4.5, got {}",
            h.distance
        );
    }

    #[test]
    fn raycast_capsule_misses() {
        let mut world = PhysicsWorld::new();
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Capsule {
                    radius: 0.5,
                    half_height: 1.0,
                },
            )
            .with_position([10.0, 0.0, 0.0]),
        );
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, 0.0, -1.0], 100.0);
        assert!(hit.is_none(), "Ray should miss capsule");
    }

    #[test]
    fn raycast_cylinder_side() {
        let mut world = PhysicsWorld::new();
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Cylinder {
                    radius: 0.5,
                    half_height: 1.0,
                },
            )
            .with_position([0.0, 0.0, -5.0]),
        );
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, 0.0, -1.0], 100.0);
        assert!(hit.is_some(), "Ray should hit cylinder barrel");
        let h = hit.unwrap();
        assert!(
            (h.distance - 4.5).abs() < 0.01,
            "Distance should be ~4.5, got {}",
            h.distance
        );
    }

    #[test]
    fn raycast_cylinder_cap() {
        let mut world = PhysicsWorld::new();
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Cylinder {
                    radius: 1.0,
                    half_height: 0.5,
                },
            )
            .with_position([0.0, -3.0, 0.0]),
        );
        // Ray straight down should hit the top cap
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, -1.0, 0.0], 100.0);
        assert!(hit.is_some(), "Ray should hit cylinder top cap");
        let h = hit.unwrap();
        assert!(
            (h.distance - 2.5).abs() < 0.01,
            "Distance to top cap should be ~2.5, got {}",
            h.distance
        );
    }

    #[test]
    fn raycast_cone_side() {
        let mut world = PhysicsWorld::new();
        // Cone at z=-5, base radius=1.0, height=2.0
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Cone {
                    radius: 1.0,
                    height: 2.0,
                },
            )
            .with_position([0.0, -1.0, -5.0]),
        );
        // Ray along -Z at y=0 (mid-height of cone). At y=0, local_y=1.0, cone_r at this height = 1.0*(1-1/2) = 0.5
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, 0.0, -1.0], 100.0);
        assert!(hit.is_some(), "Ray should hit cone side");
    }

    #[test]
    fn raycast_cone_base() {
        let mut world = PhysicsWorld::new();
        // Cone below origin, looking down at base
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Cone {
                    radius: 2.0,
                    height: 3.0,
                },
            )
            .with_position([0.0, -5.0, 0.0]),
        );
        let hit = world.raycast([0.0, 0.0, 0.0], [0.0, -1.0, 0.0], 100.0);
        assert!(hit.is_some(), "Ray should hit cone base");
    }

    // ── Spatial hash grid tests ─────────────────────────────────────

    #[test]
    fn spatial_hash_grid_finds_nearby_pair() {
        let bodies = vec![
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 0.0, 0.0]),
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.5, 0.0, 0.0]),
        ];
        let mut grid = SpatialHashGrid::new(2.0);
        grid.populate(&bodies);
        let pairs = grid.query_pairs(&bodies);
        assert!(pairs.contains(&(0, 1)), "Nearby bodies should form a pair");
    }

    #[test]
    fn spatial_hash_grid_distant_no_pair() {
        let bodies = vec![
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 0.0, 0.0]),
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([100.0, 100.0, 100.0]),
        ];
        let mut grid = SpatialHashGrid::new(2.0);
        grid.populate(&bodies);
        let pairs = grid.query_pairs(&bodies);
        assert!(pairs.is_empty(), "Distant bodies should not form a pair");
    }

    #[test]
    fn spatial_hash_grid_cross_cell_boundary() {
        // Two bodies in neighbouring cells
        let bodies = vec![
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([1.9, 0.0, 0.0]),
            RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([2.1, 0.0, 0.0]),
        ];
        let mut grid = SpatialHashGrid::new(2.0);
        grid.populate(&bodies);
        let pairs = grid.query_pairs(&bodies);
        assert!(
            pairs.contains(&(0, 1)),
            "Bodies across cell boundary should form a pair"
        );
    }

    #[test]
    fn spatial_hash_grid_many_bodies() {
        // 100 bodies in a line; each should pair with its neighbours
        let bodies: Vec<RigidBody> = (0..100)
            .map(|i| {
                RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.3 }).with_position([
                    i as f32 * 0.5,
                    0.0,
                    0.0,
                ])
            })
            .collect();
        let mut grid = SpatialHashGrid::new(2.0);
        grid.populate(&bodies);
        let pairs = grid.query_pairs(&bodies);
        // At minimum, each consecutive pair should appear
        assert!(
            pairs.len() > 50,
            "Should have many pairs for closely-spaced bodies, got {}",
            pairs.len()
        );
    }

    // ── Sleeping tests ──────────────────────────────────────────────

    #[test]
    fn body_falls_asleep_below_threshold() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0]; // no gravity — body stays still
        world.sleep_config = SleepConfig {
            linear_threshold: 0.01,
            angular_threshold: 0.01,
            frames_to_sleep: 5,
        };
        let id = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 0.0, 0.0]));
        // Body has zero velocity, should accumulate sleep frames
        for _ in 0..10 {
            world.step(1.0 / 60.0);
        }
        let body = world.body(id).unwrap();
        assert!(
            body.is_sleeping,
            "Body with zero velocity should be sleeping after enough frames"
        );
        assert!(body.sleep_frames >= 5);
    }

    #[test]
    fn moving_body_does_not_sleep() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0];
        world.sleep_config = SleepConfig {
            linear_threshold: 0.01,
            angular_threshold: 0.01,
            frames_to_sleep: 5,
        };
        let id = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 0.0, 0.0]));
        world.body_mut(id).unwrap().velocity = [1.0, 0.0, 0.0];
        for _ in 0..10 {
            world.step(1.0 / 60.0);
        }
        let body = world.body(id).unwrap();
        assert!(!body.is_sleeping, "Body with velocity > threshold should not sleep");
    }

    #[test]
    fn sleeping_body_wakes_on_contact() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0];
        world.sleep_config = SleepConfig {
            linear_threshold: 0.1,
            angular_threshold: 0.1,
            frames_to_sleep: 3,
        };
        // Body A: will be put to sleep manually
        let a = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 0.0, 0.0]));
        // Body B: approaching A
        let b = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([2.0, 0.0, 0.0]));

        // Put A to sleep manually
        {
            let body_a = world.body_mut(a).unwrap();
            body_a.is_sleeping = true;
            body_a.sleep_frames = 100;
        }

        // Give B velocity towards A
        world.body_mut(b).unwrap().velocity = [-10.0, 0.0, 0.0];

        // Step until they collide — B moves left, after a few frames they overlap
        for _ in 0..20 {
            world.step(1.0 / 60.0);
        }
        let body_a = world.body(a).unwrap();
        // A should have been woken by the contact
        assert!(!body_a.is_sleeping, "Sleeping body should wake on contact");
    }

    #[test]
    fn sleeping_body_skips_integration() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, -9.81, 0.0];
        world.sleep_config = SleepConfig {
            linear_threshold: 0.01,
            angular_threshold: 0.01,
            frames_to_sleep: 3,
        };
        let id = world
            .add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.0, 5.0, 0.0]));
        // Manually put to sleep
        {
            let body = world.body_mut(id).unwrap();
            body.is_sleeping = true;
            body.sleep_frames = 100;
        }
        let pos_before = world.body(id).unwrap().position;
        world.step(1.0 / 60.0);
        let pos_after = world.body(id).unwrap().position;
        assert_eq!(pos_before, pos_after, "Sleeping body should not be integrated");
    }

    // ── detect_contact integration for new shapes ───────────────────

    #[test]
    fn detect_contact_capsule_sphere_via_world() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0];
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Capsule {
                    radius: 0.5,
                    half_height: 1.0,
                },
            )
            .with_position([0.0, 0.0, 0.0]),
        );
        world.add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.8, 0.0, 0.0]));
        world.step(1.0 / 60.0);
        assert!(
            !world.contacts().is_empty(),
            "World should detect capsule-sphere contact"
        );
    }

    #[test]
    fn detect_contact_cylinder_sphere_via_world() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0];
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Cylinder {
                    radius: 0.5,
                    half_height: 1.0,
                },
            )
            .with_position([0.0, 0.0, 0.0]),
        );
        world.add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([0.8, 0.0, 0.0]));
        world.step(1.0 / 60.0);
        assert!(
            !world.contacts().is_empty(),
            "World should detect cylinder-sphere contact"
        );
    }

    #[test]
    fn detect_contact_cone_sphere_via_world() {
        let mut world = PhysicsWorld::new();
        world.gravity = [0.0, 0.0, 0.0];
        world.add_body(
            RigidBody::dynamic(
                1.0,
                CollisionShape::Cone {
                    radius: 1.0,
                    height: 2.0,
                },
            )
            .with_position([0.0, 0.0, 0.0]),
        );
        world.add_body(RigidBody::dynamic(1.0, CollisionShape::Sphere { radius: 0.5 }).with_position([1.2, 0.0, 0.0]));
        world.step(1.0 / 60.0);
        assert!(!world.contacts().is_empty(), "World should detect cone-sphere contact");
    }
}