1 files changed, 24 insertions, 2 deletions
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 584dc96..a2fcacc 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::MassProperties;
-use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
+use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
 use crate::utils::{WCross, WDot};
 use num::Zero;
@@ -137,6 +137,28 @@ impl RigidBody {
         crate::utils::inv(self.mass_properties.inv_mass)
     }
 
+    /// Adds a collider to this rigid-body.
+    pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
+        let mass_properties = coll
+            .mass_properties()
+            .transform_by(coll.position_wrt_parent());
+        self.colliders.push(handle);
+        self.mass_properties += mass_properties;
+        self.update_world_mass_properties();
+    }
+
+    /// 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) {
+            self.colliders.swap_remove(i);
+            let mass_properties = coll
+                .mass_properties()
+                .transform_by(coll.position_wrt_parent());
+            self.mass_properties -= mass_properties;
+            self.update_world_mass_properties();
+        }
+    }
+
     /// Put this rigid body to sleep.
     ///
     /// A sleeping body no longer moves and is no longer simulated by the physics engine unless
@@ -171,7 +193,7 @@ impl RigidBody {
     }
 
     fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
-        let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
+        let com = &self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }