Merge pull request #315 from dimforge/debug-renderer

Add a basic lines-based debug-renderer

4 files changed, 39 insertions, 13 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 77a1ff7..bdde135 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -4,7 +4,7 @@ use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
 use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
-use crate::prelude::ActiveEvents;
+use crate::prelude::{ActiveEvents, CollisionEventFlags};
 use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
 use parry::utils::hashmap::HashMap;
 use std::collections::BinaryHeap;
@@ -529,8 +529,18 @@ impl CCDSolver {
                     .contains(ActiveEvents::COLLISION_EVENTS)
             {
                 // Emit one intersection-started and one intersection-stopped event.
-                events.handle_collision_event(CollisionEvent::Started(toi.c1, toi.c2), None);
-                events.handle_collision_event(CollisionEvent::Stopped(toi.c1, toi.c2, false), None);
+                events.handle_collision_event(
+                    bodies,
+                    colliders,
+                    CollisionEvent::Started(toi.c1, toi.c2, CollisionEventFlags::SENSOR),
+                    None,
+                );
+                events.handle_collision_event(
+                    bodies,
+                    colliders,
+                    CollisionEvent::Stopped(toi.c1, toi.c2, CollisionEventFlags::SENSOR),
+                    None,
+                );
             }
         }
 
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 84c8117..6a86a2a 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -93,12 +93,12 @@ impl IntegrationParameters {
 
     /// The ERP coefficient, multiplied by the inverse timestep length.
     pub fn erp_inv_dt(&self) -> Real {
-        self.erp / self.dt
+        self.erp * self.inv_dt()
     }
 
     /// The joint ERP coefficient, multiplied by the inverse timestep length.
     pub fn joint_erp_inv_dt(&self) -> Real {
-        self.joint_erp / self.dt
+        self.joint_erp * self.inv_dt()
     }
 
     /// The CFM factor to be used in the constraints resolution.
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index 748530f..06dff5d 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -299,10 +299,20 @@ impl MultibodyJointSet {
     }
 
     /// Gets a mutable reference to the multibody identified by its `handle`.
+    pub fn get_mut(&mut self, handle: MultibodyJointHandle) -> Option<(&mut Multibody, usize)> {
+        let link = self.rb2mb.get(handle.0)?;
+        let multibody = self.multibodies.get_mut(link.multibody.0)?;
+        Some((multibody, link.id))
+    }
+
+    /// Gets a mutable reference to the multibody identified by its `handle`.
+    ///
+    /// This method will bypass any modification-detection automatically done by the MultibodyJointSet.
     pub fn get_mut_internal(
         &mut self,
         handle: MultibodyJointHandle,
     ) -> Option<(&mut Multibody, usize)> {
+        // TODO: modification tracking?
         let link = self.rb2mb.get(handle.0)?;
         let multibody = self.multibodies.get_mut(link.multibody.0)?;
         Some((multibody, link.id))
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index cf52c1f..24e9754 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -305,20 +305,26 @@ impl RigidBody {
         self.ccd.ccd_active
     }
 
-    /// Sets the rigid-body's initial mass properties.
+    /// Sets the rigid-body's additional mass properties.
     ///
     /// 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.
     #[inline]
-    pub fn set_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
-        if self.mprops.local_mprops != props {
-            if self.is_dynamic() && wake_up {
-                self.wake_up(true);
-            }
+    pub fn set_additional_mass_properties(&mut self, props: MassProperties, wake_up: bool) {
+        if let Some(add_mprops) = &mut self.mprops.additional_local_mprops {
+            self.mprops.local_mprops += props;
+            self.mprops.local_mprops -= **add_mprops;
+            **add_mprops = props;
+        } else {
+            self.mprops.additional_local_mprops = Some(Box::new(props));
+            self.mprops.local_mprops += props;
+        }
 
-            self.mprops.local_mprops = props;
-            self.update_world_mass_properties();
+        if self.is_dynamic() && wake_up {
+            self.wake_up(true);
         }
+
+        self.update_world_mass_properties();
     }
 
     /// The handles of colliders attached to this rigid body.