Merge pull request #427 from dimforge/disable

Add enable/disable, incremental query pipeline, and vehicle character contoller

CHANGELOG.md

@@ -1,8 +1,29 @@
+
+## Unreleased
+### Added
+- Add `RigidBody::set_enabled`, `RigidBody::is_enabled`, `RigidBodyBuilder::enabled` to enable/disable a rigid-body
+  without having to delete it. Disabling a rigid-body attached to a multibody joint isn’t supported yet.
+- Add `Collider::set_enabled`, `Collider::is_enabled`, `ColliderBuilder::enabled` to enable/disable a collider
+  without having to delete it.
+- Add `GenericJoint::set_enabled`, `GenericJoint::is_enabled` to enable/disable a joint without having to delete it.
+  Disabling a multibody joint isn’t supported yet.
+- Add `DynamicRayCastVehicleController`, a vehicle controller based on ray-casting and dynamic rigid-bodies (mostly
+  a port of the vehicle controller from Bullet physics).
+
+### Modified
+- Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this
+  argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time as
+  these other pipelines. In that case, calling `QueryPipeline::update` a `PhysicsPipeline::step` isn’t needed.
+- `RigidBody::set_body_type` now takes an extra boolean argument indicating if the rigid-body should be woken-up
+  (if it becomes dynamic).
+
+### Fix
+- Fix bug resulting in rigid-bodies being awakened after they are created, even if they are created sleeping.
+
 ## v0.16.1 (10 Nov. 2022)
 ### Fix
 - Fixed docs build on `docs.rs`.
 
-
 ## v0.16.0 (30 Oct. 2022)
 ### Added
 - Implement `Copy` for `CharacterCollision`.

benchmarks3d/all_benchmarks3.rs

@@ -20,6 +20,7 @@ mod joint_fixed3;
 mod joint_prismatic3;
 mod joint_revolute3;
 mod keva3;
+mod many_static3;
 mod pyramid3;
 mod stacks3;
 mod trimesh3;
@@ -54,6 +55,7 @@ pub fn main() {
         ("CCD", ccd3::init_world),
         ("Compound", compound3::init_world),
         ("Convex polyhedron", convex_polyhedron3::init_world),
+        ("Many static", many_static3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),

benchmarks3d/many_static3.rs

@@ -0,0 +1,52 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+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 = 50;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + 1.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 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;
+
+                let status = if j < num - 1 {
+                    RigidBodyType::Fixed
+                } else {
+                    RigidBodyType::Dynamic
+                };
+                let density = 0.477;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y, z]);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::ball(rad).density(density);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * 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());
+}

crates/rapier2d-f64/Cargo.toml

@@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ], optional = true }
 num-traits = "0.2"
 nalgebra = "0.31"
-parry2d-f64 = "^0.11.1"
+parry2d-f64 = "0.12"
 simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }

crates/rapier2d/Cargo.toml

@@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ], optional = true }
 num-traits = "0.2"
 nalgebra = "0.31"
-parry2d = "^0.11.1"
+parry2d = "0.12"
 simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }

crates/rapier3d-f64/Cargo.toml

@@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ], optional = true }
 num-traits = "0.2"
 nalgebra = "0.31"
-parry3d-f64 = "^0.11.1"
+parry3d-f64 = "0.12"
 simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }

crates/rapier3d/Cargo.toml

@@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true }
 instant = { version = "0.1", features = [ "now" ], optional = true }
 num-traits = "0.2"
 nalgebra = "0.31"
-parry3d = "^0.11.1"
+parry3d = "0.12"
 simba = "0.7"
 approx = "0.5"
 rayon = { version = "1", optional = true }

examples3d/all_examples3.rs

@@ -44,6 +44,7 @@ mod primitives3;
 mod restitution3;
 mod sensor3;
 mod trimesh3;
+mod vehicle_controller3;
 
 fn demo_name_from_command_line() -> Option<String> {
     let mut args = std::env::args();
@@ -101,6 +102,7 @@ pub fn main() {
         ("Restitution", restitution3::init_world),
         ("Sensor", sensor3::init_world),
         ("TriMesh", trimesh3::init_world),
+        ("Vehicle controller", vehicle_controller3::init_world),
         ("Keva tower", keva3::init_world),
         ("Newton cradle", newton_cradle3::init_world),
         ("(Debug) multibody_joints", debug_articulations3::init_world),

examples3d/vehicle_controller3.rs

@@ -0,0 +1,160 @@
+use rapier3d::control::{DynamicRayCastVehicleController, WheelTuning};
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+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 = 5.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Vehicle we will control manually.
+     */
+    let hw = 0.3;
+    let hh = 0.15;
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, 1.0, 0.0]);
+    let vehicle_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(hw * 2.0, hh, hw).density(100.0);
+    colliders.insert_with_parent(collider, vehicle_handle, &mut bodies);
+
+    let mut tuning = WheelTuning::default();
+    tuning.suspension_stiffness = 100.0;
+    tuning.suspension_damping = 10.0;
+    let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle);
+    let wheel_positions = [
+        point![hw * 1.5, -hh, hw],
+        point![hw * 1.5, -hh, -hw],
+        point![-hw * 1.5, -hh, hw],
+        point![-hw * 1.5, -hh, -hw],
+    ];
+
+    for pos in wheel_positions {
+        vehicle.add_wheel(pos, -Vector::y(), Vector::z(), hh, hh / 4.0, &tuning);
+    }
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 0.1;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num / 2) as f32;
+    let centery = rad;
+
+    for j in 0usize..1 {
+        for k in 0usize..4 {
+            for i in 0..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery;
+                let z = k as f32 * shift + centerx;
+
+                let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Create a slope we can climb.
+     */
+    let slope_angle = 0.2;
+    let slope_size = 2.0;
+    let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size)
+        .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0])
+        .rotation(Vector::z() * slope_angle);
+    colliders.insert(collider);
+
+    /*
+     * Create a slope we can’t climb.
+     */
+    let impossible_slope_angle = 0.9;
+    let impossible_slope_size = 2.0;
+    let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size)
+        .translation(vector![
+            ground_size + slope_size * 2.0 + impossible_slope_size - 0.9,
+            -ground_height + 2.3,
+            0.0
+        ])
+        .rotation(Vector::z() * impossible_slope_angle);
+    colliders.insert(collider);
+
+    // /*
+    //  * Create a moving platform.
+    //  */
+    // let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]);
+    // // .rotation(-0.3);
+    // let platform_handle = bodies.insert(body);
+    // let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0);
+    // colliders.insert_with_parent(collider, platform_handle, &mut bodies);
+
+    /*
+     * More complex ground.
+     */
+    let ground_size = Vector::new(10.0, 0.4, 10.0);
+    let nsubdivs = 20;
+
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        -(i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos()
+            - (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos()
+    });
+
+    let collider =
+        ColliderBuilder::heightfield(heights, ground_size).translation(vector![-7.0, 0.0, 0.0]);
+    colliders.insert(collider);
+
+    // /*
+    //  * A tilting dynamic body with a limited joint.
+    //  */
+    // let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]);
+    // let ground_handle = bodies.insert(ground);
+    // let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]);
+    // let handle = bodies.insert(body);
+    // let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0);
+    // colliders.insert_with_parent(collider, handle, &mut bodies);
+    // let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]);
+    // impulse_joints.insert(ground_handle, handle, joint, true);
+
+    // /*
+    //  * Setup a callback to move the platform.
+    //  */
+    // testbed.add_callback(move |_, physics, _, run_state| {
+    //     let linvel = vector![
+    //         (run_state.time * 2.0).sin() * 2.0,
+    //         (run_state.time * 5.0).sin() * 1.5,
+    //         0.0
+    //     ];
+    //     // let angvel = run_state.time.sin() * 0.5;
+    //
+    //     // Update the velocity-based kinematic body by setting its velocity.
+    //     if let Some(platform) = physics.bodies.get_mut(platform_handle) {
+    //         platform.set_linvel(linvel, true);
+    //         // NOTE: interaction with rotating platforms isn’t handled very well yet.
+    //         // platform.set_angvel(angvel, true);
+    //     }
+    // });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.set_vehicle_controller(vehicle);
+    testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin());
+}

src/control/character_controller.rs

