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

5 files changed, 86 insertions, 19 deletions

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));
+                        }
                     }
                 }
             }