astrodyn_dynamics 0.1.1

//! Storage-agnostic mass-tree composition.
//!
//! [`MassStorage`] is the storage-side counterpart of `FrameStorage`
//! (sketched in the [Frame-Tree-ECS-Native design doc Section
//! 7](https://github.com/simnaut/astrodyn/wiki/Frame-Tree-ECS-Native)):
//! a thin trait whose implementations expose a parent link plus the
//! per-node *core* mass-properties view, so a single composition
//! kernel can drive both the arena-backed [`MassTree`] used by the
//! `astrodyn_runner` and the Bevy adapter's `MassChildOf` relation.
//!
//! The kernel ([`recompute_composites_via_storage`] and the lower-level
//! [`compute_node_composite`]) ports the same parallel-axis (Steiner)
//! algorithm encoded in [`MassTree::recompute_composites`]
//! (`mass_calc_composite_cm.cc` + `mass_calc_composite_inertia.cc` from
//! JEOD v5.4):
//!
//! 1. core / atomic node:  `composite ← core`.
//! 2. composite CoM = mass-weighted average over `core` and each
//!    child's `composite_wrt_pstr.position` (the child's composite CoM
//!    expressed in *this* body's structural frame).
//! 3. composite inertia = core inertia shifted to composite CoM
//!    (point-mass shift) + each child's composite inertia rotated
//!    `T^T · I · T` and shifted by the child→composite offset.
//! 4. **every** mass-bearing node — root, internal, and atomic leaf
//!    alike — gets `inverse_inertia ← inertia.inverse()` whenever
//!    `mass > 0`, matching JEOD's `MassBody::update_mass_properties`
//!    and `mass_detach.cc:322-335` for the integration root and
//!    extending the same write to non-root nodes. The Bevy
//!    pipeline's rotational dynamics, gravity-gradient torque, and
//!    SRP / aero torques integrate *every* `DynamicsConfigC`-bearing
//!    entity using its own `MassPropertiesC.inverse_inertia` (not
//!    just the integration root), so a non-root with a zero
//!    `inverse_inertia` cache would silently drop its torque
//!    response. Mass-zero nodes get `DMat3::ZERO`, matching JEOD's
//!    `Matrix3x3::invert_symmetric` fall-through. The integration
//!    root receives the same value the arena would have produced
//!    via its second invert in `mass_update.cc:116-125`, so behaviour
//!    is bit-equivalent for the runner's existing call sites.
//!
//! Storage backends supply the *core* properties via
//! [`MassNodeView`]; the kernel returns the recomputed *composite*
//! properties via [`MassNodeOutputs`]. Mutation of underlying storage
//! (i.e. how the new composite values land back in the arena / how
//! they are written into Bevy components) stays at the call site —
//! the trait is read-only on purpose, mirroring the
//! `FrameStorage::state` shape.

use core::fmt::Debug;
use core::hash::Hash;
use std::collections::{HashMap, HashSet};

use glam::{DMat3, DVec3};

use crate::mass::MassProperties;
use crate::mass_body::{point_mass_inertia, MassBodyId, MassPointState, MassTree};

/// Read-only view of one node in a mass tree, as needed by the
/// composition kernel.
///
/// Mirrors the subset of [`crate::mass_body::MassBody`] the JEOD
/// algorithm actually consults: the body's *core* properties, the
/// `structure_point` (offset + rotation in the parent's structural
/// frame), and a string name used solely for diagnostic messages
/// (singular-inertia panic, cycle-detection diagnostics).
#[derive(Debug, Clone, Copy)]
pub struct MassNodeView<'a> {
    /// This body's core mass / inertia / CoM. Inertia is about the
    /// body axes through the core CoM; position is the core CoM in
    /// the body's own structural frame.
    pub core: MassProperties,
    /// Position + rotation of this body's structural origin in the
    /// **parent's** structural frame. Ignored for roots (the kernel
    /// never reads it for a root node).
    pub structure_point: MassPointState,
    /// Diagnostic name. Used only in panic messages — backends should
    /// supply something meaningful (`"CM"`, `Entity { … }`, …) but
    /// the kernel never compares names.
    pub name: &'a str,
}

