astrodyn_dynamics 0.1.1

//! Arena-based mass tree: a faithful port of JEOD's MassBody hierarchy.
//!
//! JEOD models rigid-body mass as a tree of `MassBody` nodes. Each node has
//! *core* properties (the body alone) and *composite* properties (this body
//! plus all descendants). Composite center of mass and inertia are recomputed
//! bottom-up after every attach/detach using the algorithms from
//! `mass_calc_composite_cm.cc` and `mass_calc_composite_inertia.cc`.
//!
//! This module is pure Rust with zero Bevy dependency.

use glam::{DMat3, DVec3};

use crate::mass::MassProperties;

/// Handle into the [`MassTree`] arena.
pub type MassBodyId = usize;

// ---------------------------------------------------------------------------
// MassPointState
// ---------------------------------------------------------------------------

/// Position and orientation of a mass point relative to a parent frame.
///
/// Maps to JEOD's `MassPointState`: `position` is an offset in the parent
/// structural frame, and `t_parent_this` is the rotation matrix from the
/// parent structural frame to this body's frame.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MassPointState {
    /// Offset in parent's structural frame (m).
    pub position: DVec3,
    /// Rotation from parent structural frame to this body frame.
    pub t_parent_this: DMat3,
}

impl Default for MassPointState {
    fn default() -> Self {
        Self {
            position: DVec3::ZERO,
            t_parent_this: DMat3::IDENTITY,
        }
    }
}

// ---------------------------------------------------------------------------
// MassPoint (named attachment point)
// ---------------------------------------------------------------------------

/// Named attachment point on a body, used for docking-style attachments.
///
/// Maps to JEOD's `MassPoint` / `MassPointInit`: each body can have zero or
/// more named points (e.g., "CM docking port", "SM interface") that define
/// a position and orientation within the body's structural frame. The
/// [`MassTree::attach_aligned`] method uses these to compute the structural
/// offset and rotation between two bodies when they dock.
#[derive(Debug, Clone)]
pub struct MassPoint {
    /// Human-readable name (e.g., "CM docking port").
    pub name: String,
    /// Position in the body's structural frame (m).
    pub position: DVec3,
    /// Rotation from structural frame to this point's frame.
    pub t_parent_this: DMat3,
}

// ---------------------------------------------------------------------------
// MassBody
// ---------------------------------------------------------------------------

/// A single node in the mass tree.
///
/// Mirrors JEOD's `MassBody` data members. `core_properties` are this body
/// alone; `composite_properties` include all descendants.
#[derive(Debug, Clone)]
pub struct MassBody {
    /// Human-readable name.
    pub name: String,
    /// Mass properties of this body alone.
    pub core_properties: MassProperties,
    /// Mass properties of this body plus all attached children.
    pub composite_properties: MassProperties,
    /// Attachment point: position and orientation of this body's structural
    /// frame origin in the **parent's** structural frame.
    pub structure_point: MassPointState,
    /// Core CoM offset from composite CoM (in structural frame coords).
    pub core_wrt_composite: MassPointState,
    /// Composite CoM position in the **parent's** structural frame.
    pub composite_wrt_pstr: MassPointState,
    /// Named attachment points on this body (JEOD `MassPoint` list).
    pub mass_points: Vec<MassPoint>,
}

// ---------------------------------------------------------------------------
// MassTree
// ---------------------------------------------------------------------------

/// Arena-based mass tree. Portable, no ECS dependency.
///
/// Bodies are stored in a flat `Vec`; parent/child relationships are tracked
/// with parallel vectors of `Option<MassBodyId>` and `Vec<MassBodyId>`.
pub struct MassTree {
    nodes: Vec<MassBody>,
    parent: Vec<Option<MassBodyId>>,
    children: Vec<Vec<MassBodyId>>,
}

impl MassTree {
    /// Create an empty tree.
    pub fn new() -> Self {
        Self {
            nodes: Vec::new(),
            parent: Vec::new(),
            children: Vec::new(),
        }
    }

    // -- accessors ----------------------------------------------------------

    /// Borrow a body by id.
    pub fn get(&self, id: MassBodyId) -> &MassBody {
        &self.nodes[id]
    }

    /// Mutably borrow a body by id.
    pub fn get_mut(&mut self, id: MassBodyId) -> &mut MassBody {
        &mut self.nodes[id]
    }

    /// Parent of the given body, or `None` for a root.
    pub fn parent(&self, id: MassBodyId) -> Option<MassBodyId> {
        self.parent[id]
    }

    /// Direct children of the given body.
    pub fn children(&self, id: MassBodyId) -> &[MassBodyId] {
        &self.children[id]
    }

    /// Number of bodies in the arena.
    pub fn len(&self) -> usize {
        self.nodes.len()
    }

    /// True when no bodies have been added yet.
    pub fn is_empty(&self) -> bool {
        self.nodes.is_empty()
    }

    /// All bodies whose composite mass properties depend on the body
    /// `id` — i.e. `id` itself plus every ancestor up to the root, in
    /// child→root order.
    ///
    /// Used at attach / detach call sites (runner `Simulation::attach` /
    /// `detach` / `detach_subtree`, Bevy `staging_system`) to mark the
    /// integrator state of every affected body topology-dirty, since
    /// `attach` / `detach` recompute composites all the way to the root
    /// (`recompute_composites` walks every node's tree post-order). A
    /// caller that only marked the immediate parent / former parent
    /// would silently leave intermediate ancestors integrating against
    /// stale predictor history (`JEOD_INV: IG.37`).
    pub fn ancestors_inclusive(&self, id: MassBodyId) -> Vec<MassBodyId> {
        let mut out = Vec::new();
        let mut cur = Some(id);
        while let Some(node) = cur {
            out.push(node);
            cur = self.parent[node];
        }
        out
    }

    /// Every body in `id`'s subtree, **including `id` itself**, in
    /// unspecified order (current implementation is depth-first via a
    /// LIFO `Vec`).
    ///
    /// Used by attach/detach call sites that need to know the full set
    /// of bodies whose integrator state goes stale on a topology change
    /// — e.g. when re-rooting a non-root subject (JEOD's chained-attach
    /// `attach_to_3` while already attached to veh2), every body in the
    /// subject's old subtree changes parent-composite and must be
    /// flagged dirty (JEOD_INV: IG.37). All current callers consume the
    /// result as a set (sort+dedup, or `HashSet`-style membership), so
    /// the traversal order is not part of the public contract.
    pub fn subtree_ids(&self, id: MassBodyId) -> Vec<MassBodyId> {
        let mut out = Vec::new();
        let mut stack = vec![id];
        while let Some(node) = stack.pop() {
            out.push(node);
            for &c in &self.children[node] {
                stack.push(c);
            }
        }
        out
    }

    /// Walk up from `id` to the root of its tree, returning the root id.
    /// Returns `id` itself when `id` has no parent.
    ///
    /// Mirrors JEOD `MassBody::get_root_body_internal()`
    /// (`models/dynamics/mass/src/mass.cc`), which the JEOD attach
    /// machinery calls before attaching a non-root child to a new
    /// parent (`mass_attach.cc:506-567`'s `attach_child`): the actual
    /// thing that re-roots is the *root* of the subject's existing
    /// tree, not the subject itself.
    pub fn root_of(&self, id: MassBodyId) -> MassBodyId {
        let mut cur = id;
        while let Some(p) = self.parent[cur] {
            cur = p;
        }
        cur
    }

    /// Compose the geometric struct-frame chain from `root_id` down to
    /// `descendant_id`, returning a [`MassPointState`] that holds the
    /// position of the descendant's structural origin **in the root's
    /// structural frame** plus the rotation **from the root's structural
    /// frame to the descendant's structural frame**.
    ///
    /// `descendant_id` must be a descendant of `root_id` (or equal to
    /// it). Walks the parent chain via [`Self::parent`]; each link's
    /// [`MassBody::structure_point`] holds that node's struct-origin
    /// offset in its parent struct frame plus the rotation from parent
    /// struct to this struct.
    ///
    /// Mirrors JEOD `MassPoint::compute_state_wrt_pred`
    /// (`models/dynamics/mass/src/mass_point_state.cc`), specialised to
    /// the structural-frame chain that backs [`Self::attach_with_reroot`].
    /// Pure geometry — no kinematics, no time, no ω — exactly what
    /// JEOD's `dyn_body_attach.cc:553-567` reroot path uses.
    ///
    /// # Panics
    /// Panics if `descendant_id` is not a descendant of `root_id`.
    pub fn struct_chain_to_root(
        &self,
        root_id: MassBodyId,
        descendant_id: MassBodyId,
    ) -> MassPointState {
        // Build the path from descendant up to root (exclusive of root).
        let mut chain = Vec::<MassBodyId>::new();
        let mut cur = descendant_id;
        while cur != root_id {
            chain.push(cur);
            cur = self.parent[cur].unwrap_or_else(|| {
                panic!(
                    "struct_chain_to_root: body {} (\"{}\") is not a descendant of {} (\"{}\")",
                    descendant_id,
                    self.nodes[descendant_id].name,
                    root_id,
                    self.nodes[root_id].name,
                )
            });
        }
        // chain now lists descendant, ..., immediate_child_of_root (in
        // descendant→root order). Reverse so we walk root→descendant.
        chain.reverse();

        // Compose: walk down from root, accumulating a single
        // (position, t_parent_this) pair that maps root struct → current
        // struct.
        let mut pos = DVec3::ZERO;
        let mut t = DMat3::IDENTITY;
        for &node in &chain {
            let sp = &self.nodes[node].structure_point;
            // current_struct_position_in_root = current + t.transpose() * node.position
            //   where `t` is currently `T_root_to_parent` and `node.position`
            //   is the node's struct origin in its parent struct frame.
            pos += t.transpose() * sp.position;
            // T_root_to_node = T_parent_to_node · T_root_to_parent
            t = sp.t_parent_this * t;
        }
        MassPointState {
            position: pos,
            t_parent_this: t,
        }
    }

