Sensors and large heightfields are slow together

[oskargustafsson]

In my game I have a decently sized heightmap representing the ground and a player around which I want to put a sensor, in order to detect when it is near things. Adding that sensor makes things terribly slow though, to the point where it feels like it could be a bug.

Repro (rapier3d = "0.12.0")

use rapier3d::{
    na::{Dynamic, VecStorage},
    prelude::*,
};

fn main() {
    let mut rigid_body_set = RigidBodySet::new();
    let mut collider_set = ColliderSet::new();
    const HEIGHTFIELD_SIDE: f32 = 256.0;

    {
        const HEIGHTFIELD_NUM_SUBDIVS: usize = 2048;
        let heights: Vec<f32> = vec![1.0; HEIGHTFIELD_NUM_SUBDIVS * HEIGHTFIELD_NUM_SUBDIVS];
        let rigid_body_handle = rigid_body_set.insert(RigidBodyBuilder::fixed());
        let collider = ColliderBuilder::heightfield(
            DMatrix::from_data(VecStorage::new(
                Dynamic::new(HEIGHTFIELD_NUM_SUBDIVS),
                Dynamic::new(HEIGHTFIELD_NUM_SUBDIVS),
                heights,
            )),
            vector!(HEIGHTFIELD_SIDE, 1.0, HEIGHTFIELD_SIDE),
        )
        .translation(0.5 * vector!(HEIGHTFIELD_SIDE, 0.0, HEIGHTFIELD_SIDE))
        .build();
        collider_set.insert_with_parent(collider, rigid_body_handle, &mut rigid_body_set);
    }

    let player_body_handle = {
        let rigid_body_handle = rigid_body_set.insert(
            RigidBodyBuilder::dynamic()
                .translation(vector!(0.5 * HEIGHTFIELD_SIDE, 4.0, 0.5 * HEIGHTFIELD_SIDE))
                .lock_rotations()
                .build(),
        );

        let player_collider = ColliderBuilder::capsule_y(0.5, 0.5)
            .restitution(0.0)
            .build();
        collider_set.insert_with_parent(player_collider, rigid_body_handle, &mut rigid_body_set);

        // This sensor makes the simulation slow.
        let player_sensor = ColliderBuilder::ball(1.1).sensor(true).build();
        collider_set.insert_with_parent(player_sensor, rigid_body_handle, &mut rigid_body_set);

        rigid_body_handle
    };

    let gravity = vector![0.0, -9.81, 0.0];
    let integration_parameters = IntegrationParameters::default();
    let mut physics_pipeline = PhysicsPipeline::new();
    let mut island_manager = IslandManager::new();
    let mut broad_phase = BroadPhase::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();

    for _ in 0..50 {
        let iter_start = std::time::Instant::now();
        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,
            &(),
            &(),
        );
        let player_body = &rigid_body_set[player_body_handle];
        println!(
            "Iter time: {:?}, player.y: {}",
            iter_start.elapsed(),
            player_body.translation().y
        );
    }
}

Output

...
Iter time: 141.909µs, player.y: 2.378625
Iter time: 170.593µs, player.y: 2.2832499
Iter time: 127.047µs, player.y: 2.18515
Iter time: 21.046758119s, player.y: 2.0843248  <---- this is where the player sensor hits the ground
Iter time: 21.025214909s, player.y: 1.9807749
Iter time: 20.945551641s, player.y: 1.99394
...

[sebcrozet]

Thank you for reporting this. That does look like a bug.

[oskargustafsson]

Any progress on this? I'm still having the same issue with rapier3d = "0.17.2"

Or is there maybe some other way of achieving the same thing (player with sensor + world with heightfield)?