/// Output of a single-node composition step.
///
/// The kernel produces this for every node in post-order; the call
/// site decides how to write it back into the underlying storage
/// (mutate the arena `composite_properties` field, or pipe into a
/// Bevy `MassPropertiesC` component, etc.).
#[derive(Debug, Clone, Copy)]
pub struct MassNodeOutputs {
    /// Recomputed composite mass / inertia / CoM. Both `inverse_*`
    /// caches are populated for every mass-bearing node (roots,
    /// internal nodes, atomic leaves) whenever `mass > 0`. Mass-zero
    /// nodes get `DMat3::ZERO` for `inverse_inertia` and `0.0` for
    /// `inverse_mass`, matching JEOD's
    /// `Matrix3x3::invert_symmetric` fall-through. See
    /// [`recompute_composites_via_storage`] for the full rule and
    /// rationale (Bevy non-root rotational dynamics need their own
    /// `inverse_inertia`, so per-node inversion is mandatory and not
    /// merely a convenience).
    pub composite: MassProperties,
    /// `core_wrt_composite`: core CoM minus composite CoM, both in
    /// this body's structural frame. Default for atomic nodes.
    pub core_wrt_composite: MassPointState,
    /// `composite_wrt_pstr`: this node's composite CoM expressed in
    /// the *parent's* structural frame, plus `t_parent_this` copied
    /// from `structure_point`. Default for roots.
    pub composite_wrt_pstr: MassPointState,
}

impl Default for MassNodeOutputs {
    fn default() -> Self {
        Self {
            composite: MassProperties::new(1.0),
            core_wrt_composite: MassPointState::default(),
            composite_wrt_pstr: MassPointState::default(),
        }
    }
}

/// Storage-agnostic read view of a mass tree.
///
/// Implementations supply parent links and a per-node
/// [`MassNodeView`]; the [`recompute_composites_via_storage`] kernel
/// drives composition without knowing whether the storage is an
/// arena, an ECS world, or something else. Designed to mirror the
/// `FrameStorage` trait sketched in the design doc Section 7.
///
/// The trait is intentionally read-only — write-back happens at the
/// call site (the [`MassTree`] arena impl uses
/// [`MassTree::recompute_composites`]; the Bevy adapter's
/// `composite_mass_system` writes outputs into `MassPropertiesC`).
pub trait MassStorage {
    /// Backend-specific node identifier (`MassBodyId` for the arena,
    /// `Entity` for the Bevy adapter).
    type Id: Copy + Eq + Hash + Debug;

    /// Direct parent of `id`, or `None` for a root.
    fn parent(&self, id: Self::Id) -> Option<Self::Id>;

    /// Core properties + structure-point view for `id`.
    fn node(&self, id: Self::Id) -> MassNodeView<'_>;

    /// Direct children of `id`. Order matters only for diagnostic
    /// reproducibility — composition is associative + commutative
    /// over Steiner-shifted child contributions.
    ///
    /// Returns a borrowed slice into the backend's already-indexed
    /// child storage so the kernel can iterate without allocating
    /// per node — the previous `Vec<Self::Id>` return forced a heap
    /// allocation on every visit, which scaled with tree size and
    /// added avoidable allocator churn on large mass trees
    /// (PR #283 review thread `PRRT_kwDORtae6c5_KZvX`). Both the
    /// arena ([`MassTree::children`]) and the Bevy view
    /// (`MassTreeView::children` in `astrodyn_bevy::mass_tree`) already
    /// keep child lists in `Vec<Self::Id>` slabs /
    /// `HashMap<Entity, Vec<Entity>>`, so `&[Self::Id]` is the
    /// natural shape.
    fn children(&self, id: Self::Id) -> &[Self::Id];

    /// Every root in the storage (parents whose `parent(...)` is
    /// `None`). The kernel walks each root post-order to honour the
    /// "leaves first" invariant.
    fn roots(&self) -> Vec<Self::Id>;

    /// Total number of nodes in the storage. Used by the kernel to
    /// detect cycles / orphans: after a roots-only walk, we assert
    /// that every node was reached (`visited.len() == node_count()`)
    /// per the "Fail Loudly" rule. A cycle starves `roots()` of any
    /// entry into the affected subtree, so without this check the
    /// kernel would silently leave stale composites in place.
    fn node_count(&self) -> usize;
}