    /// Attach `child_id` (or its current root) to `parent_id`. When
    /// `child_id` is itself a root the call is bit-identical to
    /// [`Self::attach`]; when `child_id` already has a parent in the
    /// tree this method **re-roots** the subject's existing subtree
    /// under `parent_id`, mirroring JEOD's
    /// `DynBody::attach_child(...)` semantics
    /// (`models/dynamics/dyn_body/src/dyn_body_attach.cc:506-567`).
    ///
    /// `offset` is the subject child's structural-frame origin
    /// expressed in the **parent's** structural frame, and
    /// `t_parent_child` is the parent→child structural-frame rotation
    /// — i.e. the same coordinates [`Self::attach`] takes, and the
    /// same `(offset, T_pstr_to_cstr)` pair JEOD's `BodyAttachAligned
    /// veh1.attach_to_2` resolves to. When the subject is non-root,
    /// the method recomputes the equivalent
    /// `(offset_root_in_parent_struct, T_parent_to_root_struct)` so
    /// that the subject's structural frame ends up where the caller
    /// asked, even though the underlying tree edge runs from
    /// `parent_id` to `root_of(child_id)`.
    ///
    /// Returns the [`MassBodyId`] that actually became the new child of
    /// `parent_id` in the tree. For the root-subject case that's
    /// `child_id`; for the reroot case it's `root_of(child_id)`.
    ///
    /// # Panics
    /// Panics if `parent_id` is itself in the subject's existing
    /// subtree (which would create a cycle), or if `parent_id`'s tree
    /// is the same tree the subject already lives in (JEOD warns and
    /// no-ops in that case; we panic-loud for symmetry with the cycle
    /// check on `attach`).
    // JEOD_INV: BA.12 — chained attach re-roots the subject's existing tree under the new parent
    pub fn attach_with_reroot(
        &mut self,
        child_id: MassBodyId,
        parent_id: MassBodyId,
        offset: DVec3,
        t_parent_child: DMat3,
    ) -> MassBodyId {
        // Subject case A: child is itself a root → no reroot needed,
        // delegate to the plain `attach` (which handles the cycle and
        // self-attach checks already).
        if self.parent[child_id].is_none() {
            self.attach(child_id, parent_id, offset, t_parent_child);
            return child_id;
        }

        // Subject case B: child already has a parent → walk to its
        // existing root, recompute the offset/transform so the
        // *root's* structural frame goes where the subject child's
        // would have gone, and attach the root.
        let child_root = self.root_of(child_id);
        // JEOD_INV: MA.19 — same-tree attachment is forbidden. JEOD
        // `attach_validate_parent` (`mass_attach.cc:373`) compares the
        // *roots* of the two bodies (`parent.get_root_body() ==
        // get_root_body()`); a check that only compared `child_root` to
        // `parent_id` would miss the case where `parent_id` is a
        // non-root member of the subject's existing tree. Detecting
        // same-tree here gives a single fail-loud diagnostic naming the
        // shared root, instead of falling through to `attach`'s cycle
        // walk which fires with a less informative message.
        let parent_root = self.root_of(parent_id);
        assert_ne!(
            parent_root,
            child_root,
            "attach_with_reroot: subject body {} (\"{}\") is already in the same tree as \
             parent {} (\"{}\") (shared root {} \"{}\") — JEOD `attach_validate_parent` \
             warns and no-ops; this port panics fail-loud per CLAUDE.md.",
            child_id,
            self.nodes[child_id].name,
            parent_id,
            self.nodes[parent_id].name,
            child_root,
            self.nodes[child_root].name,
        );

        // Composed geometry: subject_struct_wrt_root_struct.
        let subj_in_root = self.struct_chain_to_root(child_root, child_id);
        // T_pstr_to_rstr = (T_root_to_subj)^T · T_pstr_to_subj
        // (JEOD `dyn_body_attach.cc:559`:
        //   `Matrix3x3::product_left_transpose(child_struct_wrt_root_struct.T_parent_this,
        //                                       T_pstr_to_cstr,
        //                                       T_pstr_to_rstr)`).
        let t_pstr_to_rstr = subj_in_root.t_parent_this.transpose() * t_parent_child;
        // xyz_rstr_wrt_pstr = xyz_cstr_wrt_pstr - T_pstr_to_rstr^T · subj_origin_in_root
        // (JEOD `dyn_body_attach.cc:562`:
        //   `Vector3::transform_transpose_decr(T_pstr_to_rstr,
        //                                       child_struct_wrt_root_struct.position,
        //                                       xyz_rstr_wrt_pstr)`
        //  i.e. xyz_rstr_wrt_pstr -= T_pstr_to_rstr^T · subj_in_root.position).
        let xyz_rstr_wrt_pstr = offset - t_pstr_to_rstr.transpose() * subj_in_root.position;

        self.attach(child_root, parent_id, xyz_rstr_wrt_pstr, t_pstr_to_rstr);
        child_root
    }

    // -- construction -------------------------------------------------------

    /// Add a disconnected body (no parent, no children).
    ///
    /// Composite properties are initialised to match core properties.
    pub fn add_body(&mut self, name: String, core: MassProperties) -> MassBodyId {
        let id = self.nodes.len();
        let body = MassBody {
            name,
            composite_properties: core,
            core_properties: core,
            structure_point: MassPointState::default(),
            core_wrt_composite: MassPointState::default(),
            composite_wrt_pstr: MassPointState::default(),
            mass_points: Vec::new(),
        };
        self.nodes.push(body);
        self.parent.push(None);
        self.children.push(Vec::new());
        id
    }

    /// Add a root body (convenience wrapper around [`add_body`](Self::add_body)).
    pub fn add_root(&mut self, name: String, core: MassProperties) -> MassBodyId {
        self.add_body(name, core)
    }

    // -- mass points -----------------------------------------------------------

    /// Register a named attachment point on a body.
    ///
    /// `position` is the point's location in the body's structural frame.
    /// `t_parent_this` is the rotation from the structural frame to the
    /// point's frame.
    pub fn add_mass_point(
        &mut self,
        body_id: MassBodyId,
        name: impl Into<String>,
        position: DVec3,
        t_parent_this: DMat3,
    ) {
        let name_str: String = name.into();
        // JEOD_INV: MA.10 — mass point names must be non-empty (mass.cc ~line 359)
        assert!(
            !name_str.is_empty(),
            "mass point name must be non-empty (body '{}')",
            self.nodes[body_id].name
        );
        // JEOD_INV: MA.09 — mass point names must be unique per body (mass.cc:359-368)
        assert!(
            self.find_mass_point(body_id, &name_str).is_none(),
            "duplicate mass point name '{}' on body '{}'",
            name_str,
            self.nodes[body_id].name
        );
        self.nodes[body_id].mass_points.push(MassPoint {
            name: name_str,
            position,
            t_parent_this,
        });
    }

    /// Look up a named attachment point on a body.
    pub fn find_mass_point(&self, body_id: MassBodyId, name: &str) -> Option<&MassPoint> {
        self.nodes[body_id]
            .mass_points
            .iter()
            .find(|p| p.name == name)
    }

    /// Attach two bodies via named attachment points (docking-style).
    ///
    /// Port of JEOD `MassBody::attach_to(this_point_name, parent_point_name, parent)`
    /// from `mass_attach.cc:66-136`. The algorithm chains three transforms:
    ///
    /// 1. Invert child point: child_struct → child_point
    /// 2. 180° yaw (docking alignment): child_point → parent_point
    /// 3. Parent point → parent_struct
    ///
    /// The 180° yaw (`T = diag(-1, -1, 1)`) is JEOD's hardcoded docking
    /// convention: two attachment points face each other with opposite X/Y axes.
    ///
    /// # Panics
    ///
    /// Panics if either named point is not found on its body.
    // JEOD_INV: MA.21 — named points must exist on body (MessageHandler::fail in JEOD)
    pub fn attach_aligned(
        &mut self,
        child_id: MassBodyId,
        child_point_name: &str,
        parent_id: MassBodyId,
        parent_point_name: &str,
    ) {
        // Look up both points.
        let child_point = self
            .find_mass_point(child_id, child_point_name)
            .unwrap_or_else(|| {
                panic!(
                    "mass point '{}' not found on body '{}'",
                    child_point_name, self.nodes[child_id].name
                )
            });
        let child_pt_pos = child_point.position;
        let child_pt_t = child_point.t_parent_this;

        let parent_point = self
            .find_mass_point(parent_id, parent_point_name)
            .unwrap_or_else(|| {
                panic!(
                    "mass point '{}' not found on body '{}'",
                    parent_point_name, self.nodes[parent_id].name
                )
            });
        let parent_pt_pos = parent_point.position;
        let parent_pt_t = parent_point.t_parent_this;

        // Step 1: Invert child point (child_struct in child_point frame).
        // JEOD mass_attach.cc:103-106: inv_pos = -(T * pos), inv_T = T^T
        let inv_pos = -(child_pt_t * child_pt_pos);
        let inv_t = child_pt_t.transpose();

        // Step 2: 180° yaw docking alignment (JEOD mass_attach.cc:112-115).
        let t_yaw = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );

        // Step 3: Compose the chain child_struct → child_point → parent_point → parent_struct.
        //
        // Position: walk up from child_struct (origin) through each link.
        //   In child_point frame: inv_pos
        //   In parent_point frame: t_yaw^T * inv_pos (t_yaw is symmetric)
        //   In parent_struct frame: parent_pt_t^T * (above) + parent_pt_pos
        let pos_after_yaw = t_yaw * inv_pos; // t_yaw^T = t_yaw (symmetric)
        let offset = parent_pt_t.transpose() * pos_after_yaw + parent_pt_pos;

        // Rotation: compose T_parent_this along the chain.
        //   parent_struct → parent_point: parent_pt_t
        //   parent_point → child_point:   t_yaw
        //   child_point → child_struct:   inv_t = child_pt_t^T
        //   Total: T_parent_struct_to_child_struct = inv_t * t_yaw * parent_pt_t
        let t_parent_child = inv_t * t_yaw * parent_pt_t;