@@ -133,7 +133,7 @@ impl Default for KinematicCharacterController {
 pub struct EffectiveCharacterMovement {
     /// The movement to apply.
     pub translation: Vector<Real>,
-    /// Is the character touching the ground after applying `EffictiveKineamticMovement::translation`?
+    /// Is the character touching the ground after applying `EffectiveKineamticMovement::translation`?
     pub grounded: bool,
 }
 

src/control/mod.rs

@@ -5,4 +5,10 @@ pub use self::character_controller::{
     KinematicCharacterController,
 };
 
+#[cfg(feature = "dim3")]
+pub use self::ray_cast_vehicle_controller::{DynamicRayCastVehicleController, Wheel, WheelTuning};
+
 mod character_controller;
+
+#[cfg(feature = "dim3")]
+mod ray_cast_vehicle_controller;

src/control/ray_cast_vehicle_controller.rs

@@ -0,0 +1,808 @@
+//! A vehicle controller based on ray-casting, ported and modified from Bullet’s `btRaycastVehicle`.
+
+use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
+use crate::geometry::{ColliderHandle, ColliderSet, Ray};
+use crate::math::{Point, Real, Rotation, Vector};
+use crate::pipeline::{QueryFilter, QueryPipeline};
+use crate::utils::{WCross, WDot};
+
+/// A character controller to simulate vehicles using ray-casting for the wheels.
+pub struct DynamicRayCastVehicleController {
+    wheels: Vec<Wheel>,
+    forward_ws: Vec<Vector<Real>>,
+    axle: Vec<Vector<Real>>,
+    /// The current forward speed of the vehicle.
+    pub current_vehicle_speed: Real,
+
+    /// Handle of the vehicle’s chassis.
+    pub chassis: RigidBodyHandle,
+    /// The chassis’ local _up_ direction (`0 = x, 1 = y, 2 = z`)
+    pub index_up_axis: usize,
+    /// The chassis’ local _forward_ direction (`0 = x, 1 = y, 2 = z`)
+    pub index_forward_axis: usize,
+}
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+/// Parameters affecting the physical behavior of a wheel.
+pub struct WheelTuning {
+    /// The suspension stiffness.
+    ///
+    /// Increase this value if the suspension appears to not push the vehicle strong enough.
+    pub suspension_stiffness: Real,
+    /// The suspension’s damping when it is being compressed.
+    pub suspension_compression: Real,
+    /// The suspension’s damping when it is being released.
+    ///
+    /// Increase this value if the suspension appears to overshoot.
+    pub suspension_damping: Real,
+    /// The maximum distance the suspension can travel before and after its resting length.
+    pub max_suspension_travel: Real,
+    /// Parameter controlling how much traction the tire his.
+    ///
+    /// The larger the value, the more instantaneous braking will happen (with the risk of
+    /// causing the vehicle to flip if it’s too strong).
+    pub friction_slip: Real,
+    /// The maximum force applied by the suspension.
+    pub max_suspension_force: Real,
+}
+
+impl Default for WheelTuning {
+    fn default() -> Self {
+        Self {
+            suspension_stiffness: 5.88,
+            suspension_compression: 0.83,
+            suspension_damping: 0.88,
+            max_suspension_travel: 5.0,
+            friction_slip: 10.5,
+            max_suspension_force: 6000.0,
+        }
+    }
+}
+
+/// Objects used to initialize a wheel.
+struct WheelDesc {
+    /// The position of the wheel, relative to the chassis.
+    pub chassis_connection_cs: Point<Real>,
+    /// The direction of the wheel’s suspension, relative to the chassis.
+    ///
+    /// The ray-casting will happen following this direction to detect the ground.
+    pub direction_cs: Vector<Real>,
+    /// The wheel’s axle axis, relative to the chassis.
+    pub axle_cs: Vector<Real>,
+    /// The rest length of the wheel’s suspension spring.
+    pub suspension_rest_length: Real,
+    /// The maximum distance the suspension can travel before and after its resting length.
+    pub max_suspension_travel: Real,
+    /// The wheel’s radius.
+    pub radius: Real,
+
+    /// The suspension stiffness.
+    ///
+    /// Increase this value if the suspension appears to not push the vehicle strong enough.
+    pub suspension_stiffness: Real,
+    /// The suspension’s damping when it is being compressed.
+    pub damping_compression: Real,
+    /// The suspension’s damping when it is being released.
+    ///
+    /// Increase this value if the suspension appears to overshoot.
+    pub damping_relaxation: Real,
+    /// Parameter controlling how much traction the tire his.
+    ///
+    /// The larger the value, the more instantaneous braking will happen (with the risk of
+    /// causing the vehicle to flip if it’s too strong).
+    pub friction_slip: Real,
+    /// The maximum force applied by the suspension.
+    pub max_suspension_force: Real,
+}
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+/// A wheel attached to a vehicle.
+pub struct Wheel {
+    raycast_info: RayCastInfo,
+
+    center: Point<Real>,
+    wheel_direction_ws: Vector<Real>,
+    wheel_axle_ws: Vector<Real>,
+
+    /// The position of the wheel, relative to the chassis.
+    pub chassis_connection_point_cs: Point<Real>,
+    /// The direction of the wheel’s suspension, relative to the chassis.
+    ///
+    /// The ray-casting will happen following this direction to detect the ground.
+    pub direction_cs: Vector<Real>,
+    /// The wheel’s axle axis, relative to the chassis.
+    pub axle_cs: Vector<Real>,
+    /// The rest length of the wheel’s suspension spring.
+    pub suspension_rest_length: Real,
+    /// The maximum distance the suspension can travel before and after its resting length.
+    pub max_suspension_travel: Real,
+    /// The wheel’s radius.
+    pub radius: Real,
+    /// The suspension stiffness.
+    ///
+    /// Increase this value if the suspension appears to not push the vehicle strong enough.
+    pub suspension_stiffness: Real,
+    /// The suspension’s damping when it is being compressed.
+    pub damping_compression: Real,
+    /// The suspension’s damping when it is being released.
+    ///
+    /// Increase this value if the suspension appears to overshoot.
+    pub damping_relaxation: Real,
+    /// Parameter controlling how much traction the tire his.
+    ///
+    /// The larger the value, the more instantaneous braking will happen (with the risk of
+    /// causing the vehicle to flip if it’s too strong).
+    friction_slip: Real,
+    /// The wheel’s current rotation on its axle.
+    pub rotation: Real,
+    delta_rotation: Real,
+    roll_influence: Real, // TODO: make this public?
+    /// The maximum force applied by the suspension.
+    pub max_suspension_force: Real,
+
+    /// The forward impulses applied by the wheel on the chassis.
+    pub forward_impulse: Real,
+    /// The side impulses applied by the wheel on the chassis.
+    pub side_impulse: Real,
+
+    /// The steering angle for this wheel.
+    pub steering: Real,
+    /// The forward force applied by this wheel on the chassis.
+    pub engine_force: Real,
+    /// The maximum amount of braking impulse applied to slow down the vehicle.
+    pub brake: Real,
+
+    clipped_inv_contact_dot_suspension: Real,
+    suspension_relative_velocity: Real,
+    /// The force applied by the suspension.
+    pub wheel_suspension_force: Real,
+    skid_info: Real,
+}
+
+impl Wheel {
+    fn new(info: WheelDesc) -> Self {
+        Self {
+            raycast_info: RayCastInfo::default(),
+            suspension_rest_length: info.suspension_rest_length,
+            max_suspension_travel: info.max_suspension_travel,
+            radius: info.radius,
+            suspension_stiffness: info.suspension_stiffness,
+            damping_compression: info.damping_compression,
+            damping_relaxation: info.damping_relaxation,
+            chassis_connection_point_cs: info.chassis_connection_cs,
+            direction_cs: info.direction_cs,
+            axle_cs: info.axle_cs,
+            wheel_direction_ws: info.direction_cs,
+            wheel_axle_ws: info.axle_cs,
+            center: Point::origin(),
+            friction_slip: info.friction_slip,
+            steering: 0.0,
+            engine_force: 0.0,
+            rotation: 0.0,
+            delta_rotation: 0.0,
+            brake: 0.0,
+            roll_influence: 0.1,
+            clipped_inv_contact_dot_suspension: 0.0,
+            suspension_relative_velocity: 0.0,
+            wheel_suspension_force: 0.0,
+            max_suspension_force: info.max_suspension_force,
+            skid_info: 0.0,
+            side_impulse: 0.0,
+            forward_impulse: 0.0,
+        }
+    }
+
+    /// The world-space center of the wheel.
+    pub fn center(&self) -> Point<Real> {
+        self.center
+    }
+
+    /// The world-space direction of the wheel’s suspension.
+    pub fn suspension(&self) -> Vector<Real> {
+        self.wheel_direction_ws
+    }
+
+    /// The world-space direction of the wheel’s axle.
+    pub fn axle(&self) -> Vector<Real> {
+        self.wheel_axle_ws
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Default)]
+struct RayCastInfo {
+    // set by raycaster
+    contact_normal_ws: Vector<Real>, //contact normal
+    contact_point_ws: Point<Real>,   //raycast hitpoint
+    suspension_length: Real,
+    hard_point_ws: Point<Real>, //raycast starting point
+    is_in_contact: bool,
+    ground_object: Option<ColliderHandle>,
+}
+
+impl DynamicRayCastVehicleController {
+    /// Creates a new vehicle represented by the given rigid-body.
+    ///
+    /// Wheels have to be attached afterwards calling [`Self::add_wheel`].
+    pub fn new(chassis: RigidBodyHandle) -> Self {
+        Self {
+            wheels: vec![],
+            forward_ws: vec![],
+            axle: vec![],
+            current_vehicle_speed: 0.0,
+            chassis,
+            index_up_axis: 1,
+            index_forward_axis: 0,
+        }
+    }
+
+    //
+    // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
+    //
+    /// Adds a wheel to this vehicle.
+    pub fn add_wheel(
+        &mut self,
+        chassis_connection_cs: Point<Real>,
+        direction_cs: Vector<Real>,
+        axle_cs: Vector<Real>,
+        suspension_rest_length: Real,
+        radius: Real,
+        tuning: &WheelTuning,
+    ) -> &mut Wheel {
+        let ci = WheelDesc {
+            chassis_connection_cs,
+            direction_cs,
+            axle_cs,
+            suspension_rest_length,
+            radius,
+            suspension_stiffness: tuning.suspension_stiffness,
+            damping_compression: tuning.suspension_compression,
+            damping_relaxation: tuning.suspension_damping,
+            friction_slip: tuning.friction_slip,
+            max_suspension_travel: tuning.max_suspension_travel,
+            max_suspension_force: tuning.max_suspension_force,
+        };
+
+        let wheel_id = self.wheels.len();
+        self.wheels.push(Wheel::new(ci));
+
+        &mut self.wheels[wheel_id]
+    }
+
+    #[cfg(feature = "dim2")]
+    fn update_wheel_transform(&mut self, chassis: &RigidBody, wheel_index: usize) {
+        self.update_wheel_transforms_ws(chassis, wheel_index);
+        let wheel = &mut self.wheels[wheel_index];
+        wheel.center = (wheel.raycast_info.hard_point_ws
+            + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length)
+            .coords;
+    }
+
+    #[cfg(feature = "dim3")]
+    fn update_wheel_transform(&mut self, chassis: &RigidBody, wheel_index: usize) {
+        self.update_wheel_transforms_ws(chassis, wheel_index);
+        let wheel = &mut self.wheels[wheel_index];
+
+        let steering_orn = Rotation::new(-wheel.wheel_direction_ws * wheel.steering);
+        wheel.wheel_axle_ws = steering_orn * (chassis.position() * wheel.axle_cs);
+        wheel.center = wheel.raycast_info.hard_point_ws
+            + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length;
+    }
+
+    fn update_wheel_transforms_ws(&mut self, chassis: &RigidBody, wheel_id: usize) {
+        let wheel = &mut self.wheels[wheel_id];
+        wheel.raycast_info.is_in_contact = false;
+
+        let chassis_transform = chassis.position();
+
+        wheel.raycast_info.hard_point_ws = chassis_transform * wheel.chassis_connection_point_cs;
+        wheel.wheel_direction_ws = chassis_transform * wheel.direction_cs;
+        wheel.wheel_axle_ws = chassis_transform * wheel.axle_cs;
+    }
+
+    fn ray_cast(
+        &mut self,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        queries: &QueryPipeline,
+        filter: QueryFilter,
+        chassis: &RigidBody,
+        wheel_id: usize,
+    ) {
+        let wheel = &mut self.wheels[wheel_id];
+        let raylen = wheel.suspension_rest_length + wheel.radius;
+        let rayvector = wheel.wheel_direction_ws * raylen;
+        let source = wheel.raycast_info.hard_point_ws;
+        wheel.raycast_info.contact_point_ws = source + rayvector;
+        let ray = Ray::new(source, rayvector);
+        let hit = queries.cast_ray_and_get_normal(bodies, colliders, &ray, 1.0, true, filter);
+
+        wheel.raycast_info.ground_object = None;
+
+        if let Some((collider_hit, mut hit)) = hit {
+            if hit.toi == 0.0 {
+                let collider = &colliders[collider_hit];
+                let up_ray = Ray::new(source + rayvector, -rayvector);
+                if let Some(hit2) =
+                    collider
+                        .shape
+                        .cast_ray_and_get_normal(collider.position(), &up_ray, 1.0, false)
+                {
+                    hit.normal = -hit2.normal;
+                }
+
+                if hit.normal == Vector::zeros() {
+                    // If the hit is still not defined, set the normal.
+                    hit.normal = -wheel.wheel_direction_ws;
+                }
+            }
+
+            wheel.raycast_info.contact_normal_ws = hit.normal;
+            wheel.raycast_info.is_in_contact = true;
+            wheel.raycast_info.ground_object = Some(collider_hit);
+
+            let hit_distance = hit.toi * raylen;
+            wheel.raycast_info.suspension_length = hit_distance - wheel.radius;
+
+            // clamp on max suspension travel
+            let min_suspension_length = wheel.suspension_rest_length - wheel.max_suspension_travel;
+            let max_suspension_length = wheel.suspension_rest_length + wheel.max_suspension_travel;
+            wheel.raycast_info.suspension_length = wheel
+                .raycast_info
+                .suspension_length
+                .clamp(min_suspension_length, max_suspension_length);
+            wheel.raycast_info.contact_point_ws = ray.point_at(hit.toi);
+
+            let denominator = wheel
+                .raycast_info
+                .contact_normal_ws
+                .dot(&wheel.wheel_direction_ws);
+            let chassis_velocity_at_contact_point =
+                chassis.velocity_at_point(&wheel.raycast_info.contact_point_ws);
+            let proj_vel = wheel
+                .raycast_info
+                .contact_normal_ws
+                .dot(&chassis_velocity_at_contact_point);
+
+            if denominator >= -0.1 {
+                wheel.suspension_relative_velocity = 0.0;
+                wheel.clipped_inv_contact_dot_suspension = 1.0 / 0.1;
+            } else {
+                let inv = -1.0 / denominator;
+                wheel.suspension_relative_velocity = proj_vel * inv;
+                wheel.clipped_inv_contact_dot_suspension = inv;
+            }
+        } else {
+            // No contact, put wheel info as in rest position
+            wheel.raycast_info.suspension_length = wheel.suspension_rest_length;
+            wheel.suspension_relative_velocity = 0.0;
+            wheel.raycast_info.contact_normal_ws = -wheel.wheel_direction_ws;
+            wheel.clipped_inv_contact_dot_suspension = 1.0;
+        }
+    }
+
+    /// Updates the vehicle’s velocity based on its suspension, engine force, and brake.
+    pub fn update_vehicle(
+        &mut self,
+        dt: Real,
+        bodies: &mut RigidBodySet,
+        colliders: &ColliderSet,
+        queries: &QueryPipeline,
+        filter: QueryFilter,
+    ) {
+        let num_wheels = self.wheels.len();
+        let chassis = &bodies[self.chassis];
+
+        for i in 0..num_wheels {
+            self.update_wheel_transform(chassis, i);
+        }
+
+        self.current_vehicle_speed = chassis.linvel().norm();
+
+        let forward_w = chassis.position() * Vector::ith(self.index_forward_axis, 1.0);
+
+        if forward_w.dot(chassis.linvel()) < 0.0 {
+            self.current_vehicle_speed *= -1.0;
+        }
+
+        //
+        // simulate suspension
+        //
+
+        for wheel_id in 0..self.wheels.len() {
+            self.ray_cast(bodies, colliders, queries, filter, chassis, wheel_id);
+        }
+
+        let chassis_mass = chassis.mass();
+        self.update_suspension(chassis_mass);
+
+        let chassis = bodies
+            .get_mut_internal_with_modification_tracking(self.chassis)
+            .unwrap();
+
+        for wheel in &mut self.wheels {
+            if wheel.engine_force > 0.0 {
+                chassis.wake_up(true);
+            }
+
+            // apply suspension force
+            let mut suspension_force = wheel.wheel_suspension_force;
+
+            if suspension_force > wheel.max_suspension_force {
+                suspension_force = wheel.max_suspension_force;
+            }
+
+            let impulse = wheel.raycast_info.contact_normal_ws * suspension_force * dt;
+            chassis.apply_impulse_at_point(impulse, wheel.raycast_info.contact_point_ws, false);
+        }
+
+        self.update_friction(bodies, colliders, dt);
+
+        let chassis = bodies
+            .get_mut_internal_with_modification_tracking(self.chassis)
+            .unwrap();
+
+        for wheel in &mut self.wheels {
+            let vel = chassis.velocity_at_point(&wheel.raycast_info.hard_point_ws);
+
+            if wheel.raycast_info.is_in_contact {
+                let mut fwd = chassis.position() * Vector::ith(self.index_forward_axis, 1.0);
+                let proj = fwd.dot(&wheel.raycast_info.contact_normal_ws);
+                fwd -= wheel.raycast_info.contact_normal_ws * proj;
+
+                let proj2 = fwd.dot(&vel);
+
+                wheel.delta_rotation = (proj2 * dt) / (wheel.radius);
+                wheel.rotation += wheel.delta_rotation;
+            } else {
+                wheel.rotation += wheel.delta_rotation;
+            }
+
+            wheel.delta_rotation *= 0.99; //damping of rotation when not in contact
+        }
+    }
+
+    /// Reference to all the wheels attached to this vehicle.
+    pub fn wheels(&self) -> &[Wheel] {
+        &self.wheels
+    }
+
+    /// Mutable reference to all the wheels attached to this vehicle.
+    pub fn wheels_mut(&mut self) -> &mut [Wheel] {
+        &mut self.wheels
+    }
+
+    fn update_suspension(&mut self, chassis_mass: Real) {
+        for w_it in 0..self.wheels.len() {
+            let wheels = &mut self.wheels[w_it];
+
+            if wheels.raycast_info.is_in_contact {
+                let mut force;
+                //	Spring
+                {
+                    let rest_length = wheels.suspension_rest_length;
+                    let current_length = wheels.raycast_info.suspension_length;
+                    let length_diff = rest_length - current_length;
+
+                    force = wheels.suspension_stiffness
+                        * length_diff
+                        * wheels.clipped_inv_contact_dot_suspension;
+                }
+
+                // Damper
+                {
+                    let projected_rel_vel = wheels.suspension_relative_velocity;
+                    {
+                        let susp_damping = if projected_rel_vel < 0.0 {
+                            wheels.damping_compression
+                        } else {
+                            wheels.damping_relaxation
+                        };
+                        force -= susp_damping * projected_rel_vel;
+                    }
+                }
+
+                // RESULT
+                wheels.wheel_suspension_force = (force * chassis_mass).max(0.0);
+            } else {
+                wheels.wheel_suspension_force = 0.0;
+            }
+        }
+    }
+
+    const SIDE_FRICTION_STIFFNESS2: Real = 1.0;
+
+    fn update_friction(&mut self, bodies: &mut RigidBodySet, colliders: &ColliderSet, dt: Real) {
+        let num_wheels = self.wheels.len();
+
+        if num_wheels == 0 {
+            return;
+        }
+
+        self.forward_ws.resize(num_wheels, Default::default());
+        self.axle.resize(num_wheels, Default::default());
+
+        let mut num_wheels_on_ground = 0;
+
+        //TODO: collapse all those loops into one!
+        for wheel in &mut self.wheels {
+            let ground_object = wheel.raycast_info.ground_object;
+
+            if ground_object.is_some() {
+                num_wheels_on_ground += 1;
+            }
+
+            wheel.side_impulse = 0.0;
+            wheel.forward_impulse = 0.0;
+        }
+
+        {
+            for i in 0..num_wheels {
+                let wheel = &mut self.wheels[i];
+                let ground_object = wheel.raycast_info.ground_object;
+
+                if ground_object.is_some() {
+                    self.axle[i] = wheel.wheel_axle_ws;
+
+                    let surf_normal_ws = wheel.raycast_info.contact_normal_ws;
+                    let proj = self.axle[i].dot(&surf_normal_ws);
+                    self.axle[i] -= surf_normal_ws * proj;
+                    self.axle[i] = self.axle[i]
+                        .try_normalize(1.0e-5)
+                        .unwrap_or_else(Vector::zeros);
+                    self.forward_ws[i] = surf_normal_ws
+                        .cross(&self.axle[i])
+                        .try_normalize(1.0e-5)
+                        .unwrap_or_else(Vector::zeros);
+
+                    if let Some(ground_body) = ground_object
+                        .and_then(|h| colliders[h].parent())
+                        .map(|h| &bodies[h])
+                        .filter(|b| b.is_dynamic())
+                    {
+                        wheel.side_impulse = resolve_single_bilateral(
+                            &bodies[self.chassis],
+                            &wheel.raycast_info.contact_point_ws,
+                            &ground_body,
+                            &wheel.raycast_info.contact_point_ws,
+                            &self.axle[i],
+                        );
+                    } else {
+                        wheel.side_impulse = resolve_single_unilateral(
+                            &bodies[self.chassis],
+                            &wheel.raycast_info.contact_point_ws,
+                            &self.axle[i],
+                        );
+                    }
+
+                    wheel.side_impulse *= Self::SIDE_FRICTION_STIFFNESS2;
+                }
+            }
+        }
+
+        let side_factor = 1.0;
+        let fwd_factor = 0.5;
+
+        let mut sliding = false;
+        {
+            for wheel_id in 0..num_wheels {
+                let wheel = &mut self.wheels[wheel_id];
+                let ground_object = wheel.raycast_info.ground_object;
+
+                let mut rolling_friction = 0.0;
+
+                if ground_object.is_some() {
+                    if wheel.engine_force != 0.0 {
+                        rolling_friction = wheel.engine_force * dt;
+                    } else {
+                        let default_rolling_friction_impulse = 0.0;
+                        let max_impulse = if wheel.brake != 0.0 {
+                            wheel.brake
+                        } else {
+                            default_rolling_friction_impulse
+                        };
+                        let contact_pt = WheelContactPoint::new(
+                            &bodies[self.chassis],
+                            ground_object
+                                .and_then(|h| colliders[h].parent())
+                                .map(|h| &bodies[h]),
+                            wheel.raycast_info.contact_point_ws,
+                            self.forward_ws[wheel_id],
+                            max_impulse,
+                        );
+                        assert!(num_wheels_on_ground > 0);
+                        rolling_friction = contact_pt.calc_rolling_friction(num_wheels_on_ground);
+                    }
+                }
+
+                //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
+
+                wheel.forward_impulse = 0.0;
+                wheel.skid_info = 1.0;
+
+                if ground_object.is_some() {
+                    let max_imp = wheel.wheel_suspension_force * dt * wheel.friction_slip;
+                    let max_imp_side = max_imp;
+                    let max_imp_squared = max_imp * max_imp_side;
+                    assert!(max_imp_squared >= 0.0);
+
+                    wheel.forward_impulse = rolling_friction;
+
+                    let x = wheel.forward_impulse * fwd_factor;
+                    let y = wheel.side_impulse * side_factor;
+
+                    let impulse_squared = x * x + y * y;
+
+                    if impulse_squared > max_imp_squared {
+                        sliding = true;
+
+                        let factor = max_imp * crate::utils::inv(impulse_squared.sqrt());
+                        wheel.skid_info *= factor;
+                    }
+                }
+            }
+        }
+
+        if sliding {
+            for wheel in &mut self.wheels {
+                if wheel.side_impulse != 0.0 {
+                    if wheel.skid_info < 1.0 {
+                        wheel.forward_impulse *= wheel.skid_info;
+                        wheel.side_impulse *= wheel.skid_info;
+                    }
+                }
+            }
+        }
+
+        // apply the impulses
+        {
+            let chassis = bodies
+                .get_mut_internal_with_modification_tracking(self.chassis)
+                .unwrap();
+
+            for wheel_id in 0..num_wheels {
+                let wheel = &self.wheels[wheel_id];
+                let mut impulse_point = wheel.raycast_info.contact_point_ws;
+
+                if wheel.forward_impulse != 0.0 {
+                    chassis.apply_impulse_at_point(
+                        self.forward_ws[wheel_id] * wheel.forward_impulse,
+                        impulse_point,
+                        false,
+                    );
+                }
+                if wheel.side_impulse != 0.0 {
+                    let side_impulse = self.axle[wheel_id] * wheel.side_impulse;
+
+                    let v_chassis_world_up =
+                        chassis.position().rotation * Vector::ith(self.index_up_axis, 1.0);
+                    impulse_point -= v_chassis_world_up
+                        * (v_chassis_world_up.dot(&(impulse_point - chassis.center_of_mass()))
+                            * (1.0 - wheel.roll_influence));
+
+                    chassis.apply_impulse_at_point(side_impulse, impulse_point, false);
+
+                    // TODO: apply friction impulse on the ground
+                    // let ground_object = self.wheels[wheel_id].raycast_info.ground_object;
+                    // ground_object.apply_impulse_at_point(
+                    //     -side_impulse,
+                    //     wheels.raycast_info.contact_point_ws,
+                    //     false,
+                    // );
+                }
+            }
+        }
+    }
+}
+
+struct WheelContactPoint<'a> {
+    body0: &'a RigidBody,
+    body1: Option<&'a RigidBody>,
+    friction_position_world: Point<Real>,
+    friction_direction_world: Vector<Real>,
+    jac_diag_ab_inv: Real,
+    max_impulse: Real,
+}
+
+impl<'a> WheelContactPoint<'a> {
+    pub fn new(
+        body0: &'a RigidBody,
+        body1: Option<&'a RigidBody>,
+        friction_position_world: Point<Real>,
+        friction_direction_world: Vector<Real>,
+        max_impulse: Real,
+    ) -> Self {
+        fn impulse_denominator(body: &RigidBody, pos: &Point<Real>, n: &Vector<Real>) -> Real {
+            let dpt = pos - body.center_of_mass();
+            let gcross = dpt.gcross(*n);
+            let v = (body.mprops.effective_world_inv_inertia_sqrt
+                * (body.mprops.effective_world_inv_inertia_sqrt * gcross))
+                .gcross(dpt);
+            // TODO: take the effective inv mass into account instead of the inv_mass?
+            body.mprops.local_mprops.inv_mass + n.dot(&v)
+        }
+        let denom0 =
+            impulse_denominator(body0, &friction_position_world, &friction_direction_world);
+        let denom1 = body1
+            .map(|body1| {
+                impulse_denominator(body1, &friction_position_world, &friction_direction_world)
+            })
+            .unwrap_or(0.0);
+        let relaxation = 1.0;
+        let jac_diag_ab_inv = relaxation / (denom0 + denom1);
+
+        Self {
+            body0,
+            body1,
+            friction_position_world,
+            friction_direction_world,
+            jac_diag_ab_inv,
+            max_impulse,
+        }
+    }
+
+    pub fn calc_rolling_friction(&self, num_wheels_on_ground: usize) -> Real {
+        let contact_pos_world = self.friction_position_world;
+        let max_impulse = self.max_impulse;
+
+        let vel1 = self.body0.velocity_at_point(&contact_pos_world);
+        let vel2 = self
+            .body1
+            .map(|b| b.velocity_at_point(&contact_pos_world))
+            .unwrap_or_else(Vector::zeros);
+        let vel = vel1 - vel2;
+        let vrel = self.friction_direction_world.dot(&vel);
+
+        // calculate friction that moves us to zero relative velocity
+        (-vrel * self.jac_diag_ab_inv / (num_wheels_on_ground as Real))
+            .clamp(-max_impulse, max_impulse)
+    }
+}
+
+fn resolve_single_bilateral(
+    body1: &RigidBody,
+    pt1: &Point<Real>,
+    body2: &RigidBody,
+    pt2: &Point<Real>,
+    normal: &Vector<Real>,
+) -> Real {
+    let vel1 = body1.velocity_at_point(pt1);
+    let vel2 = body2.velocity_at_point(pt2);
+    let dvel = vel1 - vel2;
+
+    let dpt1 = pt1 - body1.center_of_mass();
+    let dpt2 = pt2 - body2.center_of_mass();
+    let aj = dpt1.gcross(*normal);
+    let bj = dpt2.gcross(-*normal);
+    let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj;
+    let ibj = body2.mprops.effective_world_inv_inertia_sqrt * bj;
+
+    // TODO: take the effective_inv_mass into account?
+    let im1 = body1.mprops.local_mprops.inv_mass;
+    let im2 = body2.mprops.local_mprops.inv_mass;
+
+    let jac_diag_ab = im1 + im2 + iaj.gdot(iaj) + ibj.gdot(ibj);
+    let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab);
+    let rel_vel = normal.dot(&dvel);
+
+    //todo: move this into proper structure
+    let contact_damping = 0.2;
+    -contact_damping * rel_vel * jac_diag_ab_inv
+}
+
+fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point<Real>, normal: &Vector<Real>) -> Real {
+    let vel1 = body1.velocity_at_point(pt1);
+    let dvel = vel1;
+    let dpt1 = pt1 - body1.center_of_mass();
+    let aj = dpt1.gcross(*normal);
+    let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj;
+
+    // TODO: take the effective_inv_mass into account?
+    let im1 = body1.mprops.local_mprops.inv_mass;
+    let jac_diag_ab = im1 + iaj.gdot(iaj);
+    let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab);
+    let rel_vel = normal.dot(&dvel);
+
+    //todo: move this into proper structure
+    let contact_damping = 0.2;
+    -contact_damping * rel_vel * jac_diag_ab_inv
+}

