Assertion failure: position change, enabled state change, and missing MODIFIED flag

[Johannes0021]

// Error ------------------------------------------------

Ball altitude: 9.998297

thread 'main' panicked at /home/johannes/.cargo/registry/src/index.crates.io-1949cf8c6b5b557f/rapier2d-0.23.0/src/geometry/broad_phase_multi_sap/sap_utils.rs:36:13:
assertion failed: e.floor() < RegionKey::MAX as Real
note: run with RUST_BACKTRACE=1 environment variable to display a backtrace

// What happened ----------------------------------------

1. Modify the translation or rotation (RigidBodyChanges::POSITION).
2. Disable the rigid body.
3. Step the simulation.

// Reproduce --------------------------------------------

In the for loop at the bottom.

use rapier2d::prelude::*;

fn main() {
    let mut rigid_body_set = RigidBodySet::new();
    let mut collider_set = ColliderSet::new();

    /* Create the ground. */
    let collider = ColliderBuilder::cuboid(100.0, 0.1).build();
    collider_set.insert(collider);

    /* Create the bouncing ball. */
    let rigid_body = RigidBodyBuilder::dynamic()
        .translation(vector![0.0, 10.0])
        .build();
    let collider = ColliderBuilder::ball(0.5).restitution(0.7).build();
    let ball_body_handle = rigid_body_set.insert(rigid_body);
    collider_set.insert_with_parent(collider, ball_body_handle, &mut rigid_body_set);

    /* Create other structures necessary for the simulation. */
    let gravity = vector![0.0, -9.81];
    let integration_parameters = IntegrationParameters::default();
    let mut physics_pipeline = PhysicsPipeline::new();
    let mut island_manager = IslandManager::new();
    let mut broad_phase = DefaultBroadPhase::new();
    let mut narrow_phase = NarrowPhase::new();
    let mut impulse_joint_set = ImpulseJointSet::new();
    let mut multibody_joint_set = MultibodyJointSet::new();
    let mut ccd_solver = CCDSolver::new();
    let mut query_pipeline = QueryPipeline::new();
    let physics_hooks = ();
    let event_handler = ();

    /* Run the game loop, stepping the simulation once per frame. */
    for _ in 0..200 {
        physics_pipeline.step(
            &gravity,
            &integration_parameters,
            &mut island_manager,
            &mut broad_phase,
            &mut narrow_phase,
            &mut rigid_body_set,
            &mut collider_set,
            &mut impulse_joint_set,
            &mut multibody_joint_set,
            &mut ccd_solver,
            Some(&mut query_pipeline),
            &physics_hooks,
            &event_handler,
        );

        let ball_body = &mut rigid_body_set[ball_body_handle];
        println!("Ball altitude: {}", ball_body.translation().y);

	// This causes the error:
	// Modify the translation (or rotation) (RigidBodyChanges::POSITION)
	// Modify enabled
        ball_body.set_translation(vector![0.0, 0.0], true);
        // ball_body.set_rotation(nalgebra::UnitComplex::new(1.0), true);
        ball_body.set_enabled(false);
    }
}

// Solution (maybe) -------------------------------------------

rapier/src/dynamics/rigid_body_components.rs
line 970

In the function update_positions in the RigidBodyColliders impl:

pub fn update_positions(
    &self,
    colliders: &mut ColliderSet,
    modified_colliders: &mut Vec<ColliderHandle>,
    parent_pos: &Isometry<Real>,
) {
for handle in &self.0 {
    // NOTE: the ColliderParent component must exist if we enter this method.
    let co = colliders.index_mut_internal(*handle);
    let new_pos = parent_pos * co.parent.as_ref().unwrap().pos_wrt_parent;

    if !co.changes.contains(ColliderChanges::MODIFIED) {
        modified_colliders.push(*handle);
    }

    // Set the modification flag so we can benefit from the modification-tracking
    // when updating the narrow-phase/broad-phase afterwards.
    co.changes |= ColliderChanges::POSITION;
    co.pos = ColliderPosition(new_pos);
}
}

The handle is only added to modified_colliders if it does not already contain ColliderChanges::MODIFIED.
However, I believe the ColliderChanges::MODIFIED flag must be inserted into the changes when pushing
the handle to modified_colliders.