/// Compute the composite properties of one node, given its core view
/// plus the *already-composed* outputs of its children.
///
/// This is the kernel of `mass_calc_composite_cm.cc` +
/// `mass_calc_composite_inertia.cc` factored so it can be driven by
/// any storage that produces children in post-order.
///
/// `inverse_*` caches: every node with `mass > 0` gets
/// `inverse_inertia = inertia.inverse()` (and `inverse_mass = 1/mass`)
/// recomputed *uniformly* from the live `mass`/`inertia`, including
/// at atomic leaves. The arena's `MassTree::update_node` copies
/// `core_properties` verbatim for leaves and re-inverts only at
/// internal nodes / the root, but the Bevy adapter also needs the
/// leaf branch to refresh its caches because mission code can edit
/// a leaf's `MassPropertiesC` mid-tick (mutating `mass`/`inertia`
/// without yet running `mass_update_system`) and immediately call
/// `MassTreeQueries::recompute_composites()` /
/// `build_view()`. Without the leaf-side recompute the output would
/// keep the stale `inverse_*` from before the edit (PR #283 review
/// thread `PRRT_kwDORtae6c5_K7p_`). Internal nodes already had this
/// behaviour via `calc_composite_inertia`; the root's second invert
/// in `mass_update.cc:116-125` is bit-equivalent to ours, so the
/// uniform leaf-side recompute is a strict superset of the arena's
/// guarantees. Mass-zero nodes get `DMat3::ZERO`, matching JEOD's
/// `Matrix3x3::invert_symmetric` fall-through.
///
/// We must invert at every node — not just roots — because the Bevy
/// pipeline integrates *every* `DynamicsConfigC`-bearing entity using
/// its own `MassPropertiesC.inverse_inertia` (rotational dynamics,
/// SRP/aero torques). Zeroing it on non-roots silently drops the
/// torque response of any attached body, which is the
/// silently-wrong-trajectory failure mode forbidden by CLAUDE.md
/// "Fail Loudly".
///
/// `is_root` is retained for API symmetry with the post-order driver
/// but no longer changes behaviour; the inversion rule is uniform.
///
/// Panics if the composite inertia is singular (matches the existing
/// arena diagnostic in the arena's `calc_composite_inertia`). The panic
/// message names the body so a mission engineer can diagnose which
/// attachment introduced the degeneracy.
// JEOD_INV: MA.06 — bottom-up mass property update (children first; the kernel takes pre-composed children)
// JEOD_INV: MA.07 — derived quantities recomputed (output composite has fresh inverse caches at every node)
pub fn compute_node_composite(
    node: MassNodeView<'_>,
    children: &[MassNodeOutputs],
    _is_root: bool,
) -> MassNodeOutputs {
    if children.is_empty() {
        // Atomic body: composite == core (JEOD mass_update.cc:59-75).
        // We recompute `inverse_mass`/`inverse_inertia` from the
        // live `mass`/`inertia` rather than copying them verbatim
        // off `node.core`, so a mid-tick edit to a leaf's
        // `MassPropertiesC` (mission code mutates `mass`/`inertia`
        // and sets `dirty = true`, but the scheduled
        // `mass_update_system` hasn't yet refreshed the inverses)
        // does not leak stale `inverse_*` through
        // `MassTreeQueries::recompute_composites()` /
        // `build_view()` (PR #283 review thread
        // `PRRT_kwDORtae6c5_K7p_`). The non-leaf branch already
        // recomputes the inverses unconditionally at every internal
        // node; doing the same here makes the kernel's contract
        // uniform — *every* output node has fresh derived caches
        // that match its `mass`/`inertia`, and `MA.07` ("derived
        // quantities recomputed") holds for atomic leaves too.
        //
        // Mass-zero leaves get `DMat3::ZERO` for `inverse_inertia`
        // and `0.0` for `inverse_mass`, matching the non-leaf
        // branch and JEOD's `Matrix3x3::invert_symmetric`
        // fall-through.
        let mut composite = node.core;
        if composite.mass > 0.0 {
            let det = composite.inertia.determinant();
            // Same diagnostic shape as the non-leaf path; names the
            // body so a mission engineer can locate the offending
            // edit per the "Fail Loudly" rule.
            assert!(
                det.abs() > 1e-30,
                "Body '{}' has singular core inertia (det={det:.2e}); \
                 check the mid-tick mass edit that produced this leaf core.",
                node.name
            );
            composite.inverse_mass = 1.0 / composite.mass;
            composite.inverse_inertia = composite.inertia.inverse();
        } else {
            composite.inverse_mass = 0.0;
            composite.inverse_inertia = DMat3::ZERO;
        }
        return MassNodeOutputs {
            composite,
            core_wrt_composite: MassPointState::default(),
            composite_wrt_pstr: MassPointState::default(),
        };
    }

    // 1. Composite CoM (mass-weighted): core + Σ child_composite (in
    //    this body's structural frame).
    let mut total_mass = node.core.mass;
    let mut weighted_pos = node.core.position * node.core.mass;
    for child in children {
        total_mass += child.composite.mass;
        weighted_pos += child.composite_wrt_pstr.position * child.composite.mass;
    }
    let cm = if total_mass > 0.0 {
        weighted_pos / total_mass
    } else {
        DVec3::ZERO
    };

    // 2. Composite inertia: core shifted to composite CoM + each
    //    rotated child shifted by its offset to the composite CoM.
    let core_offset = node.core.position - cm;
    let mut composite_inertia = node.core.inertia + point_mass_inertia(node.core.mass, core_offset);
    for child in children {
        let child_offset = child.composite_wrt_pstr.position - cm;
        let t = child.composite_wrt_pstr.t_parent_this;
        // Rotate child's composite inertia from child structural frame to
        // this body's structural frame:  T^T · I_child · T.
        let rotated_inertia = t.transpose() * child.composite.inertia * t;
        composite_inertia +=
            rotated_inertia + point_mass_inertia(child.composite.mass, child_offset);
    }

    let inverse_mass = if total_mass > 0.0 {
        1.0 / total_mass
    } else {
        0.0
    };

    // 3. inverse_inertia: invert at every internal node when mass > 0
    //    (matches arena `MassTree::calc_composite_inertia` —
    //    `mass_calc_composite_inertia.cc` lines 86-94 and the
    //    arena's `dm_invert_symm.cc` fall-through). The JEOD
    //    integration root receives a *second* invert in
    //    `mass_update.cc:116-125` against the same matrix; the
    //    result is identical, so a single uniform inversion is
    //    bit-equivalent to the arena's behaviour.
    let inverse_inertia = if total_mass > 0.0 {
        let det = composite_inertia.determinant();
        // JEOD's Matrix3x3::invert_symmetric (dm_invert_symm.cc:86-94)
        // checks for a zero determinant; we panic with a diagnostic
        // that names the body per the "Fail Loudly" rule.
        assert!(
            det.abs() > 1e-30,
            "Body '{}' has singular composite inertia (det={det:.2e}); \
             check mass-tree attach offsets and child inertias.",
            node.name
        );
        composite_inertia.inverse()
    } else {
        DMat3::ZERO
    };

    let composite = MassProperties {
        mass: total_mass,
        inverse_mass,
        inertia: composite_inertia,
        inverse_inertia,
        position: cm,
        // composite.t_parent_this remains the core's struct→body
        // rotation; JEOD propagates this through composite_properties
        // (see `MassBody::update_mass_properties`).
        t_parent_this: node.core.t_parent_this,
        dirty: false,
    };

    let core_wrt_composite = MassPointState {
        position: node.core.position - cm,
        t_parent_this: DMat3::IDENTITY,
    };

    MassNodeOutputs {
        composite,
        core_wrt_composite,
        // composite_wrt_pstr is filled in by the caller — it's a
        // function of the parent's structural frame, which the kernel
        // doesn't see. The driver fills it in before passing this
        // record to the parent.
        composite_wrt_pstr: MassPointState::default(),
    }
}

