Merge pull request #267 from dimforge/multibody

Implement multibody joints, and new velocity-based constraints solver

3 files changed, 17 insertions, 61 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index 0ed12b6..d2b0076 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -80,7 +80,6 @@ pub struct BroadPhase {
     layers: Vec<SAPLayer>,
     smallest_layer: u8,
     largest_layer: u8,
-    deleted_any: bool,
     // NOTE: we maintain this hashmap to simplify collider removal.
     //       This information is also present in the ColliderProxyId
     //       component. However if that component is removed, we need
@@ -133,7 +132,6 @@ impl BroadPhase {
             region_pool: Vec::new(),
             reporting: HashMap::default(),
             colliders_proxy_ids: HashMap::default(),
-            deleted_any: false,
         }
     }
 
@@ -622,7 +620,9 @@ impl BroadPhase {
 
 #[cfg(test)]
 mod test {
-    use crate::dynamics::{IslandManager, JointSet, RigidBodyBuilder, RigidBodySet};
+    use crate::dynamics::{
+        ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyBuilder, RigidBodySet,
+    };
     use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
 
     #[test]
@@ -630,7 +630,8 @@ mod test {
         let mut broad_phase = BroadPhase::new();
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
-        let mut joints = JointSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
+        let mut multibody_joints = MultibodyJointSet::new();
         let mut islands = IslandManager::new();
 
         let rb = RigidBodyBuilder::new_dynamic().build();
@@ -641,7 +642,13 @@ mod test {
         let mut events = Vec::new();
         broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
 
-        bodies.remove(hrb, &mut islands, &mut colliders, &mut joints);
+        bodies.remove(
+            hrb,
+            &mut islands,
+            &mut colliders,
+            &mut impulse_joints,
+            &mut multibody_joints,
+        );
         broad_phase.update(0.0, &mut colliders, &[], &[coh], &mut events);
 
         // Create another body.
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index f4e7834..92b7800 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -35,8 +35,6 @@ pub struct ContactData {
     /// collider's rigid-body.
     #[cfg(feature = "dim3")]
     pub tangent_impulse: na::Vector2<Real>,
-    /// The target velocity correction at the contact point.
-    pub rhs: Real,
 }
 
 impl Default for ContactData {
@@ -44,7 +42,6 @@ impl Default for ContactData {
         Self {
             impulse: 0.0,
             tangent_impulse: na::zero(),
-            rhs: 0.0,
         }
     }
 }
@@ -119,12 +116,9 @@ pub struct ContactManifoldData {
     pub rigid_body1: Option<RigidBodyHandle>,
     /// The second rigid-body involved in this contact manifold.
     pub rigid_body2: Option<RigidBodyHandle>,
-    pub(crate) warmstart_multiplier: Real,
     // The two following are set by the constraints solver.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     pub(crate) constraint_index: usize,
-    #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub(crate) position_constraint_index: usize,
     // We put the following fields here to avoids reading the colliders inside of the
     // contact preparation method.
     /// Flags used to control some aspects of the constraints solver for this contact manifold.
@@ -177,26 +171,15 @@ pub struct SolverContact {
     /// This is set to zero by default. Set to a non-zero value to
     /// simulate, e.g., conveyor belts.
     pub tangent_velocity: Vector<Real>,
-    /// The warmstart impulse, along the contact normal, applied by this contact to the first collider's rigid-body.
-    pub warmstart_impulse: Real,
-    /// The warmstart friction impulse along the vector orthonormal to the contact normal, applied to the first
-    /// collider's rigid-body.
-    #[cfg(feature = "dim2")]
-    pub warmstart_tangent_impulse: Real,
-    /// The warmstart friction impulses along the basis orthonormal to the contact normal, applied to the first
-    /// collider's rigid-body.
-    #[cfg(feature = "dim3")]
-    pub warmstart_tangent_impulse: na::Vector2<Real>,
-    /// The last velocity correction targeted by this contact.
-    pub prev_rhs: Real,
+    /// Whether or not this contact existed during the last timestep.
+    pub is_new: bool,
 }
 
 impl SolverContact {
     /// Should we treat this contact as a bouncy contact?
     /// If `true`, use [`Self::restitution`].
     pub fn is_bouncy(&self) -> bool {
-        let is_new = self.warmstart_impulse == 0.0;
-        if is_new {
+        if self.is_new {
             // Treat new collisions as bouncing at first, unless we have zero restitution.
             self.restitution > 0.0
         } else {
@@ -222,9 +205,7 @@ impl ContactManifoldData {
         Self {
             rigid_body1,
             rigid_body2,
-            warmstart_multiplier: Self::min_warmstart_multiplier(),
             constraint_index: 0,
-            position_constraint_index: 0,
             solver_flags,
             normal: Vector::zeros(),
             solver_contacts: Vec::new(),
@@ -239,34 +220,4 @@ impl ContactManifoldData {
     pub fn num_active_contacts(&self) -> usize {
         self.solver_contacts.len()
     }
-
-    pub(crate) fn min_warmstart_multiplier() -> Real {
-        // Multiplier used to reduce the amount of warm-starting.
-        // This coefficient increases exponentially over time, until it reaches 1.0.
-        // This will reduce significant overshoot at the timesteps that
-        // follow a timestep involving high-velocity impacts.
-        1.0 // 0.01
-    }
-
-    // pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {
-    //     // In 2D, tall stacks will actually suffer from this
-    //     // because oscillation due to inaccuracies in 2D often
-    //     // cause contacts to break, which would result in
-    //     // a reset of the warmstart multiplier.
-    //     if cfg!(feature = "dim2") {
-    //         manifold.data.warmstart_multiplier = 1.0;
-    //         return;
-    //     }
-    //
-    //     for pt in &manifold.points {
-    //         if pt.data.impulse != 0.0 {
-    //             manifold.data.warmstart_multiplier =
-    //                 (manifold.data.warmstart_multiplier * 2.0).min(1.0);
-    //             return;
-    //         }
-    //     }
-    //
-    //     // Reset the multiplier.
-    //     manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
-    // }
 }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 82ee99d..643b584 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -968,9 +968,7 @@ impl NarrowPhase {
                             friction,
                             restitution,
                             tangent_velocity: Vector::zeros(),
-                            warmstart_impulse: contact.data.impulse,
-                            warmstart_tangent_impulse: contact.data.tangent_impulse,
-                            prev_rhs: contact.data.rhs,
+                            is_new: contact.data.impulse == 0.0,
                         };
 
                         manifold.data.solver_contacts.push(solver_contact);
@@ -1027,7 +1025,7 @@ impl NarrowPhase {
     }
 
     /// Retrieve all the interactions with at least one contact point, happening between two active bodies.
-    // NOTE: this is very similar to the code from JointSet::select_active_interactions.
+    // NOTE: this is very similar to the code from ImpulseJointSet::select_active_interactions.
     pub(crate) fn select_active_contacts<'a, Bodies>(
         &'a mut self,
         islands: &IslandManager,