        self.attach(child_id, parent_id, offset, t_parent_child);
    }

    /// Compute the rigid-body offset that maps a parent reference frame
    /// onto a subject body's **structural** frame given an offset and
    /// rotation expressed at one of the body's named mass points.
    ///
    /// Returns `(offset_pframe_struct, t_pframe_struct)` ready to feed
    /// the runner's `Simulation::attach_to_frame` (or the Bevy
    /// adapter's equivalent), which both expect the offset / rotation
    /// from parent ref-frame to body struct frame.
    ///
    /// Port of JEOD's named-point `DynBody::attach_to_frame`
    /// (`models/dynamics/dyn_body/src/dyn_body_attach.cc:302-365`),
    /// specialised to a single mass-point chain (the only depth used by
    /// the SIM_ref_attach `RUN_ref_attach_pt2pt` scenario, which is the
    /// driving consumer).
    ///
    /// The math is exactly `RefFrameState::decr_right` for the static
    /// (zero-velocity, zero-ω) case:
    ///
    /// ```text
    ///   X_pframe_to_struct = X_pframe_to_cpt - X_struct_to_cpt
    ///   T_pframe_struct    = T_struct_cpt^T · T_pframe_cpt
    ///   x_pframe_struct    = x_pframe_cpt - T_pframe_struct^T · x_struct_cpt
    /// ```
    ///
    /// where `cpt` is the named mass point on the subject body. JEOD's
    /// `BodyAttachAligned` passes `x_pframe_cpt = [0,0,0]` and
    /// `T_pframe_cpt = diag(-1,-1,1)` (the 180° yaw docking convention);
    /// the more general signature here lets callers replicate the
    /// non-aligned named-point variant if they ever need it.
    ///
    /// The subject body's mass-point lookup is one level deep — JEOD's
    /// generalised `compute_state_wrt_pred` walks an arbitrary chain of
    /// nested mass points but the only verif sim that exercises this
    /// path keeps mass points directly on the body's structure. Nested
    /// mass-point chains are deferred until a consumer needs them
    /// (would call out additional structure here without a test to
    /// pin the algebra to).
    ///
    /// # Panics
    /// Panics if `subject_point_name` is not a mass point on
    /// `subject_id`.
    // JEOD_INV: MA.16 — 180° yaw convention for attach-by-point
    // JEOD_INV: MA.21 — named points must exist on body
    pub fn attach_to_frame_offset(
        &self,
        subject_id: MassBodyId,
        subject_point_name: &str,
        offset_pframe_cpt: DVec3,
        t_pframe_cpt: DMat3,
    ) -> (DVec3, DMat3) {
        let subject_pt = self
            .find_mass_point(subject_id, subject_point_name)
            .unwrap_or_else(|| {
                panic!(
                    "mass point '{}' not found on body '{}'",
                    subject_point_name, self.nodes[subject_id].name
                )
            });
        // `subject_pt.position` is the cpt's location in the subject's
        // struct frame; `subject_pt.t_parent_this` is T_struct_cpt.
        let t_struct_cpt = subject_pt.t_parent_this;
        let x_struct_cpt = subject_pt.position;

        // T_pframe_struct = T_struct_cpt^T · T_pframe_cpt
        let t_pframe_struct = t_struct_cpt.transpose() * t_pframe_cpt;
        // x_pframe_struct = x_pframe_cpt - T_pframe_struct^T · x_struct_cpt
        let x_pframe_struct = offset_pframe_cpt - t_pframe_struct.transpose() * x_struct_cpt;

        (x_pframe_struct, t_pframe_struct)
    }

    /// Convenience: compute the parent-ref-frame to struct-frame offset
    /// for the **`BodyAttachAligned` ref-parent branch** — the
    /// hardcoded 180°-yaw docking convention with zero positional
    /// offset at the named subject point.
    ///
    /// Equivalent to
    /// `attach_to_frame_offset(subject_id, subject_point_name, [0;3],
    /// diag(-1,-1,1))`. Mirrors JEOD `BodyAttachAligned::apply` lines
    /// 111-126 (`models/dynamics/body_action/src/body_attach_aligned.cc`),
    /// which always passes a zero offset and a 180°-yaw matrix to the
    /// underlying named-point `attach_to_frame` for the ref-parent
    /// case.
    // JEOD_INV: MA.16 — 180° yaw convention for attach-by-point
    pub fn attach_to_frame_offset_aligned(
        &self,
        subject_id: MassBodyId,
        subject_point_name: &str,
    ) -> (DVec3, DMat3) {
        let yaw_180 = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );
        self.attach_to_frame_offset(subject_id, subject_point_name, DVec3::ZERO, yaw_180)
    }

    // -- attach / detach ----------------------------------------------------

    /// Attach `child_id` to `parent_id` at the given offset and rotation.
    ///
    /// `offset` is the child structural origin in the parent's structural
    /// frame. `t_parent_child` rotates from the parent structural frame to
    /// the child's structural frame.
    ///
    /// Panics if the child already has a parent.
    pub fn attach(
        &mut self,
        child_id: MassBodyId,
        parent_id: MassBodyId,
        offset: DVec3,
        t_parent_child: DMat3,
    ) {
        // JEOD_INV: BA.03 — attachment requires non-null parent; the `parent_id` argument
        // is `MassBodyId` (non-null by type); invalid IDs panic at the index site below.
        assert!(
            self.parent[child_id].is_none(),
            "child {} already attached to a parent",
            child_id
        );
        assert_ne!(child_id, parent_id, "cannot attach a body to itself");

        // JEOD_INV: MA.08 — no cycle in mass tree (arena-based, cycles impossible)
        // JEOD_INV: MA.19 — no same-tree attachment (cycle prevention)
        // JEOD_INV: BA.04 — body-action attachment also forbids cycles (same check)
        // Prevent creation of cycles: walk up from parent_id to the root
        // and ensure we never encounter child_id. This matches JEOD's
        // attach_validate_parent() (mass_attach.cc:370-388): "the only invalid
        // attachment is one that would make a cyclic graph."
        {
            let mut current = Some(parent_id);
            while let Some(pid) = current {
                assert_ne!(
                    pid, child_id,
                    "cannot attach body {} under its own descendant {} (would create cycle)",
                    child_id, parent_id
                );
                current = self.parent[pid];
            }
        }

        self.parent[child_id] = Some(parent_id);
        self.children[parent_id].push(child_id);

        self.nodes[child_id].structure_point = MassPointState {
            position: offset,
            t_parent_this: t_parent_child,
        };

        self.recompute_composites();
    }

    /// Detach `child_id` from its parent.
    ///
    /// The former parent's composite properties are recomputed. The child's
    /// parent-relative fields (`structure_point`, `composite_wrt_pstr`) are
    /// reset to defaults, matching JEOD's `detach_update_properties()`
    /// (mass_detach.cc:322-324) which calls `initialize_mass_point()` on
    /// all three parent-relative mass points.
    ///
    /// Panics if the child has no parent.
    pub fn detach(&mut self, child_id: MassBodyId) {
        let parent_id = self.parent[child_id].expect("detach called on a body with no parent");

        self.children[parent_id].retain(|&c| c != child_id);
        self.parent[child_id] = None;

        // Reset parent-relative fields on the detached child (JEOD mass_detach.cc:322-324).
        self.nodes[child_id].structure_point = MassPointState::default();
        self.nodes[child_id].composite_wrt_pstr = MassPointState::default();

        // JEOD_INV: MA.15 — detach recomputes inverse inertia for new root
        // Recompute inverse inertia on detached child (JEOD mass_detach.cc:328-335).
        let child = &mut self.nodes[child_id];
        if child.composite_properties.mass > 0.0 {
            let det = child.composite_properties.inertia.determinant();
            assert!(
                det.abs() > 1e-30,
                "Detached child '{}' has singular composite inertia (det={det:.2e})",
                child.name
            );
            child.composite_properties.inverse_inertia =
                child.composite_properties.inertia.inverse();
        } else {
            child.composite_properties.inverse_inertia = DMat3::ZERO;
        }

        // Recompute composites for the tree the parent still belongs to.
        self.recompute_composites();
    }

    // -- composite recomputation (JEOD algorithm) ---------------------------

    /// Recompute composite properties for the entire tree bottom-up.
    ///
    /// JEOD requires computing from leaves to root so that each parent sees
    /// up-to-date child composites. We collect a post-order traversal and
    /// process each node.
    // JEOD_INV: MA.06 — bottom-up mass property update (children first)
    // JEOD_INV: MA.07 — needs_update flag cleared after recomputation (always recomputes)
    pub fn recompute_composites(&mut self) {
        let order = self.post_order();
        for id in order {
            self.update_node(id);
        }
    }

    /// Compute a post-order (leaves-first) traversal of the entire forest.
    fn post_order(&self) -> Vec<MassBodyId> {
        let mut result = Vec::with_capacity(self.nodes.len());
        let mut visited = vec![false; self.nodes.len()];

        for root in 0..self.nodes.len() {
            if self.parent[root].is_none() && !visited[root] {
                self.post_order_walk(root, &mut visited, &mut result);
            }
        }
        result
    }

    fn post_order_walk(&self, id: MassBodyId, visited: &mut [bool], result: &mut Vec<MassBodyId>) {
        if visited[id] {
            return;
        }
        for &child in &self.children[id] {
            self.post_order_walk(child, visited, result);
        }
        visited[id] = true;
        result.push(id);
    }

    /// Update a single node's composite properties.
    ///
    /// Mirrors JEOD `MassBody::update_mass_properties` for one node.
    fn update_node(&mut self, id: MassBodyId) {
        if self.children[id].is_empty() {
            // Atomic body: composite == core (JEOD mass_update.cc lines 59-75).
            let node = &mut self.nodes[id];
            node.composite_properties = node.core_properties;
            node.core_wrt_composite = MassPointState::default();
        } else {
            // First compute composite_wrt_pstr for each child.
            // JEOD mass_update.cc lines 137-143:
            //   composite_wrt_pstr.position =
            //       T_parent_this^T * composite_properties.position
            //       + structure_point.position
            // This transforms the child's composite CoM from child struct frame
            // to parent struct frame.
            let child_ids: Vec<MassBodyId> = self.children[id].clone();
            for &cid in &child_ids {
                let child = &self.nodes[cid];
                let t = child.structure_point.t_parent_this;
                let comp_pos = child.composite_properties.position;
                let struct_pos = child.structure_point.position;
                // T^T * comp_pos + struct_pos
                let pos_in_parent = t.transpose() * comp_pos + struct_pos;

                self.nodes[cid].composite_wrt_pstr.position = pos_in_parent;
                self.nodes[cid].composite_wrt_pstr.t_parent_this =
                    self.nodes[cid].structure_point.t_parent_this;
            }

            // Composite center of mass (JEOD mass_calc_composite_cm.cc).
            self.calc_composite_cm(id);

            // Compute core_wrt_composite (JEOD mass_update.cc lines 104-107):
            // core_wrt_composite.position = core.position - composite.position
            // (both in structural frame, so no rotation needed)
            let core_pos = self.nodes[id].core_properties.position;
            let comp_pos = self.nodes[id].composite_properties.position;
            self.nodes[id].core_wrt_composite.position = core_pos - comp_pos;

            // Composite inertia (JEOD mass_calc_composite_inertia.cc).
            self.calc_composite_inertia(id);
        }

        // For root bodies, compute inverse inertia (JEOD mass_update.cc lines 116-125).
        // JEOD's Matrix3x3::invert_symmetric (dm_invert_symm.cc:86-94) checks
        // for singular matrices via fpclassify(determinant) == FP_ZERO.
        if self.parent[id].is_none() {
            let node = &mut self.nodes[id];
            if node.composite_properties.mass > 0.0 {
                let det = node.composite_properties.inertia.determinant();
                assert!(
                    det.abs() > 1e-30,
                    "Root body '{}' has singular composite inertia (det={det:.2e})",
                    node.name
                );
                node.composite_properties.inverse_inertia =
                    node.composite_properties.inertia.inverse();
            } else {
                node.composite_properties.inverse_inertia = DMat3::ZERO;
            }
        }
    }

    /// Composite center of mass — port of JEOD `mass_calc_composite_cm.cc`.
    ///
    /// Accumulates `mass * position` over the core and all children, where
    /// child positions are their `composite_wrt_pstr.position` (composite
    /// CoM in this body's structural frame).
    fn calc_composite_cm(&mut self, id: MassBodyId) {
        let core = &self.nodes[id].core_properties;
        let mut total_mass = core.mass;
        let mut weighted_pos = core.position * core.mass;

        for &cid in &self.children[id] {
            let child = &self.nodes[cid];
            total_mass += child.composite_properties.mass;
            weighted_pos += child.composite_wrt_pstr.position * child.composite_properties.mass;
        }

        let node = &mut self.nodes[id];
        node.composite_properties.mass = total_mass;
        // JEOD mass_calc_composite_cm.cc:72 / mass_update.cc:64
        node.composite_properties.inverse_mass = if total_mass > 0.0 {
            1.0 / total_mass
        } else {
            0.0
        };
        if total_mass > 0.0 {
            node.composite_properties.position = weighted_pos / total_mass;
        } else {
            node.composite_properties.position = DVec3::ZERO;
        }
    }

    /// Composite inertia — port of JEOD `mass_calc_composite_inertia.cc`.
    ///
    /// Starts with the core body's inertia shifted to the composite CoM via
    /// the parallel axis theorem, then adds each child's composite inertia
    /// (rotated to this body's structural frame) plus the child's parallel
    /// axis contribution.
    fn calc_composite_inertia(&mut self, id: MassBodyId) {
        let cm = self.nodes[id].composite_properties.position;

        // Core contribution: inertia + point-mass shift from core CoM to
        // composite CoM (JEOD mass_calc_composite_inertia.cc lines 61-64).
        let core = &self.nodes[id].core_properties;
        let core_offset = core.position - cm;
        let mut composite_inertia = core.inertia + point_mass_inertia(core.mass, core_offset);

        // Child contributions (lines 67-84).
        for &cid in &self.children[id] {
            let child = &self.nodes[cid];
            let child_offset = child.composite_wrt_pstr.position - cm;

            // Rotate child's composite inertia from child struct frame to
            // parent struct frame: T^T * I_child * T
            // This is JEOD's transpose_transform_matrix.
            let t = child.structure_point.t_parent_this;
            let rotated_inertia = t.transpose() * child.composite_properties.inertia * t;

            composite_inertia +=
                rotated_inertia + point_mass_inertia(child.composite_properties.mass, child_offset);
        }

        self.nodes[id].composite_properties.inertia = composite_inertia;
        // JEOD's Matrix3x3::invert_symmetric (dm_invert_symm.cc:86-94) checks
        // for singular matrices via fpclassify(determinant) == FP_ZERO.
        if self.nodes[id].composite_properties.mass > 0.0 {
            let det = composite_inertia.determinant();
            assert!(
                det.abs() > 1e-30,
                "Body '{}' has singular composite inertia (det={det:.2e})",
                self.nodes[id].name
            );
            self.nodes[id].composite_properties.inverse_inertia = composite_inertia.inverse();
        } else {
            self.nodes[id].composite_properties.inverse_inertia = DMat3::ZERO;
        }
    }
}