/// Compute `composite_wrt_pstr` for a child given its post-composition
/// outputs and its `structure_point`.
///
/// JEOD `mass_update.cc:137-143`:
/// ```text
/// composite_wrt_pstr.position =
///     T_parent_this^T · composite_properties.position
///     + structure_point.position
/// composite_wrt_pstr.t_parent_this = structure_point.t_parent_this
/// ```
///
/// This shape lets a driver finish the kernel-emitted
/// [`MassNodeOutputs`] before handing it to the parent.
pub fn finalize_child_in_parent_frame(
    child: &mut MassNodeOutputs,
    structure_point: &MassPointState,
) {
    let t = structure_point.t_parent_this;
    let comp_pos = child.composite.position;
    child.composite_wrt_pstr.position = t.transpose() * comp_pos + structure_point.position;
    child.composite_wrt_pstr.t_parent_this = structure_point.t_parent_this;
}

/// Drive [`compute_node_composite`] in post-order over every root in
/// the storage and return the outputs keyed by node id.
///
/// Storage callers handle write-back themselves (the arena impl
/// mutates `MassTree::nodes[id].composite_properties`; the Bevy
/// adapter writes into `MassPropertiesC`). The kernel only computes
/// — it never mutates.
///
/// Returned vector is in post-order (children before parents) so a
/// caller that needs to write back in dependency-respecting order
/// can iterate in sequence.
///
/// Complexity is `O(N)` in the number of nodes: child output lookup
/// uses a [`HashMap`] keyed by `S::Id`, and the visited set uses a
/// [`HashSet`] for `O(1)` membership (previously both were `Vec`
/// scans, which made the walk `O(N²)` and regressed large articulated
/// vehicles vs the arena's linear-time recomputation).
///
/// Panics with a "Fail Loudly" diagnostic if the walk reaches fewer
/// than [`MassStorage::node_count`] nodes — this catches `MassChildOf`
/// cycles, children whose parent isn't reachable from any root, or
/// other invalid topologies that would otherwise leave stale
/// composites silently in place. The arena `MassTree::attach` path
/// already rejects same-tree attachments at construction; this kernel
/// is the equivalent failsafe for the Bevy adapter, where mission
/// code edits relations directly through `Commands`.
pub fn recompute_composites_via_storage<S: MassStorage>(
    storage: &S,
) -> Vec<(S::Id, MassNodeOutputs)> {
    let roots = storage.roots();
    let expected = storage.node_count();
    let mut out: Vec<(S::Id, MassNodeOutputs)> = Vec::with_capacity(expected);
    let mut index: HashMap<S::Id, usize> = HashMap::with_capacity(expected);
    let mut seen: HashSet<S::Id> = HashSet::with_capacity(expected);
    for root in roots {
        walk(storage, root, true, &mut out, &mut index, &mut seen);
    }
    // Cycle / orphan detection: every node must have been visited.
    // A `MassChildOf` cycle has no root, so the affected subtree is
    // never entered; a child whose parent points to an unreachable
    // node is similarly stranded. Panic with a diagnostic that
    // names how many nodes were missed so the caller can find the
    // broken edge — per CLAUDE.md "Fail Loudly".
    assert!(
        seen.len() == expected,
        "MassStorage topology has a cycle or orphan: {} of {} nodes unreachable from roots(). \
         Check MassChildOf edges for cycles, missing parents, or detached subtrees.",
        expected.saturating_sub(seen.len()),
        expected
    );
    out
}

