1 files changed, 47 insertions, 7 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 4783cfc..537e073 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1,5 +1,7 @@
 use crate::dynamics::MassProperties;
-use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
+use crate::geometry::{
+    Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
+};
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
 use crate::utils::{WCross, WDot};
 use num::Zero;
@@ -23,6 +25,17 @@ pub enum BodyStatus {
     // Disabled,
 }
 
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+    pub(crate) struct RigidBodyChanges: u32 {
+        const MODIFIED = 0b001;
+        const POSITION = 0b010;
+        const SLEEP = 0b100;
+        const COLLIDERS = 0b1000;
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A rigid body.
 ///
@@ -56,6 +69,7 @@ pub struct RigidBody {
     pub(crate) active_set_id: usize,
     pub(crate) active_set_offset: usize,
     pub(crate) active_set_timestamp: u32,
+    pub(crate) changes: RigidBodyChanges,
     /// The status of the body, governing how it is affected by external forces.
     pub body_status: BodyStatus,
     /// User-defined data associated to this rigid-body.
@@ -83,6 +97,7 @@ impl RigidBody {
             active_set_id: 0,
             active_set_offset: 0,
             active_set_timestamp: 0,
+            changes: RigidBodyChanges::all(),
             body_status: BodyStatus::Dynamic,
             user_data: 0,
         }
@@ -151,7 +166,14 @@ impl RigidBody {
     }
 
     /// Adds a collider to this rigid-body.
-    pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
+    pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) {
+        if !self.changes.contains(RigidBodyChanges::MODIFIED) {
+            self.changes.set(
+                RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
+                true,
+            );
+        }
+
         let mass_properties = coll
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
@@ -160,6 +182,14 @@ impl RigidBody {
         self.update_world_mass_properties();
     }
 
+    pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
+        for handle in &self.colliders {
+            let collider = &mut colliders[*handle];
+            collider.position = self.position * collider.delta;
+            collider.predicted_position = self.predicted_position * collider.delta;
+        }
+    }
+
     /// Removes a collider from this rigid-body.
     pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
         if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
@@ -189,7 +219,10 @@ impl RigidBody {
     /// If `strong` is `true` then it is assured that the rigid-body will
     /// remain awake during multiple subsequent timesteps.
     pub fn wake_up(&mut self, strong: bool) {
-        self.activation.sleeping = false;
+        if self.activation.sleeping {
+            self.changes.insert(RigidBodyChanges::SLEEP);
+            self.activation.sleeping = false;
+        }
 
         if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
             self.activation.energy = self.activation.threshold.abs() * 2.0;
@@ -301,14 +334,21 @@ impl RigidBody {
     /// 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.changes.insert(RigidBodyChanges::POSITION);
+        self.set_position_internal(pos);
+
+        // TODO: Do we really need to check that the body isn't dynamic?
+        if wake_up && self.is_dynamic() {
+            self.wake_up(true)
+        }
+    }
+
+    pub(crate) fn set_position_internal(&mut self, pos: Isometry<f32>) {
         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);
         }
     }
 
@@ -609,7 +649,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, false);
+        rb.set_position_internal(self.position);
         rb.linvel = self.linvel;
         rb.angvel = self.angvel;
         rb.body_status = self.body_status;