Allow the removal of a collider.

2020-08-31 17:58:14 +02:00
parent cc05bad041
commit df2156ffd0
7 changed files with 318 additions and 10 deletions
--- a/examples3d/compound3.rs
+++ b/examples3d/compound3.rs
@@ -0,0 +1,68 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.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);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -1,7 +1,7 @@
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
 use crate::utils;
 use num::Zero;
-use std::ops::{Add, AddAssign};
+use std::ops::{Add, AddAssign, Sub, SubAssign};
 #[cfg(feature = "dim3")]
 use {na::Matrix3, std::ops::MulAssign};

@@ -24,6 +24,59 @@ pub struct MassProperties {
    pub principal_inertia_local_frame: Rotation<f32>,
 }

+impl approx::AbsDiffEq for MassProperties {
+    type Epsilon = f32;
+    fn default_epsilon() -> Self::Epsilon {
+        f32::default_epsilon()
+    }
+
+    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
+        self.local_com.abs_diff_eq(&other.local_com, epsilon)
+            && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
+            && self
+                .inv_principal_inertia_sqrt
+                .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
+        // && self
+        //     .principal_inertia_local_frame
+        //     .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
+    }
+}
+
+impl approx::RelativeEq for MassProperties {
+    fn default_max_relative() -> Self::Epsilon {
+        f32::default_max_relative()
+    }
+
+    fn relative_eq(
+        &self,
+        other: &Self,
+        epsilon: Self::Epsilon,
+        max_relative: Self::Epsilon,
+    ) -> bool {
+        #[cfg(feature = "dim2")]
+        let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
+            &other.inv_principal_inertia_sqrt,
+            epsilon,
+            max_relative,
+        );
+
+        #[cfg(feature = "dim3")]
+        let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
+            &other.reconstruct_inverse_inertia_matrix(),
+            epsilon,
+            max_relative,
+        );
+
+        inertia_is_ok
+            && self
+                .local_com
+                .relative_eq(&other.local_com, epsilon, max_relative)
+            && self
+                .inv_mass
+                .relative_eq(&other.inv_mass, epsilon, max_relative)
+    }
+}
+
 impl MassProperties {
    #[cfg(feature = "dim2")]
    pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
@@ -90,6 +143,18 @@ impl MassProperties {
        }
    }

+    #[cfg(feature = "dim3")]
+    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
+    pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
+        let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
+        self.principal_inertia_local_frame.to_rotation_matrix()
+            * Matrix3::from_diagonal(&inv_principal_inertia)
+            * self
+                .principal_inertia_local_frame
+                .inverse()
+                .to_rotation_matrix()
+    }
+
    #[cfg(feature = "dim3")]
    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
    pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
@@ -143,6 +208,67 @@ impl Zero for MassProperties {
    }
 }

+impl Sub<MassProperties> for MassProperties {
+    type Output = Self;
+
+    #[cfg(feature = "dim2")]
+    fn sub(self, other: MassProperties) -> Self {
+        if self.is_zero() || other.is_zero() {
+            return self;
+        }
+
+        let m1 = utils::inv(self.inv_mass);
+        let m2 = utils::inv(other.inv_mass);
+        let inv_mass = utils::inv(m1 - m2);
+
+        let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
+        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+        let inertia = i1 - i2;
+        // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
+        let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt());
+
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    fn sub(self, other: MassProperties) -> Self {
+        if self.is_zero() || other.is_zero() {
+            return self;
+        }
+
+        let m1 = utils::inv(self.inv_mass);
+        let m2 = utils::inv(other.inv_mass);
+        let inv_mass = utils::inv(m1 - m2);
+        let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
+        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+        let inertia = i1 - i2;
+        let eigen = inertia.symmetric_eigen();
+        let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
+        let principal_inertia = eigen.eigenvalues;
+        // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
+        let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt()));
+
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+            principal_inertia_local_frame,
+        }
+    }
+}
+
+impl SubAssign<MassProperties> for MassProperties {
+    fn sub_assign(&mut self, rhs: MassProperties) {
+        *self = *self - rhs
+    }
+}
+
 impl Add<MassProperties> for MassProperties {
    type Output = Self;

@@ -204,3 +330,40 @@ impl AddAssign<MassProperties> for MassProperties {
        *self = *self + rhs
    }
 }
+
+#[cfg(test)]
+mod test {
+    use super::MassProperties;
+    use crate::geometry::ColliderBuilder;
+    use approx::assert_relative_eq;
+    use num::Zero;
+
+    #[test]
+    fn mass_properties_add_sub() {
+        // Check that addition and subtraction of mass properties behave as expected.
+        let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build();
+        let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build();
+        let c3 = ColliderBuilder::ball(5.0).build();
+
+        let m1 = c1.mass_properties();
+        let m2 = c2.mass_properties();
+        let m3 = c3.mass_properties();
+        let m1m2m3 = m1 + m2 + m3;
+
+        assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6);
+        assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6);
+        assert_relative_eq!(
+            m1m2m3 - m1 - m2 - m3,
+            MassProperties::zero(),
+            epsilon = 1.0e-6
+        );
+    }
+}
--- 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,24 @@ 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();
+        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();
+            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
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -662,7 +662,7 @@ mod test {
        let rb = RigidBodyBuilder::new_dynamic().build();
        let co = ColliderBuilder::ball(0.5).build();
        let hrb = bodies.insert(rb);
-        let hco = colliders.insert(co, hrb, &mut bodies);
+        colliders.insert(co, hrb, &mut bodies);

        broad_phase.update_aabbs(0.0, &bodies, &mut colliders);

@@ -681,7 +681,7 @@ mod test {
        let rb = RigidBodyBuilder::new_dynamic().build();
        let co = ColliderBuilder::ball(0.5).build();
        let hrb = bodies.insert(rb);
-        let hco = colliders.insert(co, hrb, &mut bodies);
+        colliders.insert(co, hrb, &mut bodies);

        // Make sure the proxy handles is recycled properly.
        broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -3,7 +3,7 @@ use crate::geometry::{
    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
    Proximity, Triangle, Trimesh,
 };
-use crate::math::{Isometry, Point, Vector};
+use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use na::Point3;
 use ncollide::bounding_volume::{HasBoundingVolume, AABB};
 use num::Zero;
@@ -159,6 +159,11 @@ impl Collider {
        &self.position
    }

+    /// The position of this collider wrt the body it is attached to.
+    pub fn position_wrt_parent(&self) -> &Isometry<f32> {
+        &self.delta
+    }
+
    /// The density of this collider.
    pub fn density(&self) -> f32 {
        self.density
@@ -347,7 +352,41 @@ impl ColliderBuilder {
        self
    }

+    /// Sets the initial translation of the collider to be created,
+    /// relative to the rigid-body it is attached to.
+    #[cfg(feature = "dim2")]
+    pub fn translation(mut self, x: f32, y: f32) -> Self {
+        self.delta.translation.x = x;
+        self.delta.translation.y = y;
+        self
+    }
+
+    /// Sets the initial translation of the collider to be created,
+    /// relative to the rigid-body it is attached to.
+    #[cfg(feature = "dim3")]
+    pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
+        self.delta.translation.x = x;
+        self.delta.translation.y = y;
+        self.delta.translation.z = z;
+        self
+    }
+
+    /// Sets the initial orientation of the collider to be created,
+    /// relative to the rigid-body it is attached to.
+    pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
+        self.delta.rotation = Rotation::new(angle);
+        self
+    }
+
+    /// Sets the initial position (translation and orientation) of the collider to be created,
+    /// relative to the rigid-body it is attached to.
+    pub fn position(mut self, pos: Isometry<f32>) -> Self {
+        self.delta = pos;
+        self
+    }
+
    /// Set the position of this collider in the local-space of the rigid-body it is attached to.
+    #[deprecated(note = "Use `.position` instead.")]
    pub fn delta(mut self, delta: Isometry<f32>) -> Self {
        self.delta = delta;
        self
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -47,7 +47,6 @@ impl ColliderSet {
        parent_handle: RigidBodyHandle,
        bodies: &mut RigidBodySet,
    ) -> ColliderHandle {
-        let mass_properties = coll.mass_properties();
        coll.parent = parent_handle;
        let parent = bodies
            .get_mut_internal(parent_handle)
@@ -55,9 +54,8 @@ impl ColliderSet {
        coll.position = parent.position * coll.delta;
        coll.predicted_position = parent.predicted_position * coll.delta;
        let handle = self.colliders.insert(coll);
-        parent.colliders.push(handle);
-        parent.mass_properties += mass_properties;
-        parent.update_world_mass_properties();
+        let coll = self.colliders.get(handle).unwrap();
+        parent.add_collider_internal(handle, &coll);
        bodies.activate(parent_handle);
        handle
    }
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandl
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
-    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
+    BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet,
+    ContactManifoldIndex, NarrowPhase,
 };
 use crate::math::Vector;
 use crate::pipeline::EventHandler;
@@ -245,6 +246,27 @@ impl PhysicsPipeline {
        self.counters.step_completed();
    }

+    /// Remove a collider and all its associated data.
+    pub fn remove_collider(
+        &mut self,
+        handle: ColliderHandle,
+        broad_phase: &mut BroadPhase,
+        narrow_phase: &mut NarrowPhase,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+    ) -> Option<Collider> {
+        broad_phase.remove_colliders(&[handle], colliders);
+        narrow_phase.remove_colliders(&[handle], colliders, bodies);
+        let collider = colliders.remove_internal(handle)?;
+
+        if let Some(parent) = bodies.get_mut_internal(collider.parent) {
+            parent.remove_collider_internal(handle, &collider);
+            bodies.wake_up(collider.parent);
+        }
+
+        Some(collider)
+    }
+
    /// Remove a rigid-body and all its associated data.
    pub fn remove_rigid_body(
        &mut self,