rapier/src/pipeline/user_changes.rs

• line 141 and 142
  update_positions is called and colliders may be added to the modified_colliders

• line 154 and 170
  Collider handles are added to the modified_colliders if the ColliderChanges::MODIFIED is not set

If a collider is pushed to modified_colliders in the update_positions function without setting the MODIFIED flag,
it can be pushed to modified_colliders twice.

Solution:
rapier/src/dynamics/rigid_body_components.rs
in the update_positions function:

    if !co.changes.contains(ColliderChanges::MODIFIED) {
        co.changes |= ColliderChanges::MODIFIED; // Insert ColliderChangesMODIFIED

        modified_colliders.push(*handle);
    }

// Also -----------------------------------------------

rapier/src/geometry/collider_set.rs
line 69

In the funktion iter_mut

/// Iterates mutably through all the colliders on this set.
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
    self.modified_colliders.clear();
    let modified_colliders = &mut self.modified_colliders;
    self.colliders.iter_mut().map(move |(h, b)| {
        modified_colliders.push(ColliderHandle(h));
        (ColliderHandle(h), b)
    })
}

1. self.modified_colliders.clear() is called at the beginning, resetting the list.
2. Every collider handle is then pushed into modified_colliders inside the map function without first setting the MODIFIED flag.

/// Iterates mutably through all the colliders on this set.
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
    self.modified_colliders.clear();
    let modified_colliders = &mut self.modified_colliders;
    self.colliders.iter_mut().map(move |(h, b)| {
        b.changes |= ColliderChanges::MODIFIED; // Ensure the MODIFIED flag is set
        modified_colliders.push(ColliderHandle(h));
        (ColliderHandle(h), b)
    })
}

[Johannes0021]

In the step function in rapier/src/pipeline/physics_pipeline.rs

Here in the handle_user_changes_to_rigid_bodies function some colliders are added multiple times to modified_bodies

// Apply modifications.
let mut modified_colliders = colliders.take_modified();
let mut removed_colliders = colliders.take_removed();

super::user_changes::handle_user_changes_to_colliders(
    bodies,
    colliders,
    &modified_colliders[..],
);

let mut modified_bodies = bodies.take_modified();
super::user_changes::handle_user_changes_to_rigid_bodies(  // Here ------------------------------
    Some(islands),
    bodies,
    colliders,
    impulse_joints,
    multibody_joints,
    &modified_bodies,
    &mut modified_colliders,
);

Here disabled colliders are treated as if they were removed.
Some are added multiple times because of the code above.

// Disabled colliders are treated as if they were removed.
// NOTE: this must be called here, after handle_user_changes_to_rigid_bodies to take into
//       account colliders disabled because of their parent rigid-body.
removed_colliders.extend(
    modified_colliders
        .iter()
        .copied()
        .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)),
    );

In rapier/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
in the update function line 598

// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);

Then line 154 -> self.predelete_proxy(proxy_id);
is called multiple times for the same collider

fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
    // For each removed collider, remove the corresponding proxy.
    for removed in removed_colliders {
        if let Some(proxy_id) = self.colliders_proxy_ids.get(removed).copied() {
            self.predelete_proxy(proxy_id);
        }
    }
}

In rapier/src/geometry/broad_phase_multi_sap/sap_layer.rs
in the predelete_proxy function

// Discretize the Aabb to find the regions that need to be invalidated.
let proxy_aabb = &mut proxies[proxy_index].aabb;
let start = super::point_key(proxy_aabb.mins, self.region_width);
let end = super::point_key(proxy_aabb.maxs, self.region_width);

// Set the Aabb of the proxy to a very large value.
proxy_aabb.mins.coords.fill(DELETED_AABB_VALUE);
proxy_aabb.maxs.coords.fill(DELETED_AABB_VALUE);

In the predelete_proxy function, the AABB of a proxy is set to an invalid value with DELETED_AABB_VALUE. If this function is called multiple times on the same collider, it causes an assertion failure in point_key.

This happens because the collider is added multiple times to the removed_colliders list, which occurs because it is present multiple times in the modified_colliders list.

[Vrixyz]

Thanks for the thorough investigation! I think your conclusions are correct, I'll work towards verifying and applying those fixes + add no-regression tests.

[Johannes0021]

Sounds good, glad I could help!