src/dynamics/ccd/ccd_solver.rs

@@ -123,7 +123,6 @@ impl CCDSolver {
     ) -> Option<Real> {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
-            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -245,7 +244,6 @@ impl CCDSolver {
 
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
-            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithNextPosition,

src/dynamics/island_manager.rs

@@ -1,6 +1,6 @@
 use crate::dynamics::{
-    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
-    RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
+    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders,
+    RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
@@ -96,11 +96,18 @@ impl IslandManager {
         //       attempting to wake-up a rigid-body that has already been deleted.
         if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
             let rb = bodies.index_mut_internal(handle);
-            rb.activation.wake_up(strong);
 
-            if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) {
-                rb.ids.active_set_id = self.active_dynamic_set.len();
-                self.active_dynamic_set.push(handle);
+            // Check that the user didn’t change the sleeping state explicitly, in which
+            // case we don’t overwrite it.
+            if !rb.changes.contains(RigidBodyChanges::SLEEP) {
+                rb.activation.wake_up(strong);
+
+                if rb.is_enabled()
+                    && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle)
+                {
+                    rb.ids.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
             }
         }
     }
@@ -256,12 +263,12 @@ impl IslandManager {
             // in contact or joined with this collider.
             push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
 
-            for inter in impulse_joints.attached_joints(handle) {
+            for inter in impulse_joints.attached_enabled_joints(handle) {
                 let other = crate::utils::select_other((inter.0, inter.1), handle);
                 self.stack.push(other);
             }
 
-            for other in multibody_joints.attached_bodies(handle) {
+            for other in multibody_joints.bodies_attached_with_enabled_joint(handle) {
                 self.stack.push(other);
             }
 

src/dynamics/joint/generic_joint.rs

@@ -182,6 +182,19 @@ impl JointMotor {
     }
 }
 
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// Enum indicating whether or not a joint is enabled.
+pub enum JointEnabled {
+    /// The joint is enabled.
+    Enabled,
+    /// The joint wasn’t disabled by the user explicitly but it is attached to
+    /// a disabled rigid-body.
+    DisabledByAttachedBody,
+    /// The joint is disabled by the user explicitly.
+    Disabled,
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
 /// A generic joint.
@@ -208,6 +221,8 @@ pub struct GenericJoint {
     pub motors: [JointMotor; SPATIAL_DIM],
     /// Are contacts between the attached rigid-bodies enabled?
     pub contacts_enabled: bool,
+    /// Whether or not the joint is enabled.
+    pub enabled: JointEnabled,
 }
 
 impl Default for GenericJoint {
@@ -222,6 +237,7 @@ impl Default for GenericJoint {
             limits: [JointLimits::default(); SPATIAL_DIM],
             motors: [JointMotor::default(); SPATIAL_DIM],
             contacts_enabled: true,
+            enabled: JointEnabled::Enabled,
         }
     }
 }
@@ -260,6 +276,27 @@ impl GenericJoint {
         }
     }
 
+    /// Is this joint enabled?
+    pub fn is_enabled(&self) -> bool {
+        self.enabled == JointEnabled::Enabled
+    }
+
+    /// Set whether this joint is enabled or not.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        match self.enabled {
+            JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => {
+                if !enabled {
+                    self.enabled = JointEnabled::Disabled;
+                }
+            }
+            JointEnabled::Disabled => {
+                if enabled {
+                    self.enabled = JointEnabled::Enabled;
+                }
+            }
+        }
+    }
+
     /// Add the specified axes to the set of axes locked by this joint.
     pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
         self.locked_axes |= axes;

src/dynamics/joint/impulse_joint/impulse_joint_set.rs

@@ -72,11 +72,11 @@ impl ImpulseJointSet {
     }
 
     /// Iterates through all the joints between two rigid-bodies.
-    pub fn joints_between<'a>(
-        &'a self,
+    pub fn joints_between(
+        &self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
-    ) -> impl Iterator<Item = (ImpulseJointHandle, &'a ImpulseJoint)> {
+    ) -> impl Iterator<Item = (ImpulseJointHandle, &ImpulseJoint)> {
         self.rb_graph_ids
             .get(body1.0)
             .zip(self.rb_graph_ids.get(body2.0))
@@ -86,15 +86,15 @@ impl ImpulseJointSet {
     }
 
     /// Iterates through all the impulse joints attached to the given rigid-body.
-    pub fn attached_joints<'a>(
-        &'a self,
+    pub fn attached_joints(
+        &self,
         body: RigidBodyHandle,
     ) -> impl Iterator<
         Item = (
             RigidBodyHandle,
             RigidBodyHandle,
             ImpulseJointHandle,
-            &'a ImpulseJoint,
+            &ImpulseJoint,
         ),
     > {
         self.rb_graph_ids
@@ -104,6 +104,35 @@ impl ImpulseJointSet {
             .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2))
     }
 
+    /// Iterates through all the impulse joints attached to the given rigid-body.
+    pub fn map_attached_joints_mut<'a>(
+        &'a mut self,
+        body: RigidBodyHandle,
+        mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint),
+    ) {
+        self.rb_graph_ids.get(body.0).into_iter().for_each(|id| {
+            for inter in self.joint_graph.interactions_with_mut(*id) {
+                (f)(inter.0, inter.1, inter.3.handle, inter.3)
+            }
+        })
+    }
+
+    /// Iterates through all the enabled impulse joints attached to the given rigid-body.
+    pub fn attached_enabled_joints(
+        &self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<
+        Item = (
+            RigidBodyHandle,
+            RigidBodyHandle,
+            ImpulseJointHandle,
+            &ImpulseJoint,
+        ),
+    > {
+        self.attached_joints(body)
+            .filter(|inter| inter.3.data.is_enabled())
+    }
+
     /// Is the given joint handle valid?
     pub fn contains(&self, handle: ImpulseJointHandle) -> bool {
         self.joint_ids.contains(handle.0)
@@ -246,7 +275,7 @@ impl ImpulseJointSet {
         ImpulseJointHandle(handle)
     }
 
-    /// Retrieve all the impulse_joints happening between two active bodies.
+    /// Retrieve all the enabled impulse joints happening between two active bodies.
     // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
     pub(crate) fn select_active_interactions(
         &self,
@@ -264,7 +293,8 @@ impl ImpulseJointSet {
             let rb1 = &bodies[joint.body1];
             let rb2 = &bodies[joint.body2];
 
-            if (rb1.is_dynamic() || rb2.is_dynamic())
+            if joint.data.is_enabled()
+                && (rb1.is_dynamic() || rb2.is_dynamic())
                 && (!rb1.is_dynamic() || !rb1.is_sleeping())
                 && (!rb2.is_dynamic() || !rb2.is_sleeping())
             {

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -372,7 +372,7 @@ impl MultibodyJointSet {
     }
 
     /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
-    /// by an multibody_joint.
+    /// by a multibody_joint.
     pub fn attached_bodies<'a>(
         &'a self,
         body: RigidBodyHandle,
@@ -384,6 +384,21 @@ impl MultibodyJointSet {
             .map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
     }
 
+    /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
+    /// by an enabled multibody_joint.
+    pub fn bodies_attached_with_enabled_joint<'a>(
+        &'a self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<Item = RigidBodyHandle> + 'a {
+        self.attached_bodies(body).filter(move |other| {
+            if let Some((_, _, link)) = self.joint_between(body, *other) {
+                link.joint.data.is_enabled()
+            } else {
+                false
+            }
+        })
+    }
+
     /// Iterates through all the multibodies on this set.
     pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
         self.multibodies.iter().map(|e| e.1)

src/dynamics/rigid_body.rs

@@ -35,6 +35,7 @@ pub struct RigidBody {
     pub(crate) body_type: RigidBodyType,
     /// The dominance group this rigid-body is part of.
     pub(crate) dominance: RigidBodyDominance,
+    pub(crate) enabled: bool,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
 }
@@ -61,6 +62,7 @@ impl RigidBody {
             changes: RigidBodyChanges::all(),
             body_type: RigidBodyType::Dynamic,
             dominance: RigidBodyDominance::default(),
+            enabled: true,
             user_data: 0,
         }
     }
@@ -81,6 +83,28 @@ impl RigidBody {
         &mut self.activation
     }
 
+    /// Is this rigid-body enabled?
+    pub fn is_enabled(&self) -> bool {
+        self.enabled
+    }
+
+    /// Sets whether this rigid-body is enabled or not.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        if enabled != self.enabled {
+            if enabled {
+                // NOTE: this is probably overkill, but it makes sure we don’t
+                // forget anything that needs to be updated because the rigid-body
+                // was basically interpreted as if it was removed while it was
+                // disabled.
+                self.changes = RigidBodyChanges::all();
+            } else {
+                self.changes |= RigidBodyChanges::ENABLED_OR_DISABLED;
+            }
+
+            self.enabled = enabled;
+        }
+    }
+
     /// The linear damping coefficient of this rigid-body.
     #[inline]
     pub fn linear_damping(&self) -> Real {
@@ -111,7 +135,7 @@ impl RigidBody {
     }
 
     /// Sets the type of this rigid-body.
-    pub fn set_body_type(&mut self, status: RigidBodyType) {
+    pub fn set_body_type(&mut self, status: RigidBodyType, wake_up: bool) {
         if status != self.body_type {
             self.changes.insert(RigidBodyChanges::TYPE);
             self.body_type = status;
@@ -119,9 +143,19 @@ impl RigidBody {
             if status == RigidBodyType::Fixed {
                 self.vels = RigidBodyVelocity::zero();
             }
+
+            if self.is_dynamic() && wake_up {
+                self.wake_up(true);
+            }
         }
     }
 
+    /// The world-space center-of-mass of this rigid-body.
+    #[inline]
+    pub fn center_of_mass(&self) -> &Point<Real> {
+        &self.mprops.world_com
+    }
+
     /// The mass-properties of this rigid-body.
     #[inline]
     pub fn mass_properties(&self) -> &MassProperties {
@@ -963,6 +997,8 @@ pub struct RigidBodyBuilder {
     pub ccd_enabled: bool,
     /// The dominance group of the rigid-body to be built.
     pub dominance_group: i8,
+    /// Will the rigid-body being built be enabled?
+    pub enabled: bool,
     /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
     pub user_data: u128,
 }
@@ -984,6 +1020,7 @@ impl RigidBodyBuilder {
             sleeping: false,
             ccd_enabled: false,
             dominance_group: 0,
+            enabled: true,
             user_data: 0,
         }
     }
@@ -1230,6 +1267,12 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// Enable or disable the rigid-body after its creation.
+    pub fn enabled(mut self, enabled: bool) -> Self {
+        self.enabled = enabled;
+        self
+    }
+
     /// Build a new rigid-body with the parameters configured with this builder.
     pub fn build(&self) -> RigidBody {
         let mut rb = RigidBody::new();
@@ -1252,6 +1295,7 @@ impl RigidBodyBuilder {
         rb.damping.angular_damping = self.angular_damping;
         rb.forces.gravity_scale = self.gravity_scale;
         rb.dominance = RigidBodyDominance(self.dominance_group);
+        rb.enabled = self.enabled;
         rb.enable_ccd(self.ccd_enabled);
 
         if self.can_sleep && self.sleeping {

src/dynamics/rigid_body_components.rs

@@ -112,6 +112,8 @@ bitflags::bitflags! {
         const DOMINANCE   = 1 << 5;
         /// Flag indicating that the local mass-properties of this rigid-body must be recomputed.
         const LOCAL_MASS_PROPERTIES = 1 << 6;
+        /// Flag indicating that the rigid-body was enabled or disabled.
+        const ENABLED_OR_DISABLED = 1 << 7;
     }
 }
 
@@ -329,12 +331,14 @@ impl RigidBodyMassProps {
 
         for handle in &attached_colliders.0 {
             if let Some(co) = colliders.get(*handle) {
-                if let Some(co_parent) = co.parent {
-                    let to_add = co
-                        .mprops
-                        .mass_properties(&*co.shape)
-                        .transform_by(&co_parent.pos_wrt_parent);
-                    self.local_mprops += to_add;
+                if co.is_enabled() {
+                    if let Some(co_parent) = co.parent {
+                        let to_add = co
+                            .mprops
+                            .mass_properties(&*co.shape)
+                            .transform_by(&co_parent.pos_wrt_parent);
+                        self.local_mprops += to_add;
+                    }
                 }
             }
         }

src/dynamics/solver/velocity_constraint.rs

@@ -214,6 +214,7 @@ impl VelocityConstraint {
             let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
+                    #[allow(invalid_value)]
                     out_constraints.resize_with(new_len, || {
                         AnyVelocityConstraint::Nongrouped(
                             std::mem::MaybeUninit::uninit().assume_init(),

src/dynamics/solver/velocity_ground_constraint.rs

@@ -105,6 +105,7 @@ impl VelocityGroundConstraint {
             let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
+                    #[allow(invalid_value)]
                     out_constraints.resize_with(new_len, || {
                         AnyVelocityConstraint::NongroupedGround(
                             std::mem::MaybeUninit::uninit().assume_init(),

src/geometry/broad_phase_multi_sap/broad_phase.rs

@@ -176,7 +176,11 @@ impl BroadPhase {
     /// This method will actually remove from the proxy list all the proxies
     /// marked as deletable by `self.predelete_proxy`, making their proxy
     /// handles re-usable by new proxies.
-    fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) {
+    fn complete_removals(
+        &mut self,
+        colliders: &mut ColliderSet,
+        removed_colliders: &[ColliderHandle],
+    ) {
         // If there is no layer, there is nothing to remove.
         if self.layers.is_empty() {
             return;
@@ -224,6 +228,11 @@ impl BroadPhase {
                     self.proxies.remove(proxy_id);
                 }
             }
+
+            if let Some(co) = colliders.get_mut_internal(*removed) {
+                // Reset the proxy index.
+                co.bf_data.proxy_index = crate::INVALID_U32;
+            }
         }
     }
 
@@ -460,9 +469,10 @@ impl BroadPhase {
             // NOTE: we use `get` because the collider may no longer
             //       exist if it has been removed.
             if let Some(co) = colliders.get_mut_internal(*handle) {
-                if !co.changes.needs_broad_phase_update() {
+                if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
                     continue;
                 }
+
                 let mut new_proxy_id = co.bf_data.proxy_index;
 
                 if self.handle_modified_collider(
@@ -496,7 +506,7 @@ impl BroadPhase {
 
         // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
         // layers to possible remove regions from larger layers that would become empty that way.
-        self.complete_removals(removed_colliders);
+        self.complete_removals(colliders, removed_colliders);
     }
 
     /// Propagate regions from the smallest layers up to the larger layers.

src/geometry/broad_phase_qbvh.rs

@@ -0,0 +1,88 @@
+use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
+use parry::bounding_volume::BoundingVolume;
+use parry::math::Real;
+use parry::partitioning::Qbvh;
+use parry::partitioning::QbvhUpdateWorkspace;
+use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
+pub struct BroadPhase {
+    qbvh: Qbvh<ColliderHandle>,
+    stack: Vec<(u32, u32)>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    workspace: QbvhUpdateWorkspace,
+}
+
+impl Default for BroadPhase {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl BroadPhase {
+    pub fn new() -> Self {
+        Self {
+            qbvh: Qbvh::new(),
+            stack: vec![],
+            workspace: QbvhUpdateWorkspace::default(),
+        }
+    }
+
+    #[allow(dead_code)] // This broad-phase is just experimental right now.
+    pub fn update(
+        &mut self,
+        prediction_distance: Real,
+        colliders: &ColliderSet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    ) {
+        let margin = 0.01;
+
+        if modified_colliders.is_empty() {
+            return;
+        }
+
+        // Visitor to find collision pairs.
+        let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new(
+            |co1: &ColliderHandle, co2: &ColliderHandle| {
+                events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2)));
+                true
+            },
+        );
+
+        let full_rebuild = self.qbvh.raw_nodes().is_empty();
+
+        if full_rebuild {
+            self.qbvh.clear_and_rebuild(
+                colliders.iter().map(|(handle, collider)| {
+                    (
+                        handle,
+                        collider.compute_aabb().loosened(prediction_distance / 2.0),
+                    )
+                }),
+                margin,
+            );
+            self.qbvh
+                .traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
+        } else {
+            for modified in modified_colliders {
+                self.qbvh.pre_update_or_insert(*modified);
+            }
+
+            for removed in removed_colliders {
+                self.qbvh.remove(*removed);
+            }
+
+            let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
+                colliders[*handle]
+                    .compute_aabb()
+                    .loosened(prediction_distance / 2.0)
+            });
+            self.qbvh
+                .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
+            self.qbvh.rebalance(margin, &mut self.workspace);
+        }
+    }
+}

src/geometry/collider.rs

@@ -7,6 +7,7 @@ use crate::geometry::{
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
 use crate::pipeline::{ActiveEvents, ActiveHooks};
+use crate::prelude::ColliderEnabled;
 use na::Unit;
 use parry::bounding_volume::Aabb;
 use parry::shape::{Shape, TriMeshFlags};
@@ -154,6 +155,32 @@ impl Collider {
         }
     }
 
+    /// Is this collider enabled?
+    pub fn is_enabled(&self) -> bool {
+        match self.flags.enabled {
+            ColliderEnabled::Enabled => true,
+            _ => false,
+        }
+    }
+
+    /// Sets whether or not this collider is enabled.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        match self.flags.enabled {
+            ColliderEnabled::Enabled | ColliderEnabled::DisabledByParent => {
+                if !enabled {
+                    self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED);
+                    self.flags.enabled = ColliderEnabled::Disabled;
+                }
+            }
+            ColliderEnabled::Disabled => {
+                if enabled {
+                    self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED);
+                    self.flags.enabled = ColliderEnabled::Enabled;
+                }
+            }
+        }
+    }
+
     /// Sets the translational part of this collider's position.
     pub fn set_translation(&mut self, translation: Vector<Real>) {
         self.changes.insert(ColliderChanges::POSITION);
@@ -402,6 +429,8 @@ pub struct ColliderBuilder {
     pub collision_groups: InteractionGroups,
     /// The solver groups for the collider being built.
     pub solver_groups: InteractionGroups,
+    /// Will the collider being built be enabled?
+    pub enabled: bool,
     /// The total force magnitude beyond which a contact force event can be emitted.
     pub contact_force_event_threshold: Real,
 }
@@ -424,6 +453,7 @@ impl ColliderBuilder {
             active_collision_types: ActiveCollisionTypes::default(),
             active_hooks: ActiveHooks::empty(),
             active_events: ActiveEvents::empty(),
+            enabled: true,
             contact_force_event_threshold: 0.0,
         }
     }
@@ -834,6 +864,12 @@ impl ColliderBuilder {
         self
     }
 
+    /// Enable or disable the collider after its creation.
+    pub fn enabled(mut self, enabled: bool) -> Self {
+        self.enabled = enabled;
+        self
+    }
+
     /// Builds a new collider attached to the given rigid-body.
     pub fn build(&self) -> Collider {
         let shape = self.shape.clone();
@@ -849,6 +885,11 @@ impl ColliderBuilder {
             active_collision_types: self.active_collision_types,
             active_hooks: self.active_hooks,
             active_events: self.active_events,
+            enabled: if self.enabled {
+                ColliderEnabled::Enabled
+            } else {
+                ColliderEnabled::Disabled
+            },
         };
         let changes = ColliderChanges::all();
         let pos = ColliderPosition(self.position);

src/geometry/collider_components.rs

@@ -64,6 +64,8 @@ bitflags::bitflags! {
         /// This flags is automatically set by the `PhysicsPipeline` when the `RigidBodyChanges::DOMINANCE`
         /// or `RigidBodyChanges::TYPE` of the parent rigid-body of this collider is detected.
         const PARENT_EFFECTIVE_DOMINANCE = 1 << 7; // NF update.
+        /// Flag indicating that whether or not the collider is enabled was changed.
+        const ENABLED_OR_DISABLED = 1 << 8; // BF & NF updates.
     }
 }
 
@@ -372,6 +374,19 @@ impl Default for ActiveCollisionTypes {
     }
 }
 
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// Enum indicating whether or not a collider is enabled.
+pub enum ColliderEnabled {
+    /// The collider is enabled.
+    Enabled,
+    /// The collider wasn’t disabled by the user explicitly but it is attached to
+    /// a disabled rigid-body.
+    DisabledByParent,
+    /// The collider is disabled by the user explicitly.
+    Disabled,
+}
+
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A set of flags for controlling collision/intersection filtering, modification, and events.
@@ -389,6 +404,8 @@ pub struct ColliderFlags {
     pub active_hooks: ActiveHooks,
     /// The events enabled for this collider.
     pub active_events: ActiveEvents,
+    /// Whether or not the collider is enabled.
+    pub enabled: ColliderEnabled,
 }
 
 impl Default for ColliderFlags {
@@ -399,6 +416,7 @@ impl Default for ColliderFlags {
             solver_groups: InteractionGroups::all(),
             active_hooks: ActiveHooks::empty(),
             active_events: ActiveEvents::empty(),
+            enabled: ColliderEnabled::Enabled,
         }
     }
 }

src/geometry/collider_set.rs

@@ -41,6 +41,14 @@ impl ColliderSet {
         self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c))
     }
 
+    /// Iterate through all the enabled colliders on this set.
+    pub fn iter_enabled(&self) -> impl Iterator<Item = (ColliderHandle, &Collider)> {
+        self.colliders
+            .iter()
+            .map(|(h, c)| (ColliderHandle(h), c))
+            .filter(|(_, c)| c.is_enabled())
+    }
+
     /// 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)> {
@@ -52,6 +60,12 @@ impl ColliderSet {
         })
     }
 
+    /// Iterates mutably through all the enabled colliders on this set.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
+    pub fn iter_enabled_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
+        self.iter_mut().filter(|(_, c)| c.is_enabled())
+    }
+
     /// The number of colliders on this set.
     pub fn len(&self) -> usize {
         self.colliders.len()
@@ -268,6 +282,18 @@ impl ColliderSet {
     pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
         self.colliders.get_mut(handle.0)
     }
+
+    // Just a very long name instead of `.get_mut` to make sure
+    // this is really the method we wanted to use instead of `get_mut_internal`.
+    #[allow(dead_code)]
+    pub(crate) fn get_mut_internal_with_modification_tracking(
+        &mut self,
+        handle: ColliderHandle,
+    ) -> Option<&mut Collider> {
+        let result = self.colliders.get_mut(handle.0)?;
+        Self::mark_as_modified(handle, result, &mut self.modified_colliders);
+        Some(result)
+    }
 }
 
 impl Index<crate::data::Index> for ColliderSet {

src/geometry/mod.rs

@@ -1,6 +1,9 @@
 //! Structures related to geometry: colliders, shapes, etc.
 
-pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair};
+pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
+
+pub use self::broad_phase_multi_sap::BroadPhase;
+// pub use self::broad_phase_qbvh::BroadPhase;
 pub use self::collider_components::*;
 pub use self::contact_pair::{
     ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -199,5 +202,6 @@ mod interaction_graph;
 mod interaction_groups;
 mod narrow_phase;
 
+mod broad_phase_qbvh;
 mod collider;
 mod collider_set;

src/geometry/narrow_phase.rs

@@ -299,7 +299,13 @@ impl NarrowPhase {
             }
         }
 
-        self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
+        self.handle_user_changes_on_colliders(
+            islands,
+            modified_colliders,
+            colliders,
+            bodies,
+            events,
+        );
     }
 
     pub(crate) fn remove_collider(
@@ -393,7 +399,7 @@ impl NarrowPhase {
         }
     }
 
-    pub(crate) fn handle_modified_colliders(
+    pub(crate) fn handle_user_changes_on_colliders(
         &mut self,
         mut islands: Option<&mut IslandManager>,
         modified_colliders: &[ColliderHandle],

src/pipeline/collision_pipeline.rs

@@ -5,7 +5,7 @@ use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
 };
 use crate::math::Real;
-use crate::pipeline::{EventHandler, PhysicsHooks};
+use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
 
 /// The collision pipeline, responsible for performing collision detection between colliders.
@@ -111,6 +111,7 @@ impl CollisionPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        query_pipeline: Option<&mut QueryPipeline>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -127,9 +128,20 @@ impl CollisionPipeline {
             None,
             bodies,
             colliders,
+            &mut ImpulseJointSet::new(),
+            &mut MultibodyJointSet::new(),
             &modified_bodies,
             &mut modified_colliders,
         );
+
+        // Disabled colliders are treated as if they were removed.
+        removed_colliders.extend(
+            modified_colliders
+                .iter()
+                .copied()
+                .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)),
+        );
+
         self.detect_collisions(
             prediction_distance,
             broad_phase,
@@ -143,6 +155,10 @@ impl CollisionPipeline {
             true,
         );
 
+        if let Some(queries) = query_pipeline {
+            queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true);
+        }
+
         self.clear_modified_colliders(colliders, &mut modified_colliders);
         removed_colliders.clear();
     }
@@ -187,6 +203,7 @@ mod tests {
             &mut narrow_phase,
             &mut rigid_body_set,
             &mut collider_set,
+            None,
             &physics_hooks,
             &(),
         );
@@ -238,6 +255,7 @@ mod tests {
             &mut narrow_phase,
             &mut rigid_body_set,
             &mut collider_set,
+            None,
             &physics_hooks,
             &(),
         );

src/pipeline/physics_pipeline.rs

@@ -5,7 +5,7 @@ use crate::counters::Counters;
 use crate::dynamics::IslandSolver;
 use crate::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
-    RigidBodyPosition, RigidBodyType,
+    RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType,
 };
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
@@ -14,7 +14,7 @@ use crate::geometry::{
     ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
-use crate::pipeline::{EventHandler, PhysicsHooks};
+use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
 use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
 
 /// The physics pipeline, responsible for stepping the whole physics simulation.
@@ -77,6 +77,18 @@ impl PhysicsPipeline {
         }
     }
 
+    fn clear_modified_bodies(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        modified_bodies: &mut Vec<RigidBodyHandle>,
+    ) {
+        for handle in modified_bodies.drain(..) {
+            if let Some(rb) = bodies.get_mut_internal(handle) {
+                rb.changes = RigidBodyChanges::empty();
+            }
+        }
+    }
+
     fn detect_collisions(
         &mut self,
         integration_parameters: &IntegrationParameters,
@@ -404,6 +416,7 @@ impl PhysicsPipeline {
         impulse_joints: &mut ImpulseJointSet,
         multibody_joints: &mut MultibodyJointSet,
         ccd_solver: &mut CCDSolver,
+        mut query_pipeline: Option<&mut QueryPipeline>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -422,21 +435,34 @@ impl PhysicsPipeline {
         // 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 modified_bodies = bodies.take_modified();
+        let mut modified_bodies = bodies.take_modified();
         super::user_changes::handle_user_changes_to_rigid_bodies(
             Some(islands),
             bodies,
             colliders,
+            impulse_joints,
+            multibody_joints,
             &modified_bodies,
             &mut modified_colliders,
         );
 
+        // 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)),
+        );
+
         // TODO: do this only on user-change.
         // TODO: do we want some kind of automatic inverse kinematics?
         for multibody in &mut multibody_joints.multibodies {
@@ -455,14 +481,19 @@ impl PhysicsPipeline {
             colliders,
             impulse_joints,
             multibody_joints,
-            &modified_colliders[..],
-            &mut removed_colliders,
+            &modified_colliders,
+            &removed_colliders,
             hooks,
             events,
             true,
         );
 
+        if let Some(queries) = query_pipeline.as_deref_mut() {
+            queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false);
+        }
+
         self.clear_modified_colliders(colliders, &mut modified_colliders);
+        self.clear_modified_bodies(bodies, &mut modified_bodies);
         removed_colliders.clear();
 
         let mut remaining_time = integration_parameters.dt;
@@ -582,13 +613,22 @@ impl PhysicsPipeline {
                 colliders,
                 impulse_joints,
                 multibody_joints,
-                &mut modified_colliders,
-                &mut removed_colliders,
+                &modified_colliders,
+                &[],
                 hooks,
                 events,
                 false,
             );
 
+            if let Some(queries) = query_pipeline.as_deref_mut() {
+                queries.update_incremental(
+                    colliders,
+                    &modified_colliders,
+                    &[],
+                    remaining_substeps == 0,
+                );
+            }
+
             self.clear_modified_colliders(colliders, &mut modified_colliders);
         }
 
@@ -650,6 +690,7 @@ mod test {
             &mut impulse_joints,
             &mut multibody_joints,
             &mut CCDSolver::new(),
+            None,
             &(),
             &(),
         );
@@ -705,6 +746,7 @@ mod test {
             &mut impulse_joints,
             &mut multibody_joints,
             &mut CCDSolver::new(),
+            None,
             &(),
             &(),
         );
@@ -807,6 +849,7 @@ mod test {
                 &mut impulse_joints,
                 &mut multibody_joints,
                 &mut ccd,
+                None,
                 &physics_hooks,
                 &event_handler,
             );

src/pipeline/query_pipeline.rs

@@ -1,10 +1,10 @@
-use crate::dynamics::{IslandManager, RigidBodyHandle};
+use crate::dynamics::RigidBodyHandle;
 use crate::geometry::{
     Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection,
 };
 use crate::math::{Isometry, Point, Real, Vector};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
-use parry::partitioning::QbvhDataGenerator;
+use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace};
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -30,8 +30,9 @@ pub struct QueryPipeline {
     )]
     query_dispatcher: Arc<dyn QueryDispatcher>,
     qbvh: Qbvh<ColliderHandle>,