impl Default for MassTree {
    fn default() -> Self {
        Self::new()
    }
}

// ---------------------------------------------------------------------------
// Free functions
// ---------------------------------------------------------------------------

/// Parallel axis theorem (Steiner's theorem): inertia of a point mass at
/// offset `r` from the reference point.
///
/// Port of JEOD `MassBody::compute_point_mass_inertia` from
/// `mass_point_mass_inertia.cc`:
///
/// ```text
/// I[i][j] = mass * (r^2 * delta_ij - r[i] * r[j])
/// ```
pub fn point_mass_inertia(mass: f64, offset: DVec3) -> DMat3 {
    let r_sq = offset.length_squared();
    // mass * (r^2 * I - outer(offset, offset))
    let outer = DMat3::from_cols(offset * offset.x, offset * offset.y, offset * offset.z);
    DMat3::from_diagonal(DVec3::splat(r_sq)) * mass - outer * mass
}

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

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

    /// Helper: assert two DMat3 are approximately equal.
    fn assert_mat3_close(a: DMat3, b: DMat3, tol: f64, msg: &str) {
        let diff = a - b;
        for col in [diff.x_axis, diff.y_axis, diff.z_axis] {
            assert!(
                col.length() < tol,
                "{msg}: column diff {col:?} exceeds tolerance {tol}"
            );
        }
    }

    /// Helper: assert two DVec3 are approximately equal.
    fn assert_vec3_close(a: DVec3, b: DVec3, tol: f64, msg: &str) {
        let diff = (a - b).length();
        assert!(diff < tol, "{msg}: diff {diff} exceeds tolerance {tol}");
    }

    /// Helper: check that a matrix is symmetric.
    fn assert_symmetric(m: DMat3, tol: f64, msg: &str) {
        let mt = m.transpose();
        assert_mat3_close(m, mt, tol, msg);
    }

    // -----------------------------------------------------------------------
    // 1. Single body: composite == core
    // -----------------------------------------------------------------------

    #[test]
    fn single_body_composite_equals_core() {
        let mut tree = MassTree::new();
        let core = MassProperties::new(42.0);
        let id = tree.add_root("root".into(), core);

        let body = tree.get(id);
        assert_eq!(body.composite_properties.mass, body.core_properties.mass);
        assert_eq!(
            body.composite_properties.inertia,
            body.core_properties.inertia
        );
        assert_eq!(
            body.composite_properties.position,
            body.core_properties.position
        );
    }

    // -----------------------------------------------------------------------
    // 2. Two point masses at known offset
    // -----------------------------------------------------------------------

    #[test]
    fn two_point_masses_composite() {
        let mut tree = MassTree::new();

        // Parent: 10 kg at origin, spherical inertia
        let parent_core = MassProperties::new(10.0);
        let pid = tree.add_root("parent".into(), parent_core);

        // Child: 5 kg at origin of its own structural frame, spherical inertia
        let child_core = MassProperties::new(5.0);
        let cid = tree.add_body("child".into(), child_core);

        // Attach child at offset [3, 0, 0] in parent's struct frame, identity rotation
        tree.attach(cid, pid, DVec3::new(3.0, 0.0, 0.0), DMat3::IDENTITY);

        let parent = tree.get(pid);

        // Composite mass = 15 kg
        assert!(
            (parent.composite_properties.mass - 15.0).abs() < 1e-12,
            "composite mass = {}",
            parent.composite_properties.mass
        );

        // Composite CoM: weighted average = (10*0 + 5*3) / 15 = 1.0 m along x
        let expected_cm = DVec3::new(1.0, 0.0, 0.0);
        assert_vec3_close(
            parent.composite_properties.position,
            expected_cm,
            1e-12,
            "composite CoM",
        );

        // Verify composite inertia via manual parallel axis calculation:
        //   Parent core at [0,0,0], composite CoM at [1,0,0]
        //   Parent offset from composite CoM = [-1, 0, 0]
        //   Child composite CoM in parent struct frame = [3, 0, 0]
        //   Child offset from composite CoM = [2, 0, 0]
        //
        //   I_parent_core = 10 * I_3x3 (spherical)
        //   I_parent_shift = point_mass(10, [-1,0,0]) = 10 * diag(0, 1, 1)
        //
        //   I_child_core = 5 * I_3x3 (spherical, identity rotation so no change)
        //   I_child_shift = point_mass(5, [2,0,0]) = 5 * diag(0, 4, 4) = diag(0, 20, 20)
        //
        //   Total = (10*I + diag(0,10,10)) + (5*I + diag(0,20,20))
        //         = diag(10,10,10) + diag(0,10,10) + diag(5,5,5) + diag(0,20,20)
        //         = diag(15, 45, 45)
        let expected_inertia = DMat3::from_diagonal(DVec3::new(15.0, 45.0, 45.0));
        assert_mat3_close(
            parent.composite_properties.inertia,
            expected_inertia,
            1e-10,
            "composite inertia",
        );
    }

    // -----------------------------------------------------------------------
    // 3. Parallel axis theorem — known values
    // -----------------------------------------------------------------------

    #[test]
    fn parallel_axis_theorem_known_values() {
        let inertia = point_mass_inertia(5.0, DVec3::new(3.0, 0.0, 0.0));
        // I_xx = m * (y^2 + z^2) = 5 * 0 = 0
        // I_yy = m * (x^2 + z^2) = 5 * 9 = 45
        // I_zz = m * (x^2 + y^2) = 5 * 9 = 45
        // Off-diagonals = -m * r_i * r_j = 0 (y = z = 0)
        let expected = DMat3::from_cols(
            DVec3::new(0.0, 0.0, 0.0),
            DVec3::new(0.0, 45.0, 0.0),
            DVec3::new(0.0, 0.0, 45.0),
        );
        assert_mat3_close(inertia, expected, 1e-12, "point mass inertia at [3,0,0]");
    }

    #[test]
    fn parallel_axis_theorem_off_diagonal() {
        // offset along all three axes to exercise products of inertia
        let r = DVec3::new(1.0, 2.0, 3.0);
        let m = 4.0;
        let inertia = point_mass_inertia(m, r);
        let r_sq = r.length_squared(); // 14

        // Diagonal: m * (r^2 - r_i^2)
        assert!((inertia.x_axis.x - m * (r_sq - r.x * r.x)).abs() < 1e-12);
        assert!((inertia.y_axis.y - m * (r_sq - r.y * r.y)).abs() < 1e-12);
        assert!((inertia.z_axis.z - m * (r_sq - r.z * r.z)).abs() < 1e-12);

        // Off-diagonal: -m * r_i * r_j
        assert!((inertia.y_axis.x - (-m * r.x * r.y)).abs() < 1e-12);
        assert!((inertia.z_axis.x - (-m * r.x * r.z)).abs() < 1e-12);
        assert!((inertia.z_axis.y - (-m * r.y * r.z)).abs() < 1e-12);

        assert_symmetric(inertia, 1e-12, "point mass inertia symmetry");
    }

    // -----------------------------------------------------------------------
    // 4. Attach-detach round-trip
    // -----------------------------------------------------------------------

    #[test]
    fn attach_detach_round_trip() {
        let mut tree = MassTree::new();

        let parent_core = MassProperties::new(10.0);
        let pid = tree.add_root("parent".into(), parent_core);

        // Snapshot of parent composite before attaching child.
        let orig_mass = tree.get(pid).composite_properties.mass;
        let orig_inertia = tree.get(pid).composite_properties.inertia;
        let orig_position = tree.get(pid).composite_properties.position;

        let child_core = MassProperties::new(5.0);
        let cid = tree.add_body("child".into(), child_core);

        // Attach: composite must change.
        tree.attach(cid, pid, DVec3::new(2.0, 0.0, 0.0), DMat3::IDENTITY);
        assert!(
            (tree.get(pid).composite_properties.mass - 15.0).abs() < 1e-12,
            "composite mass after attach"
        );

        // Detach: parent composite should revert to core.
        tree.detach(cid);
        assert!(
            (tree.get(pid).composite_properties.mass - orig_mass).abs() < 1e-12,
            "mass restored after detach"
        );
        assert_mat3_close(
            tree.get(pid).composite_properties.inertia,
            orig_inertia,
            1e-12,
            "inertia restored after detach",
        );
        assert_vec3_close(
            tree.get(pid).composite_properties.position,
            orig_position,
            1e-12,
            "position restored after detach",
        );
    }

    // -----------------------------------------------------------------------
    // 5. Three-body chain A -> B -> C
    // -----------------------------------------------------------------------

    #[test]
    fn three_body_chain() {
        let mut tree = MassTree::new();

        let ma = MassProperties::new(10.0);
        let mb = MassProperties::new(5.0);
        let mc = MassProperties::new(3.0);

        let a = tree.add_root("A".into(), ma);
        let b = tree.add_body("B".into(), mb);
        let c = tree.add_body("C".into(), mc);

        // B attached at [2, 0, 0] on A, C attached at [1, 0, 0] on B
        tree.attach(b, a, DVec3::new(2.0, 0.0, 0.0), DMat3::IDENTITY);
        tree.attach(c, b, DVec3::new(1.0, 0.0, 0.0), DMat3::IDENTITY);

        // A's composite mass = 10 + 5 + 3 = 18
        assert!(
            (tree.get(a).composite_properties.mass - 18.0).abs() < 1e-12,
            "A composite mass = {}",
            tree.get(a).composite_properties.mass
        );

        // B's composite mass = 5 + 3 = 8
        assert!(
            (tree.get(b).composite_properties.mass - 8.0).abs() < 1e-12,
            "B composite mass = {}",
            tree.get(b).composite_properties.mass
        );

        // C's composite mass = 3
        assert!(
            (tree.get(c).composite_properties.mass - 3.0).abs() < 1e-12,
            "C composite mass = {}",
            tree.get(c).composite_properties.mass
        );

        // A's composite CoM:
        //   A core at [0,0,0] mass 10
        //   B's composite CoM in B struct frame:
        //     B core at [0,0,0] mass 5, C at [1,0,0] mass 3 => (5*0+3*1)/8 = 0.375
        //   B's composite CoM in A struct frame: [2+0.375, 0, 0] = [2.375, 0, 0]
        //   A composite CoM: (10*0 + 8*2.375) / 18 = 19/18 ≈ 1.0556
        let expected_a_cm_x = (10.0 * 0.0 + 8.0 * 2.375) / 18.0;
        assert_vec3_close(
            tree.get(a).composite_properties.position,
            DVec3::new(expected_a_cm_x, 0.0, 0.0),
            1e-10,
            "A composite CoM",
        );

        // Detach B from A: A should revert to its own core
        tree.detach(b);
        assert!(
            (tree.get(a).composite_properties.mass - 10.0).abs() < 1e-12,
            "A mass after detach"
        );
        assert_vec3_close(
            tree.get(a).composite_properties.position,
            DVec3::ZERO,
            1e-12,
            "A position after detach",
        );

        // B should still have its composite (B+C)
        assert!(
            (tree.get(b).composite_properties.mass - 8.0).abs() < 1e-12,
            "B mass unchanged after detach from A"
        );
    }

    // -----------------------------------------------------------------------
    // 6. Non-identity rotation
    // -----------------------------------------------------------------------

    #[test]
    fn non_identity_rotation() {
        // Child attached with 90-degree rotation about Z axis.
        // T_parent_child rotates parent X to child Y, parent Y to child -X.
        //
        // If the child has inertia diag(1, 4, 9) in its own frame, then in
        // the parent frame the rotated inertia should be diag(4, 1, 9).
        //
        // T = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]  (90 deg about Z)
        // T^T * diag(1,4,9) * T = diag(4,1,9)

        let mut tree = MassTree::new();

        // Parent: 10 kg, spherical inertia
        let parent_core = MassProperties::new(10.0);
        let pid = tree.add_root("parent".into(), parent_core);

        // Child: 5 kg, non-spherical inertia
        let child_inertia = DMat3::from_diagonal(DVec3::new(1.0, 4.0, 9.0));
        let child_core = MassProperties::with_inertia(5.0, child_inertia, DVec3::ZERO);
        let cid = tree.add_body("child".into(), child_core);

        // 90 degrees about Z: T_parent_child
        let t = DMat3::from_cols(
            DVec3::new(0.0, 1.0, 0.0),  // parent X -> child: [0, 1, 0]
            DVec3::new(-1.0, 0.0, 0.0), // parent Y -> child: [-1, 0, 0]
            DVec3::new(0.0, 0.0, 1.0),  // parent Z -> child: [0, 0, 1]
        );

        // Attach child at [3, 0, 0] with 90-deg rotation
        tree.attach(cid, pid, DVec3::new(3.0, 0.0, 0.0), t);

        let parent = tree.get(pid);

        // Composite mass = 15
        assert!(
            (parent.composite_properties.mass - 15.0).abs() < 1e-12,
            "composite mass"
        );

        // Composite CoM at [1, 0, 0] (same as test 2 — child CoM is at
        // child origin so rotation doesn't move it)
        assert_vec3_close(
            parent.composite_properties.position,
            DVec3::new(1.0, 0.0, 0.0),
            1e-12,
            "composite CoM with rotation",
        );

        // Expected composite inertia:
        //   Parent core offset from composite CoM = [-1, 0, 0]
        //   Parent core inertia = 10*I + point_mass(10, [-1,0,0])
        //     = diag(10,10,10) + diag(0,10,10) = diag(10, 20, 20)
        //
        //   Child rotated inertia: T^T * diag(1,4,9) * T = diag(4, 1, 9)
        //   Child offset from composite CoM = [2, 0, 0]
        //   Child contribution = diag(4,1,9) + point_mass(5, [2,0,0])
        //     = diag(4,1,9) + diag(0,20,20) = diag(4, 21, 29)
        //
        //   Total = diag(10,20,20) + diag(4,21,29) = diag(14, 41, 49)
        let expected_inertia = DMat3::from_diagonal(DVec3::new(14.0, 41.0, 49.0));
        assert_mat3_close(
            parent.composite_properties.inertia,
            expected_inertia,
            1e-10,
            "composite inertia with rotation",
        );
    }

    // -----------------------------------------------------------------------
    // 7. Composite inertia symmetry
    // -----------------------------------------------------------------------

    #[test]
    fn composite_inertia_symmetry() {
        let mut tree = MassTree::new();

        // Use asymmetric offsets and rotations to stress symmetry
        let parent_core = MassProperties::with_inertia(
            10.0,
            DMat3::from_diagonal(DVec3::new(100.0, 200.0, 300.0)),
            DVec3::new(0.1, -0.2, 0.3),
        );
        let pid = tree.add_root("parent".into(), parent_core);

        let child_core = MassProperties::with_inertia(
            5.0,
            DMat3::from_diagonal(DVec3::new(10.0, 20.0, 30.0)),
            DVec3::new(-0.05, 0.1, 0.0),
        );
        let cid = tree.add_body("child".into(), child_core);

        // 45-degree rotation about Y
        let angle = std::f64::consts::FRAC_PI_4;
        let c = angle.cos();
        let s = angle.sin();
        let t = DMat3::from_cols(
            DVec3::new(c, 0.0, -s),
            DVec3::new(0.0, 1.0, 0.0),
            DVec3::new(s, 0.0, c),
        );

        tree.attach(cid, pid, DVec3::new(1.0, 2.0, -0.5), t);

        let inertia = tree.get(pid).composite_properties.inertia;
        assert_symmetric(inertia, 1e-10, "composite inertia after asymmetric attach");

        // Verify inverse is also consistent
        let product = inertia * tree.get(pid).composite_properties.inverse_inertia;
        assert_mat3_close(product, DMat3::IDENTITY, 1e-8, "I * I^-1 = identity");
    }

    // -----------------------------------------------------------------------
    // 8. Named attachment points: attach_aligned
    // -----------------------------------------------------------------------

    #[test]
    #[should_panic(expected = "mass point name must be non-empty")]
    fn add_mass_point_rejects_empty_name() {
        // JEOD_INV: MA.10 — empty name must panic at add_mass_point
        let mut tree = MassTree::new();
        let pid = tree.add_root("parent".into(), MassProperties::new(10.0));
        tree.add_mass_point(pid, "", DVec3::ZERO, DMat3::IDENTITY);
    }

    #[test]
    fn attach_aligned_identity_points() {
        // Two bodies with points at their origins, identity rotation.
        // After docking, child struct origin should be at parent point position
        // (no child offset to subtract), and rotation should be 180° yaw.
        let mut tree = MassTree::new();
        let pid = tree.add_root("parent".into(), MassProperties::new(10.0));
        let cid = tree.add_body("child".into(), MassProperties::new(5.0));

        tree.add_mass_point(pid, "dock", DVec3::new(3.0, 0.0, 0.0), DMat3::IDENTITY);
        tree.add_mass_point(cid, "dock", DVec3::ZERO, DMat3::IDENTITY);

        tree.attach_aligned(cid, "dock", pid, "dock");

        // Child struct origin at (3, 0, 0) in parent struct frame.
        let child = tree.get(cid);
        assert_vec3_close(
            child.structure_point.position,
            DVec3::new(3.0, 0.0, 0.0),
            1e-12,
            "child offset",
        );

        // Rotation: 180° yaw = diag(-1, -1, 1)
        let expected_t = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );
        assert_mat3_close(
            child.structure_point.t_parent_this,
            expected_t,
            1e-12,
            "child rotation",
        );
    }

    #[test]
    fn attach_aligned_offset_points() {
        // SM "CM interface" at (24.6, 0, 0) attaches to CM "SM interface" at (11.6, 0, 0).
        // All identity rotations. Expected offset: 11.6 + 24.6 = 36.2.
        let mut tree = MassTree::new();
        let cm = tree.add_root("CM".into(), MassProperties::new(10.0));
        let sm = tree.add_body("SM".into(), MassProperties::new(10.0));

        tree.add_mass_point(
            cm,
            "SM interface",
            DVec3::new(11.6, 0.0, 0.0),
            DMat3::IDENTITY,
        );
        tree.add_mass_point(
            sm,
            "CM interface",
            DVec3::new(24.6, 0.0, 0.0),
            DMat3::IDENTITY,
        );

        tree.attach_aligned(sm, "CM interface", cm, "SM interface");

        assert_vec3_close(
            tree.get(sm).structure_point.position,
            DVec3::new(36.2, 0.0, 0.0),
            1e-10,
            "SM offset in CM frame",
        );
    }

    #[test]
    fn attach_aligned_rotated_points() {
        // Child point has 180° yaw, parent point has 180° yaw.
        // The three yaws (child invert + docking + parent) should compose to
        // a net 180° yaw (odd number of 180° yaws about Z).
        let yaw_180 = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );

        let mut tree = MassTree::new();
        let pid = tree.add_root("parent".into(), MassProperties::new(10.0));
        let cid = tree.add_body("child".into(), MassProperties::new(5.0));

        // Parent point at (4, 0, 0) with 180° yaw
        tree.add_mass_point(pid, "port", DVec3::new(4.0, 0.0, 0.0), yaw_180);
        // Child point at (0, 0, 0) with 180° yaw
        tree.add_mass_point(cid, "port", DVec3::ZERO, yaw_180);

        tree.attach_aligned(cid, "port", pid, "port");

        // Rotation chain:
        //   inv_t = child_pt_t^T = yaw_180^T = yaw_180
        //   t_parent_child = inv_t * yaw_dock * parent_pt_t
        //                  = yaw_180 * yaw_180 * yaw_180
        //                  = I * yaw_180 = yaw_180
        assert_mat3_close(
            tree.get(cid).structure_point.t_parent_this,
            yaw_180,
            1e-12,
            "triple yaw rotation",
        );

        // Position: inv_pos = -(yaw_180 * (0,0,0)) = (0,0,0)
        // pos_after_yaw = yaw_dock * (0,0,0) = (0,0,0)
        // offset = yaw_180^T * (0,0,0) + (4,0,0) = (4,0,0)
        assert_vec3_close(
            tree.get(cid).structure_point.position,
            DVec3::new(4.0, 0.0, 0.0),
            1e-12,
            "offset with rotated points",
        );
    }

    #[test]
    fn attach_aligned_matches_manual() {
        // Verify that attach_aligned produces identical results to manually
        // computing the offset/rotation and calling attach() directly.
        let yaw_180 = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );

        // Setup: CM "SM interface" at (11.6, 0, 0) identity, SM "CM interface" at (24.6, 0, 0) identity
        let core_cm = MassProperties::with_inertia(
            5810.5, // 12807 lb
            DMat3::from_diagonal(DVec3::new(6631.0, 2723.0, 2723.0)),
            DVec3::new(2.65176, 0.0, 0.0),
        );
        let core_sm = MassProperties::with_inertia(
            24520.0, // 54064 lb
            DMat3::from_diagonal(DVec3::new(46648.0, 52040.0, 52040.0)),
            DVec3::new(3.74904, 0.0, 0.0),
        );

        // Method 1: attach_aligned
        let mut tree1 = MassTree::new();
        let cm1 = tree1.add_root("CM".into(), core_cm);
        let sm1 = tree1.add_body("SM".into(), core_sm);
        tree1.add_mass_point(
            cm1,
            "SM interface",
            DVec3::new(11.6 * 0.3048, 0.0, 0.0),
            DMat3::IDENTITY,
        );
        tree1.add_mass_point(
            sm1,
            "CM interface",
            DVec3::new(24.6 * 0.3048, 0.0, 0.0),
            DMat3::IDENTITY,
        );
        tree1.attach_aligned(sm1, "CM interface", cm1, "SM interface");

        // Method 2: manual attach with precomputed offset/rotation
        // offset = parent_pt_pos + parent_pt_t^T * t_yaw * (-(child_pt_t * child_pt_pos))
        // = (11.6*0.3048, 0, 0) + I * yaw * (-(I * (24.6*0.3048, 0, 0)))
        // = (11.6*0.3048, 0, 0) + (24.6*0.3048, 0, 0)  [since yaw negates the negated x]
        // = (36.2*0.3048, 0, 0)
        let manual_offset = DVec3::new(36.2 * 0.3048, 0.0, 0.0);

        let mut tree2 = MassTree::new();
        let cm2 = tree2.add_root("CM".into(), core_cm);
        let sm2 = tree2.add_body("SM".into(), core_sm);
        tree2.attach(sm2, cm2, manual_offset, yaw_180);

        // Compare composite properties
        let comp1 = &tree1.get(cm1).composite_properties;
        let comp2 = &tree2.get(cm2).composite_properties;

        assert!(
            (comp1.mass - comp2.mass).abs() < 1e-10,
            "mass: {} vs {}",
            comp1.mass,
            comp2.mass
        );
        assert_vec3_close(comp1.position, comp2.position, 1e-10, "composite CoM");
        assert_mat3_close(comp1.inertia, comp2.inertia, 1e-6, "composite inertia");
    }

    #[test]
    fn attach_aligned_non_trivial_rotation() {
        // Test with a non-symmetric rotation to catch composition order bugs.
        // Child point: 90° about Z (T = [[0,-1,0],[1,0,0],[0,0,1]])
        // Parent point: identity
        let rot_90z = 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),
        );
        let yaw_180 = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );

        let mut tree = MassTree::new();
        let pid = tree.add_root("parent".into(), MassProperties::new(10.0));
        let cid = tree.add_body("child".into(), MassProperties::new(5.0));

        tree.add_mass_point(pid, "dock", DVec3::new(5.0, 0.0, 0.0), DMat3::IDENTITY);
        tree.add_mass_point(cid, "dock", DVec3::new(2.0, 0.0, 0.0), rot_90z);

        tree.attach_aligned(cid, "dock", pid, "dock");

        // Expected rotation: inv_t * yaw * parent_pt_t
        //   inv_t = rot_90z^T = [[0,1,0],[-1,0,0],[0,0,1]]
        //   T = rot_90z^T * yaw * I = rot_90z^T * yaw
        let expected_t = rot_90z.transpose() * yaw_180;
        assert_mat3_close(
            tree.get(cid).structure_point.t_parent_this,
            expected_t,
            1e-12,
            "non-trivial rotation composition",
        );

        // Expected position:
        //   inv_pos = -(rot_90z * (2, 0, 0)) = -(0, 2, 0) = (0, -2, 0)
        //   pos_after_yaw = yaw * (0, -2, 0) = (0, 2, 0)
        //   offset = I * (0, 2, 0) + (5, 0, 0) = (5, 2, 0)
        assert_vec3_close(
            tree.get(cid).structure_point.position,
            DVec3::new(5.0, 2.0, 0.0),
            1e-12,
            "non-trivial position",
        );
    }

    // -----------------------------------------------------------------------
    // attach_to_frame_offset / attach_to_frame_offset_aligned
    // -----------------------------------------------------------------------

    /// `attach_to_frame_offset_aligned` reproduces the matrix-form values
    /// that JEOD's `BodyAttachAligned` derives for the SIM_ref_attach
    /// `RUN_ref_attach_pt2pt` scenario: subject mass-point `attach1` at
    /// `(10, 0, 0)` with identity orientation, parent reference frame
    /// `Earth.pfix`. The named-point alignment with the 180°-yaw docking
    /// convention must yield the same `(offset_pframe_struct,
    /// T_pframe_struct)` pair JEOD's matrix-form `BodyAttachMatrix`
    /// would have set directly (`offset = (10, 0, 0)`,
    /// `T = diag(-1, -1, 1)`), confirming both verif runs configure the
    /// same physical attachment.
    #[test]
    fn attach_to_frame_offset_aligned_matches_pt2pt_run() {
        let mut tree = MassTree::new();
        let bid = tree.add_root("target".into(), MassProperties::new(1.0));
        tree.add_mass_point(bid, "attach1", DVec3::new(10.0, 0.0, 0.0), DMat3::IDENTITY);

        let (offset, t) = tree.attach_to_frame_offset_aligned(bid, "attach1");

        let expected_t = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );
        assert_vec3_close(
            offset,
            DVec3::new(10.0, 0.0, 0.0),
            1e-12,
            "pframe→struct offset",
        );
        assert_mat3_close(t, expected_t, 1e-12, "pframe→struct rotation");
    }

    /// `attach_to_frame_offset` honours the user-supplied offset and
    /// rotation when the subject mass point itself is at the body's
    /// origin with identity orientation — the result is just the user
    /// inputs (the cpt is the struct).
    #[test]
    fn attach_to_frame_offset_identity_point_returns_inputs() {
        let mut tree = MassTree::new();
        let bid = tree.add_root("body".into(), MassProperties::new(1.0));
        tree.add_mass_point(bid, "origin", DVec3::ZERO, DMat3::IDENTITY);

        let user_offset = DVec3::new(1.0, 2.0, 3.0);
        let user_t = 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),
        );
        let (offset, t) = tree.attach_to_frame_offset(bid, "origin", user_offset, user_t);

        assert_vec3_close(offset, user_offset, 1e-12, "identity-point offset");
        assert_mat3_close(t, user_t, 1e-12, "identity-point rotation");
    }

    /// Non-trivial mass-point pose composes correctly: when the named
    /// point sits at `(2, 0, 0)` rotated 90° about Z relative to the
    /// struct frame, the `(offset, T)` returned for the BodyAttachAligned
    /// branch must be the rigid-body inverse mapping of that mass-point
    /// pose under the 180°-yaw convention.
    ///
    /// Worked out by hand:
    ///   T_struct_cpt = rot_90z (cpt rotated 90° about Z relative to struct)
    ///   T_pframe_cpt = diag(-1, -1, 1) (yaw_180)
    ///   T_pframe_struct = T_struct_cpt^T · T_pframe_cpt = rot_90z^T · yaw_180
    ///   x_pframe_struct = 0 - T_pframe_struct^T · (2, 0, 0)
    ///                   = -(rot_90z^T · yaw_180)^T · (2, 0, 0)
    ///                   = -(yaw_180^T · rot_90z) · (2, 0, 0)
    ///                   = -yaw_180 · (rot_90z · (2, 0, 0))
    ///                   = -yaw_180 · (0, 2, 0)
    ///                   = -(0, -2, 0)
    ///                   = (0, 2, 0)
    #[test]
    fn attach_to_frame_offset_aligned_non_trivial_point() {
        let rot_90z = 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),
        );
        let yaw_180 = DMat3::from_cols(
            DVec3::new(-1.0, 0.0, 0.0),
            DVec3::new(0.0, -1.0, 0.0),
            DVec3::new(0.0, 0.0, 1.0),
        );

        let mut tree = MassTree::new();
        let bid = tree.add_root("body".into(), MassProperties::new(1.0));
        tree.add_mass_point(bid, "port", DVec3::new(2.0, 0.0, 0.0), rot_90z);

        let (offset, t) = tree.attach_to_frame_offset_aligned(bid, "port");

        let expected_t = rot_90z.transpose() * yaw_180;
        assert_mat3_close(t, expected_t, 1e-12, "non-trivial mass-point rotation");
        assert_vec3_close(
            offset,
            DVec3::new(0.0, 2.0, 0.0),
            1e-12,
            "non-trivial mass-point offset",
        );
    }

    /// `attach_to_frame_offset` panics with an actionable diagnostic
    /// when the named subject point does not exist on the body.
    #[test]
    #[should_panic(expected = "mass point 'missing' not found on body 'body'")]
    fn attach_to_frame_offset_missing_point_panics() {
        let mut tree = MassTree::new();
        let bid = tree.add_root("body".into(), MassProperties::new(1.0));
        let _ = tree.attach_to_frame_offset(bid, "missing", DVec3::ZERO, DMat3::IDENTITY);
    }

    // -----------------------------------------------------------------------
    // 9. Angular momentum conservation across attach/detach
    // -----------------------------------------------------------------------

    /// Verifies that total angular momentum is conserved through the
    /// attach/detach cycle when the composite angular velocity is
    /// adjusted to conserve L = I * omega.
    ///
    /// This validates the formula, not an automated momentum transfer —
    /// MassTree doesn't track angular velocity.
    #[test]
    fn attach_detach_angular_momentum_conservation() {
        let mut tree = MassTree::new();

        // Parent: diagonal inertia, spinning about z
        let parent_inertia = DMat3::from_diagonal(DVec3::new(100.0, 200.0, 300.0));
        let parent_core = MassProperties::with_inertia(10.0, parent_inertia, DVec3::ZERO);
        let pid = tree.add_root("parent".into(), parent_core);

        let omega_parent = DVec3::new(0.01, 0.02, 0.1);
        let l_parent = parent_inertia * omega_parent;

        // Child: smaller body at offset, with its own spin
        let child_inertia = DMat3::from_diagonal(DVec3::new(10.0, 20.0, 30.0));
        let child_core = MassProperties::with_inertia(5.0, child_inertia, DVec3::ZERO);
        let cid = tree.add_body("child".into(), child_core);

        let omega_child = DVec3::new(0.05, -0.03, 0.02);
        let l_child = child_inertia * omega_child;

        // Total angular momentum before attach (both in same frame, simplified:
        // ignoring orbital angular momentum from offset for this unit test —
        // we test pure spin contribution)
        let l_total_before = l_parent + l_child;

        // Attach child at offset (identity rotation for simplicity)
        let offset = DVec3::new(2.0, 0.0, 0.0);
        tree.attach(cid, pid, offset, DMat3::IDENTITY);

        // After attach, composite inertia includes parallel axis contribution
        let i_composite = tree.get(pid).composite_properties.inertia;
        let i_composite_inv = tree.get(pid).composite_properties.inverse_inertia;

        // Compute the composite omega that conserves angular momentum
        let omega_composite = i_composite_inv * l_total_before;

        // Verify: I_composite * omega_composite == L_total_before
        let l_composite = i_composite * omega_composite;
        assert_vec3_close(
            l_composite,
            l_total_before,
            1e-10,
            "L = I_composite * omega_composite should equal L_total_before",
        );

        // Detach and verify parent's original properties are restored
        tree.detach(cid);
        let i_parent_after = tree.get(pid).composite_properties.inertia;

        // Recompute omega_parent from L_parent using restored inertia
        let i_parent_after_inv = tree.get(pid).composite_properties.inverse_inertia;
        let omega_parent_after = i_parent_after_inv * l_parent;

        // Angular momentum should be preserved through the formula
        let l_parent_after = i_parent_after * omega_parent_after;
        assert_vec3_close(
            l_parent_after,
            l_parent,
            1e-10,
            "parent angular momentum preserved after detach",
        );

        // Inertia should be back to original
        assert_mat3_close(
            i_parent_after,
            parent_inertia,
            1e-12,
            "parent inertia restored after detach",
        );
    }

    // -----------------------------------------------------------------------
    // root_of / struct_chain_to_root / attach_with_reroot
    // -----------------------------------------------------------------------

    /// `root_of` on a root returns itself; on a deeper descendant it
    /// walks up to the tree root.
    #[test]
    fn root_of_walks_to_tree_root() {
        let mut tree = MassTree::new();
        let a = tree.add_root("a".into(), MassProperties::new(1.0));
        let b = tree.add_root("b".into(), MassProperties::new(1.0));
        let c = tree.add_root("c".into(), MassProperties::new(1.0));
        // root case
        assert_eq!(tree.root_of(a), a);
        // chain a → b → c (we attach b under a, then c under b → so root is a)
        tree.attach(b, a, DVec3::new(1.0, 0.0, 0.0), DMat3::IDENTITY);
        tree.attach(c, b, DVec3::new(1.0, 0.0, 0.0), DMat3::IDENTITY);
        assert_eq!(tree.root_of(c), a);
        assert_eq!(tree.root_of(b), a);
        assert_eq!(tree.root_of(a), a);
    }

    /// `struct_chain_to_root` composes per-link offsets in struct
    /// frames. Single link: identity rotation, the result is just
    /// `child.structure_point.position`.
    #[test]
    fn struct_chain_single_link_is_structure_point() {
        let mut tree = MassTree::new();
        let p = tree.add_root("p".into(), MassProperties::new(1.0));
        let c = tree.add_root("c".into(), MassProperties::new(1.0));
        tree.attach(c, p, DVec3::new(2.0, 3.0, 4.0), DMat3::IDENTITY);
        let chain = tree.struct_chain_to_root(p, c);
        assert_vec3_close(
            chain.position,
            DVec3::new(2.0, 3.0, 4.0),
            1e-12,
            "single-link offset",
        );
        assert_mat3_close(
            chain.t_parent_this,
            DMat3::IDENTITY,
            1e-12,
            "single-link rotation identity",
        );
    }

    /// Two rotated links: validate the struct-frame composition is in
    /// agreement with the attach geometry.
    #[test]
    fn struct_chain_two_links_with_rotation() {
        // Build a → b → c. Each link is offset by +x in the parent
        // struct frame, with a 90° rotation about Z so the child's
        // struct +x aligns with the parent's +y.
        let r_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),
        );
        let mut tree = MassTree::new();
        let a = tree.add_root("a".into(), MassProperties::new(1.0));
        let b = tree.add_root("b".into(), MassProperties::new(1.0));
        let c = tree.add_root("c".into(), MassProperties::new(1.0));
        tree.attach(b, a, DVec3::new(1.0, 0.0, 0.0), r_z90);
        tree.attach(c, b, DVec3::new(1.0, 0.0, 0.0), r_z90);

        let chain = tree.struct_chain_to_root(a, c);
        // Walk root → a → b → c manually:
        //   root start: pos=0, t=I.
        //   step b: pos += t.transpose() * (1,0,0) = (1,0,0); t = r_z90.
        //   step c: pos += t.transpose() * (1,0,0) = (1,0,0) + r_z90^T·(1,0,0)
        //                 = (1,0,0) + (0,-1,0) = (1,-1,0).  Wait — r_z90^T·x = (0,-1,0)?
        //   r_z90 maps parent-struct +x axis → +y axis (column 0 = (0,1,0)).
        //   r_z90^T maps the b-struct +x axis back into a-struct: r_z90^T = r_z(-90°), so
        //   r_z90^T·(1,0,0) = (0,-1,0). After step c: pos = (1,0,0) + (0,-1,0) = (1,-1,0).
        //   Hmm — that doesn't match the geometric intuition; let me redo.
        //   Actually: pos in *root* frame after step b = root start (0) + T_root_to_root^T · b.position.
        //   Initially t = T_root_to_a = I (we're walking root=a → ... → c). After step b,
        //   we add t.transpose() * b.structure_point.position where t is *currently* T_root_to_a = I,
        //   so we add (1,0,0). Then update t = b.structure_point.t_parent_this · t = r_z90·I = r_z90.
        //   t now is T_a_to_b = r_z90. For step c: add t.transpose() · c.structure_point.position.
        //   t.transpose() = r_z90^T = r_z(-90°). r_z(-90°) · (1,0,0) = (0,-1,0).
        //   So pos after c = (1,0,0) + (0,-1,0) = (1,-1,0).
        // Final t = r_z90 · r_z90 = r_z(180°) (note: composition is c.t · b.t, but our
        // formula has `t = sp.t_parent_this * t`, so after c it's r_z90 * r_z90 = R_z(180°)).
        let r_z180 = r_z90 * r_z90;
        assert_mat3_close(chain.t_parent_this, r_z180, 1e-12, "T_root_to_c");
        assert_vec3_close(
            chain.position,
            DVec3::new(1.0, -1.0, 0.0),
            1e-12,
            "pos_c_in_a",
        );
    }

    /// Reroot path bit-identical to the plain `attach` path when the
    /// subject is itself a root.
    #[test]
    fn attach_with_reroot_root_subject_matches_attach() {
        let mut tree_a = MassTree::new();
        let p_a = tree_a.add_root("p".into(), MassProperties::new(2.0));
        let c_a = tree_a.add_root("c".into(), MassProperties::new(3.0));
        tree_a.attach(c_a, p_a, DVec3::new(1.0, 2.0, 3.0), DMat3::IDENTITY);

        let mut tree_b = MassTree::new();
        let p_b = tree_b.add_root("p".into(), MassProperties::new(2.0));
        let c_b = tree_b.add_root("c".into(), MassProperties::new(3.0));
        let attached =
            tree_b.attach_with_reroot(c_b, p_b, DVec3::new(1.0, 2.0, 3.0), DMat3::IDENTITY);

        assert_eq!(attached, c_b, "root-subject reroot returns child unchanged");
        // Composite mass / center-of-mass invariant — both trees should
        // produce identical composite_properties on the parent.
        let mp_a = tree_a.get(p_a).composite_properties;
        let mp_b = tree_b.get(p_b).composite_properties;
        assert_eq!(mp_a.mass, mp_b.mass);
        assert_vec3_close(mp_a.position, mp_b.position, 1e-12, "composite CoM matches");
    }

    /// Chained-attach reroot: build (a ← b), then call
    /// `attach_with_reroot(b, c)`. JEOD semantics: since b already has a
    /// parent (a is its root), the *root* (a) gets rerooted under c, so
    /// post-call the tree shape is (c ← a ← b), and a's geometric
    /// placement under c is chosen so b's struct frame ends up where the
    /// caller asked for.
    ///
    /// Concretely: a hosts b via offset = (1, 0, 0) (identity rotation).
    /// We then call attach_with_reroot(b, c, offset=(5, 0, 0), I) — i.e.
    /// "place b's struct origin at +5 along c's +x axis". Post-reroot
    /// a's struct origin should be at (4, 0, 0) in c's struct (since
    /// b is at (1, 0, 0) in a's struct, so a's origin is at (5-1, 0, 0)
    /// in c's struct — both rotations are identity).
    #[test]
    fn attach_with_reroot_chained_subject() {
        let mut tree = MassTree::new();
        let a = tree.add_root("a".into(), MassProperties::new(2.0));
        let b = tree.add_root("b".into(), MassProperties::new(3.0));
        let c = tree.add_root("c".into(), MassProperties::new(4.0));
        // Build a's tree first.
        tree.attach(b, a, DVec3::new(1.0, 0.0, 0.0), DMat3::IDENTITY);
        assert_eq!(tree.root_of(b), a);
        assert_eq!(tree.root_of(a), a);

        // Reroot b under c: subject is b (non-root), JEOD logic auto-
        // attaches a (b's root) instead.
        let attached = tree.attach_with_reroot(b, c, DVec3::new(5.0, 0.0, 0.0), DMat3::IDENTITY);
        assert_eq!(attached, a, "reroot returns the subject's existing root");

        // Topology: c is now the root, a sits under c, b sits under a.
        assert_eq!(tree.parent(a), Some(c));
        assert_eq!(tree.parent(b), Some(a));
        assert_eq!(tree.root_of(b), c);
        assert_eq!(tree.root_of(a), c);
        assert_eq!(tree.root_of(c), c);

        // Geometry: a's struct origin in c's struct is (4, 0, 0) so
        // b's struct (still at +1 in a's struct) lands at (5, 0, 0) in
        // c's struct, matching the user's intent.
        let chain_to_b = tree.struct_chain_to_root(c, b);
        assert_vec3_close(
            chain_to_b.position,
            DVec3::new(5.0, 0.0, 0.0),
            1e-12,
            "subject b ends up where the caller asked, in c's struct frame",
        );
        assert_mat3_close(
            chain_to_b.t_parent_this,
            DMat3::IDENTITY,
            1e-12,
            "identity-rotation chain composes to identity",
        );

        // Composite mass on c is the sum of all three.
        let mp_c = tree.get(c).composite_properties;
        assert_eq!(mp_c.mass, 9.0, "c's composite is a + b + c");
    }

    /// Reroot must preserve the user-requested geometry under
    /// **rotated** existing-tree links: build (a ← b) where b sits at
    /// (1,0,0) in a with a 90° rotation, then reroot b under c with the
    /// user asking for b's struct at (3, 0, 0) in c. The recomputed
    /// edge (a under c) must take a's rotation into account so b's
    /// struct still lands at (3,0,0) when composed.
    #[test]
    fn attach_with_reroot_preserves_subject_geometry_under_rotation() {
        let r_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),
        );
        let mut tree = MassTree::new();
        let a = tree.add_root("a".into(), MassProperties::new(1.0));
        let b = tree.add_root("b".into(), MassProperties::new(1.0));
        let c = tree.add_root("c".into(), MassProperties::new(1.0));
        tree.attach(b, a, DVec3::new(1.0, 0.0, 0.0), r_z90);

        // User asks for b's struct at (3, 0, 0) in c, with identity
        // rotation between c struct and b struct.
        let user_offset = DVec3::new(3.0, 0.0, 0.0);
        let user_t_pc_subject = DMat3::IDENTITY;
        let _ = tree.attach_with_reroot(b, c, user_offset, user_t_pc_subject);

        // After reroot, walk c → a → b geometrically and confirm.
        let chain = tree.struct_chain_to_root(c, b);
        assert_vec3_close(
            chain.position,
            user_offset,
            1e-12,
            "subject ends up at the user-requested offset in new root struct",
        );
        assert_mat3_close(
            chain.t_parent_this,
            user_t_pc_subject,
            1e-12,
            "subject ends up with the user-requested rotation in new root struct",
        );
    }

    /// Same-tree guard: subject's existing root is exactly `parent_id`.
    /// This is the simple "single-edge cycle" case — `attach_with_reroot`
    /// must reject it before attempting the reroot.
    #[test]
    #[should_panic(expected = "already in the same tree")]
    fn attach_with_reroot_rejects_same_tree_parent_is_root() {
        let mut tree = MassTree::new();
        let a = tree.add_root("a".into(), MassProperties::new(1.0));
        let b = tree.add_root("b".into(), MassProperties::new(1.0));
        // Build (a ← b); a is b's root.
        tree.attach(b, a, DVec3::ZERO, DMat3::IDENTITY);
        // Subject = b, parent = a (= root_of(b)). Same-tree → must panic
        // with the named diagnostic.
        let _ = tree.attach_with_reroot(b, a, DVec3::ZERO, DMat3::IDENTITY);
    }

    /// Same-tree guard: parent is a *non-root* member of the subject's
    /// existing tree (e.g. a sibling or descendant of the subject's
    /// root). The walk-up check `child_root != parent_id` alone would
    /// miss this case because the parent is not the root; without the
    /// `root_of(parent_id)` comparison, control would fall through to
    /// `attach`'s cycle walk which fires with a less informative
    /// message. JEOD's `attach_validate_parent` (`mass_attach.cc:373`)
    /// rejects this via `parent.get_root_body() == get_root_body()`.
    #[test]
    #[should_panic(expected = "already in the same tree")]
    fn attach_with_reroot_rejects_same_tree_parent_is_sibling() {
        let mut tree = MassTree::new();
        let a = tree.add_root("a".into(), MassProperties::new(1.0));
        let b = tree.add_root("b".into(), MassProperties::new(1.0));
        let c = tree.add_root("c".into(), MassProperties::new(1.0));
        // Build (a ← b) and (a ← c). b and c are siblings under root a.
        tree.attach(b, a, DVec3::new(1.0, 0.0, 0.0), DMat3::IDENTITY);
        tree.attach(c, a, DVec3::new(0.0, 1.0, 0.0), DMat3::IDENTITY);
        assert_eq!(tree.root_of(b), a);
        assert_eq!(tree.root_of(c), a);
        // Subject = b (root_of = a), parent = c (root_of = a, but c ≠ a).
        // The old `child_root != parent_id` check would pass (a ≠ c) and
        // hand off to `attach`, which would then panic with its cycle
        // message. The fix detects same-tree here and panics with the
        // intended diagnostic naming the shared root.
        let _ = tree.attach_with_reroot(b, c, DVec3::ZERO, DMat3::IDENTITY);
    }
}