fn walk<S: MassStorage>(
    storage: &S,
    id: S::Id,
    is_root: bool,
    out: &mut Vec<(S::Id, MassNodeOutputs)>,
    index: &mut HashMap<S::Id, usize>,
    seen: &mut HashSet<S::Id>,
) {
    if !seen.insert(id) {
        return;
    }
    // Borrow the backend's child slice directly (no allocation;
    // PR #283 review thread `PRRT_kwDORtae6c5_KZvX`). Both
    // `MassTree::children` and `MassTreeView::children` return a
    // pre-indexed `&[Self::Id]`, so this loop adds zero allocator
    // pressure as the walk recurses.
    let children = storage.children(id);
    let mut child_outputs: Vec<MassNodeOutputs> = Vec::with_capacity(children.len());
    for &child_id in children {
        walk(storage, child_id, false, out, index, seen);
        // O(1) lookup of the child's pre-finalised output via the
        // index map (replaces the previous tail-scan over `out`,
        // which made the walk O(n²)).
        let slot = *index.get(&child_id).expect(
            "child not pushed by post-order walk: kernel invariant — \
             walk(child) above must always push exactly one (id, outputs) entry",
        );
        let child_view = storage.node(child_id);
        let mut child_out = out[slot].1;
        // Fill in composite_wrt_pstr against this child's
        // structure_point (in *its parent's* structural frame).
        finalize_child_in_parent_frame(&mut child_out, &child_view.structure_point);
        child_outputs.push(child_out);
        // Persist the parent-relative fields back into the stored
        // entry so the caller writing back to storage sees the
        // finalized `composite_wrt_pstr`.
        out[slot].1 = child_out;
    }
    let view = storage.node(id);
    let outputs = compute_node_composite(view, &child_outputs, is_root);
    let slot = out.len();
    out.push((id, outputs));
    index.insert(id, slot);
}

// ---------------------------------------------------------------------------
// Arena impl: MassTree
// ---------------------------------------------------------------------------

impl MassStorage for MassTree {
    type Id = MassBodyId;

    fn parent(&self, id: Self::Id) -> Option<Self::Id> {
        MassTree::parent(self, id)
    }

    fn node(&self, id: Self::Id) -> MassNodeView<'_> {
        let body = MassTree::get(self, id);
        MassNodeView {
            core: body.core_properties,
            structure_point: body.structure_point,
            name: &body.name,
        }
    }

    fn children(&self, id: Self::Id) -> &[Self::Id] {
        // Pass through the arena's `&[MassBodyId]` slab directly —
        // no allocation per visit (PR #283 review thread
        // `PRRT_kwDORtae6c5_KZvX`).
        MassTree::children(self, id)
    }

    fn roots(&self) -> Vec<Self::Id> {
        let mut roots = Vec::new();
        for id in 0..self.len() {
            if MassTree::parent(self, id).is_none() {
                roots.push(id);
            }
        }
        roots
    }

    fn node_count(&self) -> usize {
        MassTree::len(self)
    }
}

