Merge pull request #64 from dimforge/explicit_wake_up

Add more explicit parameters to wake-up a rigid-body

7 files changed, 107 insertions, 21 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 303d1a0..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);
         }
     }
 
@@ -388,6 +454,7 @@ pub struct RigidBodyBuilder {
     body_status: BodyStatus,
     mass_properties: MassProperties,
     can_sleep: bool,
+    sleeping: bool,
     user_data: u128,
 }
 
@@ -403,6 +470,7 @@ impl RigidBodyBuilder {
             body_status,
             mass_properties: MassProperties::zero(),
             can_sleep: true,
+            sleeping: false,
             user_data: 0,
         }
     }
@@ -531,11 +599,17 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// Sets whether or not the rigid-body is to be created asleep.
+    pub fn sleeping(mut self, sleeping: bool) -> Self {
+        self.sleeping = sleeping;
+        self
+    }
+
     /// Build a new rigid-body with the parameters configured with this builder.
     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;
@@ -544,6 +618,10 @@ impl RigidBodyBuilder {
         rb.linear_damping = self.linear_damping;
         rb.angular_damping = self.angular_damping;
 
+        if self.can_sleep && self.sleeping {
+            rb.sleep();
+        }
+
         if !self.can_sleep {
             rb.activation.threshold = -1.0;
         }
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index b857173..ec4d388 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -207,7 +207,7 @@ impl RigidBodySet {
          * Remove colliders attached to this rigid-body.
          */
         for collider in &rb.colliders {
-            colliders.remove(*collider, self);
+            colliders.remove(*collider, self, false);
         }
 
         /*
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/collider_set.rs b/src/geometry/collider_set.rs
index fd94675..60b9225 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -78,10 +78,14 @@ impl ColliderSet {
     }
 
     /// Remove a collider from this set and update its parent accordingly.
+    ///
+    /// If `wake_up` is `true`, the rigid-body the removed collider is attached to
+    /// will be woken up.
     pub fn remove(
         &mut self,
         handle: ColliderHandle,
         bodies: &mut RigidBodySet,
+        wake_up: bool,
     ) -> Option<Collider> {
         let collider = self.colliders.remove(handle)?;
 
@@ -90,7 +94,10 @@ impl ColliderSet {
          */
         if let Some(parent) = bodies.get_mut_internal(collider.parent) {
             parent.remove_collider_internal(handle, &collider);
-            bodies.wake_up(collider.parent, true);
+
+            if wake_up {
+                bodies.wake_up(collider.parent, true);
+            }
         }
 
         /*
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));
+                        }
                     }
                 }
             }