Merge pull request #267 from dimforge/multibody

Implement multibody joints, and new velocity-based constraints solver

85 files changed, 8148 insertions, 11508 deletions

diff --git a/src/counters/mod.rs b/src/counters/mod.rs
index 4d4d05d..324696f 100644
--- a/src/counters/mod.rs
+++ b/src/counters/mod.rs
@@ -14,7 +14,7 @@ mod solver_counters;
 mod stages_counters;
 mod timer;
 
-/// Aggregation of all the performances counters tracked by nphysics.
+/// Aggregation of all the performances counters tracked by rapier.
 #[derive(Clone, Copy)]
 pub struct Counters {
     /// Whether thi counter is enabled or not.
@@ -34,7 +34,7 @@ pub struct Counters {
 }
 
 impl Counters {
-    /// Create a new set of counters initialized to wero.
+    /// Create a new set of counters initialized to zero.
     pub fn new(enabled: bool) -> Self {
         Counters {
             enabled,
diff --git a/src/data/coarena.rs b/src/data/coarena.rs
index 124d85a..1f01c05 100644
--- a/src/data/coarena.rs
+++ b/src/data/coarena.rs
@@ -13,6 +13,14 @@ impl<T> Coarena<T> {
         Self { data: Vec::new() }
     }
 
+    pub fn iter(&self) -> impl Iterator<Item = (Index, &T)> {
+        self.data
+            .iter()
+            .enumerate()
+            .filter(|(_, elt)| elt.0 != u32::MAX)
+            .map(|(i, elt)| (Index::from_raw_parts(i as u32, elt.0), &elt.1))
+    }
+
     /// Gets a specific element from the coarena without specifying its generation number.
     ///
     /// It is strongly encouraged to use `Coarena::get` instead of this method because this method
@@ -23,12 +31,12 @@ impl<T> Coarena<T> {
 
     /// Deletes an element for the coarena and returns its value.
     ///
-    /// We can't really remove an element from the coarena. So instead of actually removing
-    /// it, this method will reset the value to the given `removed_value`.
+    /// This method will reset the value to the given `removed_value`.
     pub fn remove(&mut self, index: Index, removed_value: T) -> Option<T> {
         let (i, g) = index.into_raw_parts();
         let data = self.data.get_mut(i as usize)?;
         if g == data.0 {
+            data.0 = u32::MAX; // invalidate the generation number.
             Some(std::mem::replace(&mut data.1, removed_value))
         } else {
             None
diff --git a/src/data/graph.rs b/src/data/graph.rs
index de958c3..2dc7fbf 100644
--- a/src/data/graph.rs
+++ b/src/data/graph.rs
@@ -644,19 +644,24 @@ impl<'a, E> Iterator for Edges<'a, E> {
 
         // For iterate_over, "both" is represented as None.
         // For reverse, "no" is represented as None.
-        let (iterate_over, reverse) = (None, Some(self.direction.opposite()));
+        let (iterate_over, _reverse) = (None, Some(self.direction.opposite()));
 
         if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing {
             let i = self.next[0].index();
-            if let Some(Edge { node, weight, next }) = self.edges.get(i) {
+            if let Some(Edge {
+                node: _node,
+                weight,
+                next,
+            }) = self.edges.get(i)
+            {
                 self.next[0] = next[0];
                 return Some(EdgeReference {
                     index: EdgeIndex(i as u32),
-                    node: if reverse == Some(Direction::Outgoing) {
-                        swap_pair(*node)
-                    } else {
-                        *node
-                    },
+                    // node: if reverse == Some(Direction::Outgoing) {
+                    //     swap_pair(*node)
+                    // } else {
+                    //     *node
+                    // },
                     weight,
                 });
             }
@@ -674,11 +679,11 @@ impl<'a, E> Iterator for Edges<'a, E> {
 
                 return Some(EdgeReference {
                     index: edge_index,
-                    node: if reverse == Some(Direction::Incoming) {
-                        swap_pair(*node)
-                    } else {
-                        *node
-                    },
+                    // node: if reverse == Some(Direction::Incoming) {
+                    //     swap_pair(*node)
+                    // } else {
+                    //     *node
+                    // },
                     weight,
                 });
             }
@@ -688,10 +693,10 @@ impl<'a, E> Iterator for Edges<'a, E> {
     }
 }
 
-fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
-    x.swap(0, 1);
-    x
-}
+// fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
+//     x.swap(0, 1);
+//     x
+// }
 
 impl<'a, E> Clone for Edges<'a, E> {
     fn clone(&self) -> Self {
@@ -742,24 +747,11 @@ impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> {
     }
 }
 
-/// A “walker” object that can be used to step through the edge list of a node.
-///
-/// Created with [`.detach()`](struct.Neighbors.html#method.detach).
-///
-/// The walker does not borrow from the graph, so it lets you step through
-/// neighbors or incident edges while also mutating graph weights, as
-/// in the following example:
-#[derive(Clone)]
-pub struct WalkNeighbors {
-    skip_start: NodeIndex,
-    next: [EdgeIndex; 2],
-}
-
 /// Reference to a `Graph` edge.
 #[derive(Debug)]
 pub struct EdgeReference<'a, E: 'a> {
     index: EdgeIndex,
-    node: [NodeIndex; 2],
+    // node: [NodeIndex; 2],
     weight: &'a E,
 }
 
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 4725319..d998eec 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -18,18 +18,6 @@ pub struct IntegrationParameters {
     /// to numerical instabilities.
     pub min_ccd_dt: Real,
 
-    /// The Error Reduction Parameter in `[0, 1]` is the proportion of
-    /// the positional error to be corrected at each time step (default: `0.2`).
-    pub erp: Real,
-    /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
-    /// the positional error to be corrected at each time step (default: `0.2`).
-    pub joint_erp: Real,
-    /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
-    /// when they are re-used to initialize the solver (default `1.0`).
-    pub warmstart_coeff: Real,
-    /// Correction factor to avoid large warmstart impulse after a strong impact (default `10.0`).
-    pub warmstart_correction_slope: Real,
-
     /// 0-1: how much of the velocity to dampen out in the constraint solver?
     /// (default `1.0`).
     pub velocity_solve_fraction: Real,
@@ -40,23 +28,21 @@ pub struct IntegrationParameters {
     /// If non-zero, you do not need the positional solver.
     /// A good non-zero value is around `0.2`.
     /// (default `0.0`).
-    pub velocity_based_erp: Real,
+    pub erp: Real,
 
-    /// Amount of penetration the engine wont attempt to correct (default: `0.005m`).
+    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
     pub prediction_distance: Real,
-    /// Amount of angular drift of joint limits the engine wont
-    /// attempt to correct (default: `0.001rad`).
-    pub allowed_angular_error: Real,
-    /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
-    pub max_linear_correction: Real,
-    /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
-    pub max_angular_correction: Real,
-    /// Maximum number of iterations performed by the velocity constraints solver (default: `4`).
+    /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`).
     pub max_velocity_iterations: usize,
-    /// Maximum number of iterations performed by the position-based constraints solver (default: `1`).
-    pub max_position_iterations: usize,
+    /// Maximum number of iterations performed to solve friction constraints (default: `8`).
+    pub max_velocity_friction_iterations: usize,
+    /// Maximum number of iterations performed to remove the energy introduced by penetration corrections  (default: `1`).
+    pub max_stabilization_iterations: usize,
+    /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise,
+    /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`).
+    pub interleave_restitution_and_friction_resolution: bool,
     /// Minimum number of dynamic bodies in each active island (default: `128`).
     pub min_island_size: usize,
     /// Maximum number of substeps performed by the  solver (default: `1`).
@@ -64,46 +50,6 @@ pub struct IntegrationParameters {
 }
 
 impl IntegrationParameters {
-    /// Creates a set of integration parameters with the given values.
-    #[deprecated = "Use `IntegrationParameters { dt: 60.0, ..Default::default() }` instead"]
-    pub fn new(
-        dt: Real,
-        erp: Real,
-        joint_erp: Real,
-        warmstart_coeff: Real,
-        allowed_linear_error: Real,
-        allowed_angular_error: Real,
-        max_linear_correction: Real,
-        max_angular_correction: Real,
-        prediction_distance: Real,
-        max_velocity_iterations: usize,
-        max_position_iterations: usize,
-        max_ccd_substeps: usize,
-    ) -> Self {
-        IntegrationParameters {
-            dt,
-            erp,
-            joint_erp,
-            warmstart_coeff,
-            allowed_linear_error,
-            allowed_angular_error,
-            max_linear_correction,
-            max_angular_correction,
-            prediction_distance,
-            max_velocity_iterations,
-            max_position_iterations,
-            max_ccd_substeps,
-            ..Default::default()
-        }
-    }
-
-    /// The current time-stepping length.
-    #[inline(always)]
-    #[deprecated = "You can just read the `I