// ---------------------------------------------------------------------------
// Tests — verify the trait-driven kernel reproduces the arena's
// in-place `recompute_composites` bit-for-bit.
// ---------------------------------------------------------------------------

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

    fn assert_props_close(a: &MassProperties, b: &MassProperties, tol: f64, label: &str) {
        assert!(
            (a.mass - b.mass).abs() < tol,
            "{label}: mass {} vs {}",
            a.mass,
            b.mass
        );
        let dpos = (a.position - b.position).length();
        assert!(dpos < tol, "{label}: position diff {dpos:.3e}");
        for (col_a, col_b) in [
            (a.inertia.x_axis, b.inertia.x_axis),
            (a.inertia.y_axis, b.inertia.y_axis),
            (a.inertia.z_axis, b.inertia.z_axis),
        ] {
            let d = (col_a - col_b).length();
            assert!(d < tol, "{label}: inertia col diff {d:.3e}");
        }
    }

    #[test]
    fn storage_kernel_matches_arena_single_attach() {
        // Single child attached to a root — composite must agree with
        // the arena's in-place recomputation byte-for-byte.
        let mut tree = MassTree::new();
        let parent = tree.add_root(
            "parent".into(),
            MassProperties::with_inertia(
                10.0,
                DMat3::from_diagonal(DVec3::new(50.0, 60.0, 70.0)),
                DVec3::new(0.1, -0.2, 0.0),
            ),
        );
        let child = tree.add_body(
            "child".into(),
            MassProperties::with_inertia(
                4.0,
                DMat3::from_diagonal(DVec3::new(8.0, 9.0, 10.0)),
                DVec3::new(-0.05, 0.0, 0.0),
            ),
        );
        tree.attach(child, parent, DVec3::new(2.0, 0.5, -0.3), DMat3::IDENTITY);

        // Reference: the arena re-runs recompute_composites on attach.
        let arena_parent_composite = tree.get(parent).composite_properties;
        let arena_child_composite = tree.get(child).composite_properties;

        // Kernel via trait.
        let outs = recompute_composites_via_storage(&tree);
        let kernel_parent = outs.iter().find(|(id, _)| *id == parent).unwrap().1;
        let kernel_child = outs.iter().find(|(id, _)| *id == child).unwrap().1;

        assert_props_close(
            &arena_parent_composite,
            &kernel_parent.composite,
            1e-12,
            "parent composite",
        );
        // Non-root entries: kernel matches the arena bit-for-bit on
        // mass / position. Inverse-inertia is preserved at every
        // node (post-#283-review), so we also check that
        // I · I^-1 ≈ identity for the child when mass > 0.
        assert!((kernel_child.composite.mass - arena_child_composite.mass).abs() < 1e-12);
        assert!(
            (kernel_child.composite.position - arena_child_composite.position).length() < 1e-12
        );
        if kernel_child.composite.mass > 0.0 {
            let identity = kernel_child.composite.inertia * kernel_child.composite.inverse_inertia;
            for r in 0..3 {
                for c in 0..3 {
                    let expected = if r == c { 1.0 } else { 0.0 };
                    assert!(
                        (identity.col(r)[c] - expected).abs() < 1e-9,
                        "child I·I^-1 not identity at [{r},{c}]: got {}",
                        identity.col(r)[c]
                    );
                }
            }
        }
    }

    #[test]
    fn storage_kernel_matches_arena_three_body_chain() {
        // A → B → C with non-trivial offsets and a 90° rotation on C.
        let mut tree = MassTree::new();
        let a = tree.add_root("A".into(), MassProperties::new(10.0));
        let b = tree.add_body("B".into(), MassProperties::new(5.0));
        let c = tree.add_body(
            "C".into(),
            MassProperties::with_inertia(
                3.0,
                DMat3::from_diagonal(DVec3::new(1.0, 4.0, 9.0)),
                DVec3::ZERO,
            ),
        );

        tree.attach(b, a, DVec3::new(2.0, 0.0, 0.0), DMat3::IDENTITY);
        // 90 deg about Z attaching C to B.
        let rot_z90 = DMat3::from_cols(
            DVec3::new(0.0, 1.0, 0.0),
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );
        tree.attach(c, b, DVec3::new(1.0, 0.0, 0.0), rot_z90);

        let arena_a = tree.get(a).composite_properties;
        let arena_b = tree.get(b).composite_properties;

        let outs = recompute_composites_via_storage(&tree);
        let ka = outs.iter().find(|(id, _)| *id == a).unwrap().1;
        let kb = outs.iter().find(|(id, _)| *id == b).unwrap().1;

        assert_props_close(&arena_a, &ka.composite, 1e-10, "A composite");
        // Non-root B: compare mass / pos / inertia / inverse_inertia
        // — post-#283-review, the kernel now preserves
        // inverse_inertia at every node, matching the arena's
        // `calc_composite_inertia`.
        assert!((arena_b.mass - kb.composite.mass).abs() < 1e-12);
        assert!((arena_b.position - kb.composite.position).length() < 1e-12);
        for (ca, cb) in [
            (arena_b.inertia.x_axis, kb.composite.inertia.x_axis),
            (arena_b.inertia.y_axis, kb.composite.inertia.y_axis),
            (arena_b.inertia.z_axis, kb.composite.inertia.z_axis),
        ] {
            let d = (ca - cb).length();
            assert!(d < 1e-10, "B inertia diff {d:.3e}");
        }
        for (ca, cb) in [
            (
                arena_b.inverse_inertia.x_axis,
                kb.composite.inverse_inertia.x_axis,
            ),
            (
                arena_b.inverse_inertia.y_axis,
                kb.composite.inverse_inertia.y_axis,
            ),
            (
                arena_b.inverse_inertia.z_axis,
                kb.composite.inverse_inertia.z_axis,
            ),
        ] {
            let d = (ca - cb).length();
            assert!(d < 1e-10, "B inverse_inertia diff {d:.3e}");
        }
    }

    #[test]
    fn storage_children_returns_borrowed_slice() {
        // PR #283 review thread `PRRT_kwDORtae6c5_KZvX`: the
        // `MassStorage::children` contract is `&[Self::Id]` (a
        // borrow into the backend's existing index), not an owned
        // `Vec`. Pin the contract so a future regression that
        // reverts to `.cloned().to_vec()` etc. would fail to compile
        // here. The arena `MassTree::children` already stores a
        // `Vec<MassBodyId>` per node, so passing it through is
        // zero-allocation per visit.
        let mut tree = MassTree::new();
        let parent = tree.add_root("parent".into(), MassProperties::new(10.0));
        let child_a = tree.add_body("child_a".into(), MassProperties::new(2.0));
        let child_b = tree.add_body("child_b".into(), MassProperties::new(3.0));
        tree.attach(child_a, parent, DVec3::new(1.0, 0.0, 0.0), DMat3::IDENTITY);
        tree.attach(child_b, parent, DVec3::new(0.0, 1.0, 0.0), DMat3::IDENTITY);

        // Compile-time shape check: the value returned binds to
        // `&[MassBodyId]`, not an owned `Vec<MassBodyId>`. The
        // pointer-equality check below would fail to compile if
        // `children` ever returned `Vec<_>` (a temporary's address
        // would change between calls).
        let slice_a: &[MassBodyId] = <MassTree as MassStorage>::children(&tree, parent);
        let slice_b: &[MassBodyId] = <MassTree as MassStorage>::children(&tree, parent);
        assert_eq!(slice_a.len(), 2);
        // Two consecutive calls borrow the *same* backend slab; if
        // the trait silently re-introduced a `Vec::clone()` wrapper,
        // the addresses would differ.
        assert!(
            std::ptr::eq(slice_a.as_ptr(), slice_b.as_ptr()),
            "MassStorage::children re-allocates between calls — \
             trait must return a borrow into the backend, not an owned Vec"
        );
    }

    #[test]
    fn storage_kernel_chain_of_100_recomposes() {
        // PR #283 review thread `PRRT_kwDORtae6c5_KZvX`: composing
        // a 100-body chain exercises the iterator/slice path
        // through 100 child-list visits. With the old
        // `Vec<Self::Id>` return there were 100 heap allocations
        // (one per `walk` visit); after the fix every visit borrows
        // into the backend's existing slab, so allocator pressure
        // is bounded by the kernel's own bookkeeping (the
        // `out` / `index` / `seen` collections, each grown once).
        //
        // We don't install a counting allocator — that would
        // require a global hook that conflicts with the rest of
        // the crate's tests — but the trait return type
        // (`&[Self::Id]`) statically guarantees the per-visit
        // allocation cannot reappear without changing the trait
        // signature, which is a breaking change reviewers will
        // notice. The test pins correctness on the tall chain:
        // composite mass at the root must equal the sum of all
        // 100 core masses (1.0 each => 100.0).
        let mut tree = MassTree::new();
        let mut prev = tree.add_root("body_0".into(), MassProperties::new(1.0));
        let mut bodies = vec![prev];
        for i in 1..100 {
            let body = tree.add_body(format!("body_{i}"), MassProperties::new(1.0));
            tree.attach(body, prev, DVec3::new(0.1, 0.0, 0.0), DMat3::IDENTITY);
            bodies.push(body);
            prev = body;
        }
        // Drive the trait-driven kernel; if any `children()` call
        // panics, allocates unexpectedly, or the post-order walk
        // misses a body, the cycle / orphan detection in
        // `recompute_composites_via_storage` will fire.
        let outs = recompute_composites_via_storage(&tree);
        assert_eq!(outs.len(), 100, "kernel must visit every body");
        let root_idx = outs.iter().position(|(id, _)| *id == bodies[0]).unwrap();
        let root_composite_mass = outs[root_idx].1.composite.mass;
        assert!(
            (root_composite_mass - 100.0).abs() < 1e-9,
            "root composite mass: expected 100.0, got {root_composite_mass}"
        );
    }

    #[test]
    fn storage_kernel_atomic_leaf_refreshes_stale_inverses() {
        // PR #283 review thread `PRRT_kwDORtae6c5_K7p_`: mission code
        // edits a leaf's `MassPropertiesC` mid-tick (mutating
        // `mass`/`inertia` and setting `dirty`) and immediately
        // calls `MassTreeQueries::recompute_composites()` /
        // `build_view()` before the scheduled `mass_update_system`
        // has refreshed the inverses. The kernel's atomic-leaf
        // branch must recompute `inverse_mass`/`inverse_inertia`
        // from the live `mass`/`inertia` rather than copying the
        // stale caches off `node.core` — otherwise the output keeps
        // the old derived values and downstream rotational dynamics
        // (Euler equation, gravity-gradient torque) integrate against
        // stale inverses for one step.
        //
        // Construct a leaf whose `mass`/`inertia` describe one body
        // but whose `inverse_*` caches are explicitly seeded to a
        // *different* (stale) body. After running the kernel the
        // composite must satisfy `mass · inverse_mass ≈ 1` and
        // `I · I^-1 ≈ identity` against the *live* `mass`/`inertia`,
        // not the stale caches.
        let live_mass = 12.0_f64;
        let live_inertia = DMat3::from_diagonal(DVec3::new(40.0, 50.0, 60.0));

        // Stale caches from a previous mass / inertia (e.g. before a
        // fuel-burn edit reduced `mass` from 20 to 12).
        let stale_inverse_mass = 1.0 / 20.0;
        let stale_inertia = DMat3::from_diagonal(DVec3::new(80.0, 100.0, 120.0));
        let stale_inverse_inertia = stale_inertia.inverse();

        let leaf_core = MassProperties {
            mass: live_mass,
            inverse_mass: stale_inverse_mass,
            inertia: live_inertia,
            inverse_inertia: stale_inverse_inertia,
            position: DVec3::ZERO,
            t_parent_this: DMat3::IDENTITY,
            dirty: true,
        };
        let view = MassNodeView {
            core: leaf_core,
            structure_point: MassPointState::default(),
            name: "midtick_leaf",
        };
        let out = compute_node_composite(view, &[], true);
        let composite = out.composite;

        assert!(
            (composite.mass - live_mass).abs() < 1e-12,
            "leaf composite mass: expected {live_mass}, got {}",
            composite.mass
        );
        assert!(
            (composite.mass * composite.inverse_mass - 1.0).abs() < 1e-12,
            "leaf inverse_mass not refreshed: {} * {} = {} (expected 1.0)",
            composite.mass,
            composite.inverse_mass,
            composite.mass * composite.inverse_mass
        );
        let identity = composite.inertia * composite.inverse_inertia;
        for r in 0..3 {
            for c in 0..3 {
                let expected = if r == c { 1.0 } else { 0.0 };
                assert!(
                    (identity.col(r)[c] - expected).abs() < 1e-9,
                    "leaf I·I^-1 not identity at [{r},{c}]: got {} \
                     (kernel returned stale inverse_inertia from `node.core`)",
                    identity.col(r)[c]
                );
            }
        }
        // Conversely, the stale inverse_inertia must NOT have been
        // copied through verbatim: if it had, multiplying by the
        // live inertia would not give identity (because the stale
        // inverse was inverted from a different inertia).
        let stale_product = composite.inertia * stale_inverse_inertia;
        assert!(
            (stale_product - DMat3::IDENTITY).x_axis.length() > 0.1,
            "test setup invalid: stale inverse happens to satisfy \
             I·I^-1 ≈ identity against the live inertia"
        );
    }

    #[test]
    fn storage_kernel_atomic_root_round_trip() {
        // Single-node tree: composite must equal core, inverse caches
        // unchanged.
        let mut tree = MassTree::new();
        let r = tree.add_root(
            "alone".into(),
            MassProperties::with_inertia(
                7.0,
                DMat3::from_diagonal(DVec3::new(20.0, 30.0, 40.0)),
                DVec3::new(0.0, 0.1, 0.0),
            ),
        );
        let outs = recompute_composites_via_storage(&tree);
        let k = outs.iter().find(|(id, _)| *id == r).unwrap().1;
        let arena = tree.get(r).composite_properties;
        assert_props_close(&arena, &k.composite, 1e-12, "atomic root");
    }
}