feat: switch to the new Bvh from parry for the broad-phase (#853)

* feat: switch to the new Bvh from parry for the broad-phase

* chore: cargo fmt + update testbed

* chore: remove the multi-grid SAP broad-phase

* fix soft-ccd handling in broad-phase

* Fix contact cleanup in broad-phase after collider removal

* chore: clippy fixes

* fix CCD regression

* chore: update changelog

* fix build with the parallel feature enabled

* chore: remove the now useless broad-phase proxy index from colliders

* fix tests

CHANGELOG.md

@@ -1,3 +1,15 @@
+## Unreleased
+
+### Modified
+
+- Replace the hierarchical SAP broad-phase by a broad-phase based on parry’s new BVH structure.
+- The `QueryPipeline` is now and ephemeral object obtained from the broad-phase with `broad_phase.as_query_pipeline()`.
+  It no longer needs to be updated separately from the broad-phase.
+
+### Fixed
+
+- Fix NaN resulting from non-clamped input to simd_asin in angular motor solver.
+
 ## v0.26.1 (23 May 2025)
 
 ### Added

Cargo.toml

@@ -32,7 +32,7 @@ needless_lifetimes = "allow"
 #parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
 #parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
 #nalgebra = { path = "../nalgebra" }
-
+#simba = { path = "../simba" }
 
 #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
 #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }

benchmarks2d/Cargo.toml

@@ -2,7 +2,7 @@
 name = "rapier-benchmarks-2d"
 version = "0.1.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
-edition = "2021"
+edition = "2024"
 
 [features]
 parallel = ["rapier2d/parallel", "rapier_testbed2d/parallel"]

benchmarks2d/balls2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/boxes2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/capsules2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/convex_polygons2.rs

@@ -1,7 +1,7 @@
 use rand::distributions::{Distribution, Standard};
-use rand::{rngs::StdRng, SeedableRng};
+use rand::{SeedableRng, rngs::StdRng};
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/heightfield2.rs

@@ -1,6 +1,6 @@
+use rapier_testbed2d::Testbed;
 use rapier2d::na::DVector;
 use rapier2d::prelude::*;
-use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/joint_ball2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/joint_fixed2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/joint_prismatic2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/pyramid2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks2d/vertical_stacks2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/Cargo.toml

@@ -2,7 +2,7 @@
 name = "rapier-benchmarks-3d"
 version = "0.1.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
-edition = "2021"
+edition = "2024"
 
 [features]
 parallel = ["rapier3d/parallel", "rapier_testbed3d/parallel"]
@@ -14,6 +14,7 @@ enhanced-determinism = ["rapier3d/enhanced-determinism"]
 [dependencies]
 rand = "0.8"
 Inflector = "0.11"
+oorandom = "11"
 
 [dependencies.rapier_testbed3d]
 path = "../crates/rapier_testbed3d"

benchmarks3d/all_benchmarks3.rs

@@ -20,10 +20,12 @@ mod joint_fixed3;
 mod joint_prismatic3;
 mod joint_revolute3;
 mod keva3;
+mod many_kinematics3;
 mod many_pyramids3;
 mod many_sleep3;
 mod many_static3;
 mod pyramid3;
+mod ray_cast3;
 mod stacks3;
 mod trimesh3;
 
@@ -47,9 +49,8 @@ fn parse_command_line() -> Command {
     Command::RunAll
 }
 
-pub fn main() {
+#[allow(clippy::type_complexity)]
-    let command = parse_command_line();
+pub fn demo_builders() -> Vec<(&'static str, fn(&mut Testbed))> {
-
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Balls", balls3::init_world),
         ("Boxes", boxes3::init_world),
@@ -57,6 +58,7 @@ pub fn main() {
         ("CCD", ccd3::init_world),
         ("Compound", compound3::init_world),
         ("Convex polyhedron", convex_polyhedron3::init_world),
+        ("Many kinematics", many_kinematics3::init_world),
         ("Many static", many_static3::init_world),
         ("Many sleep", many_sleep3::init_world),
         ("Heightfield", heightfield3::init_world),
@@ -69,6 +71,7 @@ pub fn main() {
         ("ImpulseJoint prismatic", joint_prismatic3::init_world),
         ("Many pyramids", many_pyramids3::init_world),
         ("Keva tower", keva3::init_world),
+        ("Ray cast", ray_cast3::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.
@@ -77,6 +80,12 @@ pub fn main() {
         (true, false) => Ordering::Greater,
         (false, true) => Ordering::Less,
     });
+    builders
+}
+
+pub fn main() {
+    let command = parse_command_line();
+    let builders = demo_builders();
 
     match command {
         Command::Run(demo) => {
@@ -86,7 +95,7 @@ pub fn main() {
             {
                 TestbedApp::from_builders(vec![builders[i]]).run()
             } else {
-                eprintln!("Invalid example to run provided: '{}'", demo);
+                eprintln!("Invalid example to run provided: '{demo}'");
             }
         }
         Command::RunAll => TestbedApp::from_builders(builders).run(),

benchmarks3d/balls3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/boxes3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*
@@ -24,21 +24,21 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Create the cubes
      */
-    let num = 8;
+    let num = 10;
     let rad = 1.0;
 
-    let shift = rad * 2.0 + rad;
+    let shift = rad * 2.0;
     let centerx = shift * (num / 2) as f32;
     let centery = shift / 2.0;
     let centerz = shift * (num / 2) as f32;
 
-    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+    let mut offset = -(num as f32) * (rad * 2.0) * 0.5;
 
-    for j in 0usize..47 {
+    for j in 0usize..num {
         for i in 0..num {
             for k in 0usize..num {
                 let x = i as f32 * shift - centerx + offset;
-                let y = j as f32 * shift + centery + 3.0;
+                let y = j as f32 * shift + centery;
                 let z = k as f32 * shift - centerz + offset;
 
                 // Build the rigid body.

benchmarks3d/capsules3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/ccd3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/compound3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/convex_polyhedron3.rs

@@ -1,7 +1,7 @@
 use rand::distributions::{Distribution, Standard};
-use rand::{rngs::StdRng, SeedableRng};
+use rand::{SeedableRng, rngs::StdRng};
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/heightfield3.rs

@@ -1,6 +1,6 @@
+use rapier_testbed3d::Testbed;
 use rapier3d::na::ComplexField;
 use rapier3d::prelude::*;
-use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/joint_ball3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/joint_fixed3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/joint_prismatic3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/joint_revolute3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/keva3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn build_block(
     testbed: &mut Testbed,
@@ -99,7 +99,6 @@ pub fn init_world(testbed: &mut Testbed) {
     // These should only be set to odd values otherwise
     // the blocks won't align in the nicest way.
     let numy = [0, 9, 13, 17, 21, 41];
-    let mut num_blocks_built = 0;
 
     for i in (1..=5).rev() {
         let numx = i;
@@ -115,11 +114,8 @@ pub fn init_world(testbed: &mut Testbed) {
             (numx, numy, numz),
         );
         block_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0;
-        num_blocks_built += numx * numy * numz;
     }
 
-    println!("Num keva blocks: {}", num_blocks_built);
-
     /*
      * Set up the testbed.
      */

benchmarks3d/many_kinematics3.rs

@@ -0,0 +1,68 @@
+use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Create the balls
+     */
+    let num = 30;
+    let rad = 1.0;
+
+    let shift = rad * 6.0 + 1.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift * (num as f32) / 2.0;
+    let centerz = shift * (num as f32) / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num {
+            for k in 0..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift - centery;
+                let z = k as f32 * shift - centerz;
+
+                // Build the rigid body.
+                let velocity = Vector::new(
+                    rand::random::<f32>() - 0.5,
+                    rand::random::<f32>() - 0.5,
+                    rand::random::<f32>() - 0.5,
+                ) * 30.0;
+                let rigid_body = RigidBodyBuilder::new(RigidBodyType::KinematicVelocityBased)
+                    .translation(vector![x, y, z])
+                    .linvel(velocity);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::ball(rad);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    testbed.add_callback(move |_, physics, _, _| {
+        for (_, rb) in physics.bodies.iter_mut() {
+            let mut linvel = *rb.linvel();
+
+            for dim in 0..3 {
+                if (linvel[dim] > 0.0 && rb.translation()[dim] > (shift * num as f32) / 2.0)
+                    || (linvel[dim] < 0.0 && rb.translation()[dim] < -(shift * num as f32) / 2.0)
+                {
+                    linvel[dim] = -linvel[dim];
+                }
+            }
+
+            rb.set_linvel(linvel, false);
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}

benchmarks3d/many_pyramids3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 fn create_pyramid(
     bodies: &mut RigidBodySet,

benchmarks3d/many_sleep3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/many_static3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

benchmarks3d/pyramid3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 fn create_pyramid(
     bodies: &mut RigidBodySet,

benchmarks3d/ray_cast3.rs

@@ -0,0 +1,89 @@
+use rapier_testbed3d::{Color, Testbed};
+use rapier3d::prelude::*;
+
+pub fn init_world(testbed: &mut Testbed) {
+    let settings = testbed.example_settings_mut();
+
+    // NOTE: this demo is a bit special. It takes as a setting the builder of another demo,
+    //       builds it, and add a ton of rays into it. This gives us an easy way to check
+    //       ray-casting in a wide variety of situations.
+    let demos: Vec<_> = crate::demo_builders()
+        .into_iter()
+        .filter(|(_, builder)| {
+            !std::ptr::fn_addr_eq(*builder, self::init_world as fn(&mut Testbed))
+        })
+        .collect();
+    let demo_names: Vec<_> = demos.iter().map(|(name, _)| name.to_string()).collect();
+    let selected = settings.get_or_set_string("Scene", 0, demo_names);
+    demos[selected].1(testbed);
+
+    /*
+     * Cast rays at each frame.
+     */
+    let ray_ball_radius = 100.0;
+    let ray_ball = Ball::new(ray_ball_radius);
+    let (ray_origins, _) = ray_ball.to_trimesh(100, 100);
+    let rays: Vec<_> = ray_origins
+        .into_iter()
+        .map(|pt| Ray::new(pt, -pt.coords.normalize()))
+        .collect();
+    let mut centered_rays = rays.clone();
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        let Some(graphics) = graphics else {
+            return;
+        };
+
+        // Re-center the ray relative to the current position of all objects.
+        // This ensures demos with falling objects don’t end up with a boring situation
+        // where all the rays point into the void.
+        let mut center = Point::origin();
+        for (_, b) in physics.bodies.iter() {
+            center += b.translation();
+        }
+        center /= physics.bodies.len() as Real;
+
+        for (centered, ray) in centered_rays.iter_mut().zip(rays.iter()) {
+            centered.origin = center + ray.origin.coords;
+        }
+
+        // Cast the rays.
+        let t1 = std::time::Instant::now();
+        let max_toi = ray_ball_radius - 1.0;
+
+        let Some(broad_phase) = physics.broad_phase.downcast_ref::<BroadPhaseBvh>() else {
+            return;
+        };
+        let query_pipeline = broad_phase.as_query_pipeline(
+            physics.narrow_phase.query_dispatcher(),
+            &physics.bodies,
+            &physics.colliders,
+            Default::default(),
+        );
+
+        for ray in &centered_rays {
+            if let Some((_, toi)) = query_pipeline.cast_ray(ray, max_toi, true) {
+                let a = ray.origin;
+                let b = ray.point_at(toi);
+                graphics
+                    .gizmos
+                    .line(a.into(), b.into(), Color::srgba(0.0, 1.0f32, 0.0, 0.1));
+            } else {
+                let a = ray.origin;
+                let b = ray.point_at(max_toi);
+                graphics
+                    .gizmos
+                    .line(a.into(), b.into(), Color::srgba(1.0f32, 0.0, 0.0, 0.1));
+            }
+        }
+        let main_check_time = t1.elapsed().as_secs_f32();
+
+        if let Some(settings) = &mut graphics.settings {
+            settings.set_label("Ray count:", format!("{}", rays.len()));
+            settings.set_label(
+                "Ray-cast time",
+                format!("{:.2}ms", main_check_time * 1000.0,),
+            );
+        }
+    });
+}

benchmarks3d/stacks3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 fn create_tower_circle(
     bodies: &mut RigidBodySet,

benchmarks3d/trimesh3.rs

@@ -1,6 +1,6 @@
+use rapier_testbed3d::Testbed;
 use rapier3d::na::ComplexField;
 use rapier3d::prelude::*;
-use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

crates/rapier2d-f64/Cargo.toml

@@ -16,7 +16,8 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+rust-version = "1.86"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -68,7 +69,7 @@ vec_map = { version = "0.8", optional = true }
 web-time = { version = "1.1", optional = true }
 num-traits = "0.2"
 nalgebra = "0.33"
-parry2d-f64 = "0.21.1"
+parry2d-f64 = "0.22.0-beta.1"
 simba = "0.9"
 approx = "0.5"
 rayon = { version = "1", optional = true }

crates/rapier2d/Cargo.toml

@@ -16,7 +16,8 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+rust-version = "1.86"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -69,7 +70,7 @@ vec_map = { version = "0.8", optional = true }
 web-time = { version = "1.1", optional = true }
 num-traits = "0.2"
 nalgebra = "0.33"
-parry2d = "0.21.1"
+parry2d = "0.22.0-beta.1"
 simba = "0.9"
 approx = "0.5"
 rayon = { version = "1", optional = true }

crates/rapier3d-f64/Cargo.toml

@@ -16,7 +16,8 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+rust-version = "1.86"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -71,7 +72,7 @@ vec_map = { version = "0.8", optional = true }
 web-time = { version = "1.1", optional = true }
 num-traits = "0.2"
 nalgebra = "0.33"
-parry3d-f64 = "0.21.1"
+parry3d-f64 = "0.22.0-beta.1"
 simba = "0.9"
 approx = "0.5"
 rayon = { version = "1", optional = true }

crates/rapier3d-meshloader/Cargo.toml

@@ -16,7 +16,7 @@ categories = [
 ]
 keywords = ["physics", "joints", "multibody", "robotics", "urdf"]
 license = "Apache-2.0"
-edition = "2021"
+edition = "2024"
 
 [features]
 default = ["stl", "collada", "wavefront"]

crates/rapier3d-urdf/Cargo.toml

@@ -16,7 +16,7 @@ categories = [
 ]
 keywords = ["physics", "joints", "multibody", "robotics", "urdf"]
 license = "Apache-2.0"
-edition = "2021"
+edition = "2024"
 
 [features]
 stl = ["dep:rapier3d-meshloader", "rapier3d-meshloader/stl", "__meshloader_is_enabled"]

crates/rapier3d-urdf/src/lib.rs

@@ -497,7 +497,9 @@ fn urdf_to_colliders(
         }
         #[cfg(not(feature = "__meshloader_is_enabled"))]
         Geometry::Mesh { .. } => {
-            log::error!("Mesh loading is disabled by default. Enable one of the format features (`stl`, `collada`, `wavefront`) of `rapier3d-urdf` for mesh support.");
+            log::error!(
+                "Mesh loading is disabled by default. Enable one of the format features (`stl`, `collada`, `wavefront`) of `rapier3d-urdf` for mesh support."
+            );
         }
         #[cfg(feature = "__meshloader_is_enabled")]
         Geometry::Mesh { filename, scale } => {

crates/rapier3d/Cargo.toml

@@ -16,7 +16,8 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+rust-version = "1.86"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }
@@ -41,7 +42,7 @@ simd-nightly = [
 ]
 # Do not enable this feature directly. It is automatically
 # enabled with the "simd-stable" or "simd-nightly" feature.
-simd-is-enabled = ["dep:vec_map"]
+simd-is-enabled = []
 serde-serialize = [
     "nalgebra/serde-serialize",
     "parry3d/serde-serialize",
@@ -69,11 +70,11 @@ required-features = ["dim3", "f32"]
 
 
 [dependencies]
-vec_map = { version = "0.8", optional = true }
+vec_map = "0.8"
 web-time = { version = "1.1", optional = true }
 num-traits = "0.2"
 nalgebra = "0.33"
-parry3d = "0.21.1"
+parry3d = "0.22.0-beta.1"
 simba = "0.9"
 approx = "0.5"
 rayon = { version = "1", optional = true }
@@ -89,6 +90,11 @@ log = "0.4"
 ordered-float = "5"
 thiserror = "2"
 profiling = "1.0"
+smallvec = "1"
+
+# TODO: remove this, just for experiment.
+wide = "0.7"
+petgraph = "0.8"
 
 [dev-dependencies]
 bincode = "1"

crates/rapier_testbed2d-f64/Cargo.toml

@@ -14,7 +14,7 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }

crates/rapier_testbed2d/Cargo.toml

@@ -14,7 +14,7 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }

crates/rapier_testbed3d-f64/Cargo.toml

@@ -14,7 +14,7 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }

crates/rapier_testbed3d/Cargo.toml

@@ -14,7 +14,7 @@ categories = [
 ]
 keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
 license = "Apache-2.0"
-edition = "2021"
+edition = "2024"
 
 [badges]
 maintenance = { status = "actively-developed" }

examples2d/Cargo.toml

@@ -2,7 +2,7 @@
 name = "rapier-examples-2d"
 version = "0.1.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
-edition = "2021"
+edition = "2024"
 default-run = "all_examples2"
 
 [features]

examples2d/add_remove2.rs

@@ -1,5 +1,5 @@
-use rapier2d::{na::UnitComplex, prelude::*};
 use rapier_testbed2d::Testbed;
+use rapier2d::{na::UnitComplex, prelude::*};
 
 pub fn init_world(testbed: &mut Testbed) {
     let mut bodies = RigidBodySet::new();
@@ -24,7 +24,7 @@ pub fn init_world(testbed: &mut Testbed) {
 
     // Callback that will be executed on the main loop to handle proximities.
     testbed.add_callback(move |mut graphics, physics, _, state| {
-        let rot = state.time * -1.0;
+        let rot = -state.time;
         for rb_handle in &platform_handles {
             let rb = physics.bodies.get_mut(*rb_handle).unwrap();
             rb.set_next_kinematic_rotation(UnitComplex::new(rot));

examples2d/all_examples2.rs

@@ -20,6 +20,7 @@ mod debug_total_overlap2;
 mod debug_vertical_column2;
 mod drum2;
 mod heightfield2;
+mod inv_pyramid2;
 mod inverse_kinematics2;
 mod joint_motor_position2;
 mod joints2;
@@ -59,6 +60,7 @@ pub fn main() {
         ("Drum", drum2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Inverse kinematics", inverse_kinematics2::init_world),
+        ("Inv pyramid", inv_pyramid2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),
         ("One-way platforms", one_way_platforms2::init_world),

examples2d/ccd2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/character_controller2.rs

@@ -1,8 +1,8 @@
 use crate::utils::character;
 use crate::utils::character::CharacterControlMode;
+use rapier_testbed2d::Testbed;
 use rapier2d::control::{KinematicCharacterController, PidController};
 use rapier2d::prelude::*;
-use rapier_testbed2d::Testbed;
 use std::f32::consts::PI;
 
 pub fn init_world(testbed: &mut Testbed) {

examples2d/collision_groups2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/convex_polygons2.rs

@@ -1,7 +1,7 @@
 use rand::distributions::{Distribution, Standard};
-use rand::{rngs::StdRng, SeedableRng};
+use rand::{SeedableRng, rngs::StdRng};
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/damping2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/debug_box_ball2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/debug_compression2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/debug_intersection2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*
@@ -39,22 +39,29 @@ pub fn init_world(testbed: &mut Testbed) {
     testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
     testbed.look_at(point![0.0, 0.0], 50.0);
 
-    testbed.add_callback(move |graphics, physics, _, run| {
+    testbed.add_callback(move |mut graphics, physics, _, run| {
         let slow_time = run.timestep_id as f32 / 3.0;
-        let intersection = physics.query_pipeline.intersection_with_shape(
+
+        let Some(broad_phase) = physics.broad_phase.downcast_ref::<BroadPhaseBvh>() else {
+            return;
+        };
+        let query_pipeline = broad_phase.as_query_pipeline(
+            physics.narrow_phase.query_dispatcher(),
             &physics.bodies,
             &physics.colliders,
-            &Isometry::translation(slow_time.cos() * 10.0, slow_time.sin() * 10.0),
-            &Ball::new(rad / 2.0),
             QueryFilter::default(),
         );
 
-        if let Some(graphics) = graphics {
+        for intersection in query_pipeline.intersect_shape(
+            &Isometry::translation(slow_time.cos() * 10.0, slow_time.sin() * 10.0),
+            &Ball::new(rad / 2.0),
+        ) {
+            if let Some(graphics) = graphics.as_deref_mut() {
                 for (handle, _) in physics.bodies.iter() {
                     graphics.set_body_color(handle, [0.5, 0.5, 0.5]);
                 }
-            if let Some(intersection) = intersection {
+
-                let collider = physics.colliders.get(intersection).unwrap();
+                let collider = physics.colliders.get(intersection.0).unwrap();
                 let body_handle = collider.parent().unwrap();
 
                 graphics.set_body_color(body_handle, [1.0, 0.0, 0.0]);

examples2d/debug_total_overlap2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/debug_vertical_column2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/drum2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/heightfield2.rs

@@ -1,6 +1,6 @@
+use rapier_testbed2d::Testbed;
 use rapier2d::na::DVector;
 use rapier2d::prelude::*;
-use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/inv_pyramid2.rs

@@ -0,0 +1,47 @@
+use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 10.0;
+    let ground_thickness = 1.0;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_thickness);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 6;
+    let mut rad = 0.5;
+    let mut y = rad;
+
+    for _ in 0usize..num {
+        // Build the rigid body.
+        let rigid_body =
+            RigidBodyBuilder::dynamic().translation(vector![0.0, y + ground_thickness]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+        y += rad + rad * 2.0;
+        rad *= 2.0;
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/inverse_kinematics2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/joint_motor_position2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/joints2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/locked_rotations2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 // This shows a bug when a cylinder is in contact with a very large
 // but very thin cuboid. In this case the EPA returns an incorrect

examples2d/one_way_platforms2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 struct OneWayPlatformHook {
     platform1: ColliderHandle,

examples2d/platform2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/polyline2.rs

@@ -1,6 +1,6 @@
+use rapier_testbed2d::Testbed;
 use rapier2d::na::ComplexField;
 use rapier2d::prelude::*;
-use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/pyramid2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/restitution2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/rope_joints2.rs

@@ -1,8 +1,8 @@
 use crate::utils::character;
 use crate::utils::character::CharacterControlMode;
+use rapier_testbed2d::Testbed;
 use rapier2d::control::{KinematicCharacterController, PidController};
 use rapier2d::prelude::*;
-use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_arch.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_ball_and_chain.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_bridge.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_card_house.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_confined.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_far_pyramid.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_high_mass_ratio_1.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_high_mass_ratio_2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_high_mass_ratio_3.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_joint_grid.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/s2d_pyramid.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/sensor2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/trimesh2.rs

@@ -1,5 +1,5 @@
-use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
+use rapier2d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples2d/utils/character.rs

@@ -1,12 +1,12 @@
+use rapier_testbed2d::ui::egui::Align2;
+use rapier_testbed2d::{
+    KeyCode, PhysicsState, TestbedGraphics,
+    ui::egui::{ComboBox, Slider, Ui, Window},
+};
 use rapier2d::{
     control::{CharacterLength, KinematicCharacterController, PidController},
     prelude::*,
 };
-use rapier_testbed2d::ui::egui::Align2;
-use rapier_testbed2d::{
-    ui::egui::{ComboBox, Slider, Ui, Window},
-    KeyCode, PhysicsState, TestbedGraphics,
-};
 
 pub type CharacterSpeed = Real;
 
@@ -133,18 +133,27 @@ fn update_kinematic_controller(
 
     let character_body = &phx.bodies[character_handle];
     let character_collider = &phx.colliders[character_body.colliders()[0]];
+    let character_collider_pose = *character_collider.position();
+    let character_shape = character_collider.shared_shape().clone();
     let character_mass = character_body.mass();
 
+    let Some(broad_phase) = phx.broad_phase.downcast_ref::<BroadPhaseBvh>() else {
+        return;
+    };
+    let query_pipeline = broad_phase.as_query_pipeline_mut(
+        phx.narrow_phase.query_dispatcher(),
+        &mut phx.bodies,
+        &mut phx.colliders,
+        QueryFilter::new().exclude_rigid_body(character_handle),
+    );
+
     let mut collisions = vec![];
     let mvt = controller.move_shape(
         phx.integration_parameters.dt,
-        &phx.bodies,
+        &query_pipeline.as_ref(),
-        &phx.colliders,
+        &*character_shape,
-        &phx.query_pipeline,
+        &character_collider_pose,
-        character_collider.shape(),
+        desired_movement.cast::<Real>(),
-        character_collider.position(),
-        desired_movement,
-        QueryFilter::new().exclude_rigid_body(character_handle),
         |c| collisions.push(c),
     );
 
@@ -156,13 +165,10 @@ fn update_kinematic_controller(
 
     controller.solve_character_collision_impulses(
         phx.integration_parameters.dt,
-        &mut phx.bodies,
+        query_pipeline,
-        &phx.colliders,
+        &*character_shape,
-        &phx.query_pipeline,
-        character_collider.shape(),
         character_mass,
         &*collisions,
-        QueryFilter::new().exclude_rigid_body(character_handle),
     );
 
     let character_body = &mut phx.bodies[character_handle];

examples2d/voxels2.rs

@@ -1,6 +1,6 @@
+use rapier_testbed2d::Testbed;
 use rapier2d::parry::transformation::voxelization::FillMode;
 use rapier2d::prelude::*;
-use rapier_testbed2d::Testbed;
 
 const VOXEL_SIZE: Real = 0.1; // 0.25;
 

examples3d-f64/Cargo.toml

@@ -2,7 +2,7 @@
 name = "rapier-examples-3d-f64"
 version = "0.1.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
-edition = "2021"
+edition = "2024"
 default-run = "all_examples3-f64"
 
 [features]

examples3d-f64/debug_serialized3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 #[derive(serde::Deserialize)]
 struct State {
@@ -27,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) {
         state.multibody_joints,
     );
     testbed.harness_mut().physics.islands = state.islands;
-    testbed.harness_mut().physics.broad_phase = state.broad_phase;
+    testbed.harness_mut().physics.broad_phase = Box::new(state.broad_phase);
     testbed.harness_mut().physics.narrow_phase = state.narrow_phase;
     testbed.harness_mut().physics.ccd_solver = state.ccd_solver;
 

examples3d/Cargo.toml

@@ -2,7 +2,7 @@
 name = "rapier-examples-3d"
 version = "0.1.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
-edition = "2021"
+edition = "2024"
 default-run = "all_examples3"
 
 [features]

examples3d/ccd3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 fn create_wall(
     testbed: &mut Testbed,

examples3d/character_controller3.rs

@@ -1,9 +1,9 @@
 use crate::utils::character::{self, CharacterControlMode};
+use rapier_testbed3d::Testbed;
 use rapier3d::{
     control::{KinematicCharacterController, PidController},
     prelude::*,
 };
-use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/collision_groups3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/compound3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/convex_polyhedron3.rs

@@ -1,7 +1,7 @@
 use rand::distributions::{Distribution, Standard};
-use rand::{rngs::StdRng, SeedableRng};
+use rand::{SeedableRng, rngs::StdRng};
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/damping3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/debug_add_remove_collider3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/debug_articulations3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 fn create_ball_articulations(
     bodies: &mut RigidBodySet,

examples3d/debug_big_colliders3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*

examples3d/debug_boxes3.rs

@@ -1,5 +1,5 @@
-use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
+use rapier3d::prelude::*;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*