-    tree_built: bool,
     dilation_factor: Real,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    workspace: QbvhUpdateWorkspace,
 }
 
 struct QueryPipelineAsCompositeShape<'a> {
@@ -310,8 +311,8 @@ impl QueryPipeline {
         Self {
             query_dispatcher: Arc::new(d),
             qbvh: Qbvh::new(),
-            tree_built: false,
             dilation_factor: 0.01,
+            workspace: QbvhUpdateWorkspace::default(),
         }
     }
 
@@ -320,25 +321,46 @@ impl QueryPipeline {
         &*self.query_dispatcher
     }
 
-    /// Update the acceleration structure on the query pipeline.
-    pub fn update(
+    /// Update the query pipeline incrementally, avoiding a complete rebuild of its
+    /// internal data-structure.
+    pub fn update_incremental(
         &mut self,
-        islands: &IslandManager,
-        bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        refit_and_rebalance: bool,
     ) {
-        self.update_with_mode(
-            islands,
-            bodies,
-            colliders,
-            QueryPipelineMode::CurrentPosition,
-        )
+        // We remove first. This is needed to avoid the ABA problem: if a collider was removed
+        // and another added right after with the same handle index, we can remove first, and
+        // then update the new one (but only if its actually exists, to address the case where
+        // a collider was added/modified and then removed during the same frame).
+        for removed in removed_colliders {
+            self.qbvh.remove(*removed);
+        }
+
+        for modified in modified_colliders {
+            // Check that the collider still exists as it may have been removed.
+            if colliders.contains(*modified) {
+                self.qbvh.pre_update_or_insert(*modified);
+            }
+        }
+
+        if refit_and_rebalance {
+            let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| {
+                colliders[*handle].compute_aabb()
+            });
+            self.qbvh.rebalance(0.0, &mut self.workspace);
+        }
+    }
+
+    /// Update the acceleration structure on the query pipeline.
+    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+        self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
     }
 
     /// Update the acceleration structure on the query pipeline.
     pub fn update_with_mode(
         &mut self,
-        islands: &IslandManager,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         mode: QueryPipelineMode,
@@ -358,12 +380,12 @@ impl QueryPipeline {
             fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) {
                 match self.mode {
                     QueryPipelineMode::CurrentPosition => {
-                        for (h, co) in self.colliders.iter() {
+                        for (h, co) in self.colliders.iter_enabled() {
                             f(h, co.shape.compute_aabb(&co.pos))
                         }
                     }
                     QueryPipelineMode::SweepTestWithNextPosition => {
-                        for (h, co) in self.colliders.iter() {
+                        for (h, co) in self.colliders.iter_enabled() {
                             if let Some(co_parent) = co.parent {
                                 let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position;
                                 let next_position = rb_next_pos * co_parent.pos_wrt_parent;
@@ -374,7 +396,7 @@ impl QueryPipeline {
                         }
                     }
                     QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                        for (h, co) in self.colliders.iter() {
+                        for (h, co) in self.colliders.iter_enabled() {
                             if let Some(co_parent) = co.parent {
                                 let rb = &self.bodies[co_parent.handle];
                                 let predicted_pos = rb.pos.integrate_forces_and_velocities(
@@ -392,71 +414,12 @@ impl QueryPipeline {
             }
         }
 
-        if !self.tree_built {
-            let generator = DataGenerator {
-                bodies,
-                colliders,
-                mode,
-            };
-            self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
-
-            // FIXME: uncomment this once we handle insertion/removals properly.
-            // self.tree_built = true;
-            return;
-        }
-
-        for handle in islands.iter_active_bodies() {
-            let rb = &bodies[handle];
-            for handle in &rb.colliders.0 {
-                self.qbvh.pre_update(*handle)
-            }
-        }
-
-        match mode {
-            QueryPipelineMode::CurrentPosition => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        co.shape.compute_aabb(&co.pos)
-                    },
-                    self.dilation_factor,
-                );
-            }
-            QueryPipelineMode::SweepTestWithNextPosition => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        if let Some(parent) = &co.parent {
-                            let rb_next_pos = &bodies[parent.handle].pos.next_position;
-                            let next_position = rb_next_pos * parent.pos_wrt_parent;
-                            co.shape.compute_swept_aabb(&co.pos, &next_position)
-                        } else {
-                            co.shape.compute_aabb(&co.pos)
-                        }
-                    },
-                    self.dilation_factor,
-                );
-            }
-            QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        if let Some(parent) = co.parent {
-                            let rb = &bodies[parent.handle];
-                            let predicted_pos = rb.pos.integrate_forces_and_velocities(
-                                dt, &rb.forces, &rb.vels, &rb.mprops,
-                            );
-
-                            let next_position = predicted_pos * parent.pos_wrt_parent;
-                            co.shape.compute_swept_aabb(&co.pos, &next_position)
-                        } else {
-                            co.shape.compute_aabb(&co.pos)
-                        }
-                    },
-                    self.dilation_factor,
-                );
-            }
-        }
+        let generator = DataGenerator {
+            bodies,
+            colliders,
+            mode,
+        };
+        self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
     }
 
     /// Find the closest intersection between a ray and a set of collider.

