Add explicit wake_up parameter to method setting the position and velocity of a rigid-body.

14 files changed, 154 insertions, 48 deletions

diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs
index 67075fe..a3d223d 100644
--- a/examples2d/add_remove2.rs
+++ b/examples2d/add_remove2.rs
@@ -24,7 +24,7 @@ pub fn init_world(testbed: &mut Testbed) {
         let to_remove: Vec<_> = physics
             .bodies
             .iter()
-            .filter(|(_, b)| b.position.translation.vector.y < -10.0)
+            .filter(|(_, b)| b.position().translation.vector.y < -10.0)
             .map(|e| e.0)
             .collect();
         for handle in to_remove {
diff --git a/examples2d/platform2.rs b/examples2d/platform2.rs
index fd836a3..3f55cb6 100644
--- a/examples2d/platform2.rs
+++ b/examples2d/platform2.rs
@@ -62,7 +62,7 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     testbed.add_callback(move |_, physics, _, _, time| {
         let mut platform = physics.bodies.get_mut(platform_handle).unwrap();
-        let mut next_pos = platform.position;
+        let mut next_pos = *platform.position();
 
         let dt = 0.016;
         next_pos.translation.vector.y += (time * 5.0).sin() * dt;
diff --git a/examples3d/add_remove3.rs b/examples3d/add_remove3.rs
index 6b58adf..77350e5 100644
--- a/examples3d/add_remove3.rs
+++ b/examples3d/add_remove3.rs
@@ -3,43 +3,82 @@ use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
 
+const MAX_NUMBER_OF_BODIES: usize = 400;
+
 pub fn init_world(testbed: &mut Testbed) {
-    let bodies = RigidBodySet::new();
-    let colliders = ColliderSet::new();
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
     let joints = JointSet::new();
     let rad = 0.5;
 
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 2.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+    let mut k = 0;
+
     // Callback that will be executed on the main loop to handle proximities.
     testbed.add_callback(move |window, physics, _, graphics, _| {
+        k += 1;
         let rigid_body = RigidBodyBuilder::new_dynamic()
             .translation(0.0, 10.0, 0.0)
             .build();
         let handle = physics.bodies.insert(rigid_body);
-        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        let collider = match k % 3 {
+            0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
+            1 => ColliderBuilder::cone(rad, rad).build(),
+            _ => ColliderBuilder::cuboid(rad, rad, rad).build(),
+        };
+
         physics
             .colliders
             .insert(collider, handle, &mut physics.bodies);
         graphics.add(window, handle, &physics.bodies, &physics.colliders);
 
-        let to_remove: Vec<_> = physics
-            .bodies
-            .iter()
-            .filter(|(_, b)| b.position.translation.vector.y < -10.0)
-            .map(|e| e.0)
-            .collect();
-        for handle in to_remove {
-            physics
+        if physics.bodies.len() > MAX_NUMBER_OF_BODIES {
+            let mut to_remove: Vec<_> = physics
                 .bodies
-                .remove(handle, &mut physics.colliders, &mut physics.joints);
-            graphics.remove_body_nodes(window, handle);
+                .iter()
+                .filter(|e| e.1.is_dynamic())
+                .map(|e| (e.0, e.1.position().translation.vector))
+                .collect();
+
+            to_remove.sort_by(|a, b| {
+                (a.1.x.abs() + a.1.z.abs())
+                    .partial_cmp(&(b.1.x.abs() + b.1.z.abs()))
+                    .unwrap()
+                    .reverse()
+            });
+
+            let num_to_remove = to_remove.len() - MAX_NUMBER_OF_BODIES;
+            for (handle, _) in &to_remove[..num_to_remove] {
+                physics
+                    .bodies
+                    .remove(*handle, &mut physics.colliders, &mut physics.joints);
+                physics.broad_phase.maintain(&mut physics.colliders);
+                physics
+                    .narrow_phase
+                    .maintain(&mut physics.colliders, &mut physics.bodies);
+                graphics.remove_body_nodes(window, *handle);
+            }
         }
+
+        println!("Num bodies: {}", physics.bodies.len());
     });
 
     /*
      * Set up the testbed.
      */
     testbed.set_world(bodies, colliders, joints);
-    testbed.look_at(Point3::new(-30.0, -4.0, -30.0), Point3::new(0.0, 1.0, 0.0));
+    testbed.look_at(Point3::new(-30.0, 4.0, -30.0), Point3::new(0.0, 1.0, 0.0));
 }
 
 fn main() {
diff --git a/examples3d/damping3.rs b/examples3d/damping3.rs
index e055d8e..8c68d3b 100644
--- a/examples3d/damping3.rs
+++ b/examples3d/damping3.rs
@@ -12,7 +12,7 @@ pub fn init_world(testbed: &mut Testbed) {
     let joints = JointSet::new();
 
     /*
-     * Create the balls
+     * Create the cubes
      */
     let num = 10;
     let rad = 0.2;
diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs
index 6c3483e..0843300 100644
--- a/examples3d/platform3.rs
+++ b/examples3d/platform3.rs
@@ -72,7 +72,7 @@ pub fn init_world(testbed: &mut Testbed) {
         }
 
         if let Some(mut platform) = physics.bodies.get_mut(platform_handle) {
-            let mut next_pos = platform.position;
+            let mut next_pos = *platform.position();
 
             let dt = 0.016;
             next_pos.translation.vector.y += (time * 5.0).sin() * dt;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 6704c51..4783cfc 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -30,7 +30,7 @@ pub enum BodyStatus {
 #[derive(Debug, Clone)]
 pub struct RigidBody {
     /// The world-space position of the rigid-body.
-    pub position: Isometry<f32>,
+    pub(crate) position: Isometry<f32>,
     pub(crate) predicted_position: Isometry<f32>,
     /// The local mass properties of the rigid-body.
     pub mass_properties: MassProperties,
@@ -39,9 +39,9 @@ pub struct RigidBody {
     /// The square-root of the inverse angular inertia tensor of the rigid-body.
     pub world_inv_inertia_sqrt: AngularInertia<f32>,
     /// The linear velocity of the rigid-body.
-    pub linvel: Vector<f32>,
+    pub(crate) linvel: Vector<f32>,
     /// The angular velocity of the rigid-body.
-    pub angvel: AngVector<f32>,
+    pub(crate) angvel: AngVector<f32>,
     /// Damping factor for gradually slowing down the translational motion of the rigid-body.
     pub linear_damping: f32,
     /// Damping factor for gradually slowing down the angular motion of the rigid-body.
@@ -231,18 +231,84 @@ impl RigidBody {
         self.position = self.integrate_velocity(dt) * self.position;
     }
 
+    /// The linear velocity of this rigid-body.
+    pub fn linvel(&self) -> &Vector<f32> {
+        &self.linvel
+    }
+
+    /// The angular velocity of this rigid-body.
+    #[cfg(feature = "dim2")]
+    pub fn angvel(&self) -> f32 {
+        self.angvel
+    }
+
+    /// The angular velocity of this rigid-body.
+    #[cfg(feature = "dim3")]
+    pub fn angvel(&self) -> &Vector<f32> {
+        &self.angvel
+    }
+
+    /// The linear velocity of this rigid-body.
+    ///
+    /// If `wake_up` is `true` then the rigid-body will be woken up if it was
+    /// put to sleep because it did not move for a while.
+    pub fn set_linvel(&mut self, linvel: Vector<f32>, wake_up: bool) {
+        self.linvel = linvel;
+
+        if self.is_dynamic() && wake_up {
+            self.wake_up(true)
+        }
+    }
+
+    /// The angular velocity of this rigid-body.
+    ///
+    /// If `wake_up` is `true` then the rigid-body will be woken up if it was
+    /// put to sleep because it did not move for a while.
+    #[cfg(feature = "dim2")]
+    pub fn set_angvel(&mut self, angvel: f32, wake_up: bool) {
+        self.angvel = angvel;
+
+        if self.is_dynamic() && wake_up {
+            self.wake_up(true)
+        }
+    }
+
+    /// The angular velocity of this rigid-body.
+    ///
+    /// If `wake_up` is `true` then the rigid-body will be woken up if it was
+    /// put to sleep because it did not move for a while.
+    #[cfg(feature = "dim3")]
+    pub fn set_angvel(&mut self, angvel: Vector<f32>, wake_up: bool) {
+        self.angvel = angvel;
+
+        if self.is_dynamic() && wake_up {
+            self.wake_up(true)
+        }
+    }
+
+    /// The world-space position of this rigid-body.
+    pub fn position(&self) -> &Isometry<f32> {
+        &self.position
+    }
+
     /// Sets the position and `next_kinematic_position` of this rigid body.
     ///
     /// This will teleport the rigid-body to the specified position/orientation,
     /// completely ignoring any physics rule. If this body is kinematic, this will
     /// also set the next kinematic position to the same value, effectively
     /// resetting to zero the next interpolated velocity of the kinematic body.
-    pub fn set_position(&mut self, pos: Isometry<f32>) {
+    ///
+    /// If `wake_up` is `true` then the rigid-body will be woken up if it was
+    /// put to sleep because it did not move for a while.
+    pub fn set_position(&mut self, pos: Isometry<f32>, wake_up: bool) {
         self.position = pos;
 
         // TODO: update the predicted position for dynamic bodies too?
         if self.is_static() || self.is_kinematic() {
             self.predicted_position = pos;
+        } else if wake_up {
+            // wake_up is true and the rigid-body is dynamic.
+            self.wake_up(true);
         }
     }
 
@@ -543,7 +609,7 @@ impl RigidBodyBuilder {
     pub fn build(&self) -> RigidBody {
         let mut rb = RigidBody::new();
         rb.predicted_position = self.position; // FIXME: compute the correct value?
-        rb.set_position(self.position);
+        rb.set_position(self.position, false);
         rb.linvel = self.linvel;
         rb.angvel = self.angvel;
         rb.body_status = self.body_status;
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index edbfa40..dd5e535 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -250,7 +250,7 @@ impl ParallelIslandSolver {
                     let batch_size = thread.batch_size;
                     for handle in active_bodies[thread.position_writeback_index] {
                         let rb = &mut bodies[*handle];
-                        rb.set_position(positions[rb.active_set_offset]);
+                        rb.set_position(positions[rb.active_set_offset], false);
                     }
                 }
             })
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index 2cfd629..dac1d9e 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -120,7 +120,7 @@ impl PositionSolver {
         }
 
         bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-            rb.set_position(self.positions[rb.active_set_offset])
+            rb.set_position(self.positions[rb.active_set_offset], false)
         });
     }
 }
diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs
index 5c96f68..66840a0 100644
--- a/src/geometry/shape.rs
+++ b/src/geometry/shape.rs
@@ -18,7 +18,7 @@ use {
 /// Enum representing the type of a shape.
 pub enum ShapeType {
     /// A ball shape.
-    Ball = 1,
+    Ball = 0,
     /// A convex polygon shape.
     Polygon,
     /// A cuboid shape.
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index c21574a..665fee8 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -72,17 +72,18 @@ impl QueryPipeline {
         let mut result = None;
 
         for handle in inter {
-            let collider = &colliders[handle];
-            if collider.collision_groups.test(groups) {
-                if let Some(inter) = collider.shape().toi_and_normal_with_ray(
-                    collider.position(),
-                    ray,
-                    max_toi,
-                    true,
-                ) {
-                    if inter.toi < best {
-                        best = inter.toi;
-                        result = Some((handle, collider, inter));
+            if let Some(collider) = colliders.get(handle) {
+                if collider.collision_groups.test(groups) {
+                    if let Some(inter) = collider.shape().toi_and_normal_with_ray(
+                        collider.position(),
+                        ray,
+                        max_toi,
+                        true,
+                    ) {
+                        if inter.toi < best {
+                            best = inter.toi;
+                            result = Some((handle, collider, inter));
+                        }
                     }
                 }
             }
diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index 213819a..0d0664f 100644
--- a/src_testbed/box2d_backend.rs
+++ b/src_testbed/box2d_backend.rs
@@ -71,10 +71,10 @@ impl Box2dWorld {
 
             let def = b2::BodyDef {
                 body_type,
-                position: na_vec_to_b2_vec(body.position.translation.vector),
-                angle: body.position.rotation.angle(),
-                linear_velocity: na_vec_to_b2_vec(body.linvel),
-                angular_velocity: body.angvel,
+                position: na_vec_to_b2_vec(body.position().translation.vector),
+                angle: body.position().rotation.angle(),
+                linear_velocity: na_vec_to_b2_vec(*body.linvel()),
+                angular_velocity: body.angvel(),
                 linear_damping,
                 angular_damping,
                 ..b2::BodyDef::new()
@@ -223,7 +223,7 @@ impl Box2dWorld {
             if let Some(pb2_handle) = self.rapier2box2d.get(&handle) {
                 let b2_body = self.world.body(*pb2_handle);
                 let pos = b2_transform_to_na_isometry(b2_body.transform().clone());
-                body.set_position(pos);
+                body.set_position(pos, false);
 
                 for coll_handle in body.colliders() {
                     let collider = &mut colliders[*coll_handle];
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index 2896377..0ef3d36 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -46,7 +46,7 @@ impl NPhysicsWorld {
 
         for (rapier_handle, rb) in bodies.iter() {
             // let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0);
-            let nphysics_rb = RigidBodyDesc::new().position(rb.position).build();
+            let nphysics_rb = RigidBodyDesc::new().position(*rb.position()).build();
             let nphysics_rb_handle = nphysics_bodies.insert(nphysics_rb);
 
             rapier2nphysics.insert(rapier_handle, nphysics_rb_handle);
@@ -161,7 +161,7 @@ impl NPhysicsWorld {
             let mut rb = bodies.get_mut(*rapier_handle).unwrap();
             let ra = self.bodies.rigid_body(*nphysics_handle).unwrap();
             let pos = *ra.position();
-            rb.set_position(pos);
+            rb.set_position(pos, false);
 
             for coll_handle in rb.colliders() {
                 let collider = &mut colliders[*coll_handle];
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 74d6af2..7fcb813 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -154,7 +154,7 @@ impl PhysxWorld {
             use physx::rigid_static::RigidStatic;
             use physx::transform;
 
-            let pos = transform::gl_to_px_tf(rb.position.to_homogeneous().into_glam());
+            let pos = transform::gl_to_px_tf(rb.position().to_homogeneous().into_glam());
             if rb.is_dynamic() {
                 let actor = unsafe {
                     physx_sys::PxPhysics_createRigidDynamic_mut(physics.get_raw_mut(), &pos)
@@ -406,7 +406,7 @@ impl PhysxWorld {
             let ra = self.scene.get_rigid_actor(*physx_handle).unwrap();
             let pos = ra.get_global_pose().into_na();
             let iso = na::convert_unchecked(pos);
-            rb.set_position(iso);
+            rb.set_position(iso, false);
 
             if rb.is_kinematic() {}
 
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 4b998c3..1e0dd5e 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -414,7 +414,7 @@ impl Testbed {
         {
             if self.state.selected_backend == BOX2D_BACKEND {
                 self.box2d = Some(Box2dWorld::from_rapier(
-                    self.gravity,
+                    self.physics.gravity,
                     &self.physics.bodies,
                     &self.physics.colliders,
                     &self.physics.joints,
@@ -647,7 +647,7 @@ impl Testbed {
                                 if self.state.selected_backend == BOX2D_BACKEND {
                                     self.box2d.as_mut().unwrap().step(
                                         &mut self.physics.pipeline.counters,
-                                        &self.integration_parameters,
+                                        &self.physics.integration_parameters,
                                     );
                                     self.box2d.as_mut().unwrap().sync(
                                         &mut self.physics.bodies,