MultibodyJointSet: two panics — stale rb2mb after remove(), and fill_jacobians OOB on branching trees

[gdemoya]

Summary

Two bugs in MultibodyJointSet (rapier3d 0.31.0) that panic during normal usage of multibody joints for vehicle-like branching trees.

Related issues: #382 (closed, rapier2d 0.14, same class of bugs), #906 (open, Multibody::append() overflow)

Bug 1: MultibodyJointSet::remove() leaves stale rb2mb entries → iter() panics

When removing a joint that isolates a single-link body, remove() removes the multibody from the arena but does not clean up the isolated body's rb2mb entry. The stale entry retains the old MultibodyIndex (pointing to a freed arena slot) and a non-zero link.id.

Subsequently, MultibodyJointSet::iter() panics at multibody_joint_set.rs:126:

// iter() dereferences the stale multibody index:
let mb = &self.multibodies[link.multibody.0]; // panics: "No element at index"

The bug is in the num_links() == 1 branch of remove() (~line 239):

if multibody.num_links() == 1 {
    // Removes the graph node but does NOT remove or update the rb2mb entry.
    let isolated_link = multibody.link(0).unwrap();
    let isolated_graph_id = self.rb2mb.get(isolated_link.rigid_body.0).unwrap().graph_id;
    if let Some(other) = self.connectivity_graph.remove_node(isolated_graph_id) {
        self.rb2mb.get_mut(other.0).unwrap().graph_id = isolated_graph_id;
    }
    // BUG: rb2mb[isolated_link.rigid_body] still has stale multibody index + non-zero link.id
}

Workaround: remove_multibody_articulations() properly cleans up all rb2mb entries for the entire multibody.

Reproduction (Bug 1)

use rapier3d::prelude::*;

fn main() {
    let mut bodies = RigidBodySet::new();
    let mut multibody_joints = MultibodyJointSet::new();

    let a = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]).additional_mass(1.0));
    let b = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1.0, 5.0, 0.0]).additional_mass(1.0));
    let c = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![2.0, 5.0, 0.0]).additional_mass(1.0));

    let joint = RevoluteJointBuilder::new(Vector::z_axis());
    multibody_joints.insert(a, b, joint.clone(), true);
    let handle_bc = multibody_joints.insert(b, c, joint, true).unwrap();

    // Remove B->C, isolating C as a single-link body
    multibody_joints.remove(handle_bc, true);

    // This panics: "No element at index"
    let _count = multibody_joints.iter().count();
}

Bug 2: Multibody::fill_jacobians index out of bounds on branching trees

When a multibody tree has branches (e.g., a vehicle chassis connected to 4 axle sub-chains), body_jacobians doesn't grow correctly during Multibody::append(). The solver panics at multibody.rs:1213:

self.body_jacobians[link.internal_id] // panics: "index out of bounds: the len is 9 but the index is 9"

This happens specifically when append() merges a multi-link subtree (not a single-body). The internal_id rebasing in append() appears to have an off-by-one that only manifests when mb2.num_links() > 1.

Reproduction (Bug 2)

use rapier3d::prelude::*;

fn main() {
    let mut bodies = RigidBodySet::new();
    let mut colliders = ColliderSet::new();
    let mut multibody_joints = MultibodyJointSet::new();
    let mut impulse_joints = ImpulseJointSet::new();

    // Ground
    let ground = bodies.insert(RigidBodyBuilder::fixed().translation(vector![0.0, -0.5, 0.0]));
    colliders.insert_with_parent(ColliderBuilder::cuboid(50.0, 0.5, 50.0), ground, &mut bodies);

    let make_body = |bodies: &mut RigidBodySet, colliders: &mut ColliderSet, pos: Vector<f32>| {
        let h = bodies.insert(RigidBodyBuilder::dynamic().translation(pos).additional_mass(5.0));
        colliders.insert_with_parent(ColliderBuilder::ball(0.3), h, bodies);
        h
    };

    // Chassis
    let chassis = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0.0, 1.5, 0.0]).additional_mass(50.0));
    colliders.insert_with_parent(ColliderBuilder::cuboid(2.0, 0.3, 1.0), chassis, &mut bodies);

    // Build 4 separate chains FIRST (key: mb2 must have >1 link to trigger the bug)
    let axle_fl = make_body(&mut bodies, &mut colliders, vector![-1.5, 1.2, 1.2]);
    let mangueta_fl = make_body(&mut bodies, &mut colliders, vector![-1.5, 1.0, 1.5]);
    let wheel_fl = make_body(&mut bodies, &mut colliders, vector![-1.5, 0.5, 1.8]);
    let joint = || RevoluteJointBuilder::new(Vector::x_axis());
    multibody_joints.insert(axle_fl, mangueta_fl, joint(), true);
    multibody_joints.insert(mangueta_fl, wheel_fl, joint(), true);

    let axle_fr = make_body(&mut bodies, &mut colliders, vector![1.5, 1.2, 1.2]);
    let mangueta_fr = make_body(&mut bodies, &mut colliders, vector![1.5, 1.0, 1.5]);
    let wheel_fr = make_body(&mut bodies, &mut colliders, vector![1.5, 0.5, 1.8]);
    multibody_joints.insert(axle_fr, mangueta_fr, joint(), true);
    multibody_joints.insert(mangueta_fr, wheel_fr, joint(), true);

    let axle_rl = make_body(&mut bodies, &mut colliders, vector![-1.5, 1.2, -1.2]);
    let wheel_rl = make_body(&mut bodies, &mut colliders, vector![-1.5, 0.5, -1.5]);
    multibody_joints.insert(axle_rl, wheel_rl, joint(), true);

    let axle_rr = make_body(&mut bodies, &mut colliders, vector![1.5, 1.2, -1.2]);
    let wheel_rr = make_body(&mut bodies, &mut colliders, vector![1.5, 0.5, -1.5]);
    multibody_joints.insert(axle_rr, wheel_rr, joint(), true);

    // Connect chains to chassis (triggers append() with multi-link mb2)
    multibody_joints.insert(chassis, axle_fl, joint(), true);
    multibody_joints.insert(chassis, axle_fr, joint(), true);
    multibody_joints.insert(chassis, axle_rl, joint(), true);
    multibody_joints.insert(chassis, axle_rr, joint(), true);

    // Step simulation — panics in fill_jacobians
    let gravity = vector![0.0, -9.81, 0.0];
    let mut pipeline = PhysicsPipeline::new();
    let mut islands = IslandManager::new();
    let mut broad_phase = DefaultBroadPhase::new();
    let mut narrow_phase = NarrowPhase::new();
    let mut ccd = CCDSolver::new();
    for _ in 0..10 {
        pipeline.step(&gravity, &IntegrationParameters::default(), &mut islands,
            &mut broad_phase, &mut narrow_phase, &mut bodies, &mut colliders,
            &mut impulse_joints, &mut multibody_joints, &mut ccd, &(), &());
    }
}

Note on Bug 2: If joints are inserted one-at-a-time from root to leaf (so mb2 is always a single-body multibody), the bug doesn't trigger. It only manifests when append() merges a multi-link subtree, which happens when sub-chains are built separately and then connected to a parent.

Environment

• rapier3d: 0.31.0
• Rust: stable (edition 2024)