src/pipeline/user_changes.rs

@@ -1,7 +1,10 @@
 use crate::dynamics::{
-    IslandManager, RigidBodyChanges, RigidBodyHandle, RigidBodySet, RigidBodyType,
+    ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges,
+    RigidBodyHandle, RigidBodySet, RigidBodyType,
+};
+use crate::geometry::{
+    ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet,
 };
-use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet};
 
 pub(crate) fn handle_user_changes_to_colliders(
     bodies: &mut RigidBodySet,
@@ -21,10 +24,12 @@ pub(crate) fn handle_user_changes_to_colliders(
                 }
             }
 
-            if co
-                .changes
-                .intersects(ColliderChanges::SHAPE | ColliderChanges::LOCAL_MASS_PROPERTIES)
-            {
+            if co.changes.intersects(
+                ColliderChanges::SHAPE
+                    | ColliderChanges::LOCAL_MASS_PROPERTIES
+                    | ColliderChanges::ENABLED_OR_DISABLED
+                    | ColliderChanges::PARENT,
+            ) {
                 if let Some(rb) = co
                     .parent
                     .and_then(|p| bodies.get_mut_internal_with_modification_tracking(p.handle))
@@ -40,12 +45,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
     mut islands: Option<&mut IslandManager>,
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    _multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies
     modified_bodies: &[RigidBodyHandle],
     modified_colliders: &mut Vec<ColliderHandle>,
 ) {
     enum FinalAction {
-        UpdateActiveKinematicSetId,
-        UpdateActiveDynamicSetId,
+        UpdateActiveKinematicSetId(usize),
+        UpdateActiveDynamicSetId(usize),
+        RemoveFromIsland,
     }
 
     for handle in modified_bodies {
@@ -57,94 +65,92 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
         }
 
         let rb = bodies.index_mut_internal(*handle);
-        let mut changes = rb.changes;
         let mut ids = rb.ids;
-        let mut activation = rb.activation;
+        let changes = rb.changes;
+        let activation = rb.activation;
 
         {
-            // The body's status changed. We need to make sure
-            // it is on the correct active set.
-            if let Some(islands) = islands.as_deref_mut() {
-                if changes.contains(RigidBodyChanges::TYPE) {
-                    match rb.body_type {
-                        RigidBodyType::Dynamic => {
-                            // Remove from the active kinematic set if it was there.
-                            if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
-                                islands.active_kinematic_set.swap_remove(ids.active_set_id);
-                                final_action = Some((
-                                    FinalAction::UpdateActiveKinematicSetId,
-                                    ids.active_set_id,
-                                ));
+            if rb.is_enabled() {
+                // The body's status changed. We need to make sure
+                // it is on the correct active set.
+                if let Some(islands) = islands.as_deref_mut() {
+                    if changes.contains(RigidBodyChanges::TYPE) {
+                        match rb.body_type {
+                            RigidBodyType::Dynamic => {
+                                // Remove from the active kinematic set if it was there.
+                                if islands.active_kinematic_set.get(ids.active_set_id)
+                                    == Some(handle)
+                                {
+                                    islands.active_kinematic_set.swap_remove(ids.active_set_id);
+                                    final_action = Some(FinalAction::UpdateActiveKinematicSetId(
+                                        ids.active_set_id,
+                                    ));
+                                }
                             }
+                            RigidBodyType::KinematicVelocityBased
+                            | RigidBodyType::KinematicPositionBased => {
+                                // Remove from the active dynamic set if it was there.
+                                if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle)
+                                {
+                                    islands.active_dynamic_set.swap_remove(ids.active_set_id);
+                                    final_action = Some(FinalAction::UpdateActiveDynamicSetId(
+                                        ids.active_set_id,
+                                    ));
+                                }
 
-                            // Add to the active dynamic set.
-                            activation.wake_up(true);
-                            // Make sure the sleep change flag is set (even if for some
-                            // reasons the rigid-body was already awake) to make
-                            // sure the code handling sleeping change adds the body to
-                            // the active_dynamic_set.
-                            changes.set(RigidBodyChanges::SLEEP, true);
-                        }
-                        RigidBodyType::KinematicVelocityBased
-                        | RigidBodyType::KinematicPositionBased => {
-                            // Remove from the active dynamic set if it was there.
-                            if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) {
-                                islands.active_dynamic_set.swap_remove(ids.active_set_id);
-                                final_action = Some((
-                                    FinalAction::UpdateActiveDynamicSetId,
-                                    ids.active_set_id,
-                                ));
-                            }
-
-                            // Add to the active kinematic set.
-                            if islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) {
-                                ids.active_set_id = islands.active_kinematic_set.len();
-                                islands.active_kinematic_set.push(*handle);
+                                // Add to the active kinematic set.
+                                if islands.active_kinematic_set.get(ids.active_set_id)
+                                    != Some(handle)
+                                {
+                                    ids.active_set_id = islands.active_kinematic_set.len();
+                                    islands.active_kinematic_set.push(*handle);
+                                }
                             }
+                            RigidBodyType::Fixed => {}
                         }
-                        RigidBodyType::Fixed => {}
                     }
-                }
 
-                // Update the positions of the colliders.
-                if changes.contains(RigidBodyChanges::POSITION)
-                    || changes.contains(RigidBodyChanges::COLLIDERS)
-                {
-                    rb.colliders
-                        .update_positions(colliders, modified_colliders, &rb.pos.position);
-
-                    if rb.is_kinematic()
-                        && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
+                    // Update the active kinematic set.
+                    if changes.contains(RigidBodyChanges::POSITION)
+                        || changes.contains(RigidBodyChanges::COLLIDERS)
                     {
-                        ids.active_set_id = islands.active_kinematic_set.len();
-                        islands.active_kinematic_set.push(*handle);
+                        if rb.is_kinematic()
+                            && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
+                        {
+                            ids.active_set_id = islands.active_kinematic_set.len();
+                            islands.active_kinematic_set.push(*handle);
+                        }
+                    }
+
+                    // Push the body to the active set if it is not
+                    // sleeping and if it is not already inside of the active set.
+                    if changes.contains(RigidBodyChanges::SLEEP)
+                        && rb.is_enabled()
+                        && !rb.activation.sleeping // May happen if the body was put to sleep manually.
+                        && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+                        && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
+                    {
+                        ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+                        islands.active_dynamic_set.push(*handle);
                     }
                 }
+            }
 
-                // Push the body to the active set if it is not
-                // sleeping and if it is not already inside of the active set.
-                if changes.contains(RigidBodyChanges::SLEEP)
-                    && !activation.sleeping // May happen if the body was put to sleep manually.
-                    && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
-                    && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
-                {
-                    ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
-                    islands.active_dynamic_set.push(*handle);
-                }
-            } else {
-                // We don't use islands. So just update the colliders' positions.
-                if changes.contains(RigidBodyChanges::POSITION)
-                    || changes.contains(RigidBodyChanges::COLLIDERS)
-                {
-                    rb.colliders
-                        .update_positions(colliders, modified_colliders, &rb.pos.position);
-                }
+            // Update the colliders' positions.
+            if changes.contains(RigidBodyChanges::POSITION)
+                || changes.contains(RigidBodyChanges::COLLIDERS)
+            {
+                rb.colliders
+                    .update_positions(colliders, modified_colliders, &rb.pos.position);
             }
 
             if changes.contains(RigidBodyChanges::DOMINANCE)
                 || changes.contains(RigidBodyChanges::TYPE)
             {
                 for handle in rb.colliders.0.iter() {
+                    // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking`
+                    // here because that would modify the `modified_colliders` inside of the `ColliderSet`
+                    // instead of the one passed to this method.
                     let co = colliders.index_mut_internal(*handle);
                     if !co.changes.contains(ColliderChanges::MODIFIED) {
                         modified_colliders.push(*handle);
@@ -165,22 +171,68 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                 );
             }
 
-            rb.changes = RigidBodyChanges::empty();
+            if changes.contains(RigidBodyChanges::ENABLED_OR_DISABLED) {
+                // Propagate the rigid-body’s enabled/disable status to its colliders.
+                for handle in rb.colliders.0.iter() {
+                    // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking`
+                    // here because that would modify the `modified_colliders` inside of the `ColliderSet`
+                    // instead of the one passed to this method.
+                    let co = colliders.index_mut_internal(*handle);
+                    if !co.changes.contains(ColliderChanges::MODIFIED) {
+                        modified_colliders.push(*handle);
+                    }
+
+                    if rb.enabled && co.flags.enabled == ColliderEnabled::DisabledByParent {
+                        co.flags.enabled = ColliderEnabled::Enabled;
+                    } else if !rb.enabled && co.flags.enabled == ColliderEnabled::Enabled {
+                        co.flags.enabled = ColliderEnabled::DisabledByParent;
+                    }
+
+                    co.changes |= ColliderChanges::MODIFIED | ColliderChanges::ENABLED_OR_DISABLED;
+                }
+
+                // Propagate the rigid-body’s enabled/disable status to its attached impulse joints.
+                impulse_joints.map_attached_joints_mut(*handle, |_, _, _, joint| {
+                    if rb.enabled && joint.data.enabled == JointEnabled::DisabledByAttachedBody {
+                        joint.data.enabled = JointEnabled::Enabled;
+                    } else if !rb.enabled && joint.data.enabled == JointEnabled::Enabled {
+                        joint.data.enabled = JointEnabled::DisabledByAttachedBody;
+                    }
+                });
+
+                // FIXME: Propagate the rigid-body’s enabled/disable status to its attached multibody joints.
+
+                // Remove the rigid-body from the island manager.
+                if !rb.enabled {
+                    final_action = Some(FinalAction::RemoveFromIsland);
+                }
+            }
+
             rb.ids = ids;
             rb.activation = activation;
         }
 
         // Adjust some ids, if needed.
         if let Some(islands) = islands.as_deref_mut() {
-            if let Some((action, id)) = final_action {
-                let active_set = match action {
-                    FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set,
-                    FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set,
+            if let Some(action) = final_action {
+                match action {
+                    FinalAction::RemoveFromIsland => {
+                        let ids = rb.ids;
+                        islands.rigid_body_removed(*handle, &ids, bodies);
+                    }
+                    FinalAction::UpdateActiveKinematicSetId(id) => {
+                        let active_set = &mut islands.active_kinematic_set;
+                        if id < active_set.len() {
+                            bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
+                        }
+                    }
+                    FinalAction::UpdateActiveDynamicSetId(id) => {
+                        let active_set = &mut islands.active_dynamic_set;
+                        if id < active_set.len() {
+                            bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
+                        }
+                    }
                 };
-
-                if id < active_set.len() {
-                    bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
-                }
             }
         }
     }

src_testbed/harness/mod.rs

@@ -215,6 +215,7 @@ impl Harness {
                     &mut physics.impulse_joints,
                     &mut physics.multibody_joints,
                     &mut physics.ccd_solver,
+                    Some(&mut physics.query_pipeline),
                     &*physics.hooks,
                     event_handler,
                 );
@@ -233,16 +234,11 @@ impl Harness {
             &mut self.physics.impulse_joints,
             &mut self.physics.multibody_joints,
             &mut self.physics.ccd_solver,
+            Some(&mut self.physics.query_pipeline),
             &*self.physics.hooks,
             &self.event_handler,
         );
 
-        self.physics.query_pipeline.update(
-            &self.physics.islands,
-            &self.physics.bodies,
-            &self.physics.colliders,
-        );
-
         for plugin in &mut self.plugins {
             plugin.step(&mut self.physics, &self.state)
         }

src_testbed/testbed.rs

@@ -9,6 +9,8 @@ use crate::{debug_render, ui};
 use crate::{graphics::GraphicsManager, harness::RunState};
 
 use na::{self, Point2, Point3, Vector3};
+#[cfg(feature = "dim3")]
+use rapier::control::DynamicRayCastVehicleController;
 use rapier::control::KinematicCharacterController;
 use rapier::dynamics::{
     ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation,
@@ -100,6 +102,8 @@ pub struct TestbedState {
     pub draw_colls: bool,
     pub highlighted_body: Option<RigidBodyHandle>,
     pub character_body: Option<RigidBodyHandle>,
+    #[cfg(feature = "dim3")]
+    pub vehicle_controller: Option<DynamicRayCastVehicleController>,
     //    pub grabbed_object: Option<DefaultBodyPartHandle>,
     //    pub grabbed_object_constraint: Option<DefaultJointConstraintHandle>,
     pub grabbed_object_plane: (Point3<f32>, Vector3<f32>),
@@ -178,6 +182,8 @@ impl TestbedApp {
             draw_colls: false,
             highlighted_body: None,
             character_body: None,
+            #[cfg(feature = "dim3")]
+            vehicle_controller: None,
             //            grabbed_object: None,
             //            grabbed_object_constraint: None,
             grabbed_object_plane: (Point3::origin(), na::zero()),
@@ -456,6 +462,11 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         self.state.character_body = Some(handle);
     }
 
+    #[cfg(feature = "dim3")]
+    pub fn set_vehicle_controller(&mut self, controller: DynamicRayCastVehicleController) {
+        self.state.vehicle_controller = Some(controller);
+    }
+
     pub fn allow_grabbing_behind_ground(&mut self, allow: bool) {
         self.state.can_grab_behind_ground = allow;
     }
@@ -511,8 +522,12 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
             .action_flags
             .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
 
-        self.state.character_body = None;
         self.state.highlighted_body = None;
+        self.state.character_body = None;
+        #[cfg(feature = "dim3")]
+        {
+            self.state.vehicle_controller = None;
+        }
 
         #[cfg(all(feature = "dim2", feature = "other-backends"))]
         {
@@ -624,6 +639,50 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         self.plugins.0.push(Box::new(plugin));
     }
 
+    #[cfg(feature = "dim3")]
+    fn update_vehicle_controller(&mut self, events: &Input<KeyCode>) {
+        if self.state.running == RunMode::Stop {
+            return;
+        }
+
+        if let Some(vehicle) = &mut self.state.vehicle_controller {
+            let mut engine_force = 0.0;
+            let mut steering_angle = 0.0;
+
+            for key in events.get_pressed() {
+                match *key {
+                    KeyCode::Right => {
+                        steering_angle += -0.7;
+                    }
+                    KeyCode::Left => {
+                        steering_angle += 0.7;
+                    }
+                    KeyCode::Up => {
+                        engine_force += 30.0;
+                    }
+                    KeyCode::Down => {
+                        engine_force += -30.0;
+                    }
+                    _ => {}
+                }
+            }
+
+            let wheels = vehicle.wheels_mut();
+            wheels[0].engine_force = engine_force;
+            wheels[0].steering = steering_angle;
+            wheels[1].engine_force = engine_force;
+            wheels[1].steering = steering_angle;
+
+            vehicle.update_vehicle(
+                self.harness.physics.integration_parameters.dt,
+                &mut self.harness.physics.bodies,
+                &self.harness.physics.colliders,
+                &self.harness.physics.query_pipeline,
+                QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle.chassis),
+            );
+        }
+    }
+
     fn update_character_controller(&mut self, events: &Input<KeyCode>) {
         if self.state.running == RunMode::Stop {
             return;
@@ -1063,6 +1122,10 @@ fn update_testbed(
 
         testbed.handle_common_events(&*keys);
         testbed.update_character_controller(&*keys);
+        #[cfg(feature = "dim3")]
+        {
+            testbed.update_vehicle_controller(&*keys);
+        }
     }
 
     // Update UI
@@ -1245,10 +1308,10 @@ fn update_testbed(
             || state.prev_flags.contains(TestbedStateFlags::WIREFRAME)
                 != state.flags.contains(TestbedStateFlags::WIREFRAME)
         {
-            // graphics.toggle_wireframe_mode(
-            //     &harness.physics.colliders,
-            //     state.flags.contains(TestbedStateFlags::WIREFRAME),
-            // )
+            graphics.toggle_wireframe_mode(
+                &harness.physics.colliders,
+                state.flags.contains(TestbedStateFlags::WIREFRAME),
+            )
         }
 
         if state.prev_flags.contains(TestbedStateFlags::SLEEP)