Merge pull request #579 from dimforge/joints-improvements

Feat: implement a "small-steps" velocity-based constraints solver + joint improvements

59 files changed, 6087 insertions, 4226 deletions

diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs
index 1e2da51..30979e1 100644
--- a/src/control/ray_cast_vehicle_controller.rs
+++ b/src/control/ray_cast_vehicle_controller.rs
@@ -4,7 +4,7 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{ColliderHandle, ColliderSet, Ray};
 use crate::math::{Point, Real, Rotation, Vector};
 use crate::pipeline::{QueryFilter, QueryPipeline};
-use crate::utils::{WCross, WDot};
+use crate::utils::{SimdCross, SimdDot};
 
 /// A character controller to simulate vehicles using ray-casting for the wheels.
 pub struct DynamicRayCastVehicleController {
diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs
index 84807fa..a73ca85 100644
--- a/src/dynamics/ccd/mod.rs
+++ b/src/dynamics/ccd/mod.rs
@@ -1,3 +1,6 @@
+// TODO: not sure why it complains about PredictedImpacts being unused,
+//       making it private or pub(crate) triggers a different error.
+#[allow(unused_imports)]
 pub use self::ccd_solver::{CCDSolver, PredictedImpacts};
 pub use self::toi_entry::TOIEntry;
 
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 6d3acc5..d07527f 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -1,4 +1,5 @@
 use crate::math::Real;
+use std::num::NonZeroUsize;
 
 /// Parameters for a time-step of the physics engine.
 #[derive(Copy, Clone, Debug)]
@@ -43,15 +44,10 @@ pub struct IntegrationParameters {
     pub max_penetration_correction: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
     pub prediction_distance: Real,
-    /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`).
-    pub max_velocity_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,
+    /// Number of iterations performed to solve friction constraints at solver iteration (default: `2`).
+    pub num_friction_iteration_per_solver_iteration: usize,
+    /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
+    pub num_solver_iterations: NonZeroUsize,
     /// 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`).
@@ -151,17 +147,15 @@ impl Default for IntegrationParameters {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            erp: 0.8,
-            damping_ratio: 0.25,
+            erp: 0.6,
+            damping_ratio: 1.0,
             joint_erp: 1.0,
             joint_damping_ratio: 1.0,
-            allowed_linear_error: 0.001, // 0.005
+            allowed_linear_error: 0.001,
             max_penetration_correction: Real::MAX,
             prediction_distance: 0.002,
-            max_velocity_iterations: 4,
-            max_velocity_friction_iterations: 8,
-            max_stabilization_iterations: 1,
-            interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
+            num_friction_iteration_per_solver_iteration: 2,
+            num_solver_iterations: NonZeroUsize::new(4).unwrap(),
             // TODO: what is the optimal value for min_island_size?
             // It should not be too big so that we don't end up with
             // huge islands that don't fit in cache.
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index ef5d50a..43ac4ec 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -4,7 +4,7 @@ use crate::dynamics::{
 };
 use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
-use crate::utils::WDot;
+use crate::utils::SimdDot;
 
 /// Structure responsible for maintaining the set of active rigid-bodies, and
 /// putting non-moving rigid-bodies to sleep to save computation times.
@@ -14,6 +14,7 @@ pub struct IslandManager {
     pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
     pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
     pub(crate) active_islands: Vec<usize>,
+    pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
     active_set_timestamp: u32,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     can_sleep: Vec<RigidBodyHandle>, // Workspace.
@@ -28,6 +29,7 @@ impl IslandManager {
             active_dynamic_set: vec![],
             active_kinematic_set: vec![],
             active_islands: vec![],
+            active_islands_additional_solver_iterations: vec![],
             active_set_timestamp: 0,
             can_sleep: vec![],
             stack: vec![],
@@ -127,6 +129,10 @@ impl IslandManager {
         &self.active_dynamic_set[island_range]
     }
 
+    pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize {
+        self.active_islands_additional_solver_iterations[island_id]
+    }
+
     #[inline(always)]
     pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator<Item = RigidBodyHandle> + 'a {
         self.active_dynamic_set
@@ -136,6 +142,7 @@ impl IslandManager {
     }
 
     #[cfg(feature = "parallel")]
+    #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism.
     pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
         self.active_islands[island_id]..self.active_islands[island_id + 1]
     }
@@ -232,12 +239,21 @@ impl IslandManager {
         //        let t = instant::now();
         // Propagation of awake state and awake island computation through the
         // traversal of the interaction graph.
+        self.active_islands_additional_solver_iterations.clear();
         self.active_islands.clear();
         self.active_islands.push(0);
 
         // The max avoid underflow when the stack is empty.
         let mut island_marker = self.stack.len().max(1) - 1;
 
+        // NOTE: islands containing a body with non-standard number of iterations won’t
+        //       be merged with another island, unless another island with standard
+        //       iterations number already started before and got continued due to the
+        //       `min_island_size`. That could be avoided by pushing bodies with non-standard
+        //       iterations on top of the stack (and other bodies on the back). Not sure it’s
+        //       worth it though.
+        let mut additional_solver_iterations = 0;
+
         while let Some(handle) = self.stack.pop() {
             let rb = bodies.index_mut_internal(handle);
 
@@ -248,16 +264,23 @@ impl IslandManager {
             }
 
             if self.stack.len() < island_marker {
-                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
-                    >= min_island_size
+                if additional_solver_iterations != rb.additional_solver_iterations
+                    || self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
+                        >= min_island_size
                 {
                     // We are starting a new island.
+                    self.active_islands_additional_solver_iterations
+                        .push(additional_solver_iterations);
                     self.active_islands.push(self.active_dynamic_set.len());
+                    additional_solver_iterations = 0;
                 }
 
                 island_marker = self.stack.len();
             }
 
+            additional_solver_iterations =
+                additional_solver_iterations.max(rb.additional_solver_iterations);
+
             // Transmit the active state to all the rigid-bodies with colliders
             // in contact or joined with this collider.
             push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
@@ -281,6 +304,8 @@ impl IslandManager {
             self.active_dynamic_set.push(handle);
         }
 
+        self.active_islands_additional_solver_iterations
+            .push(additional_solver_iterations);
         self.active_islands.push(self.active_dynamic_set.len());
         //        println!(
         //            "Extraction: {}, num islands: {}",
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index 5096e15..83e27be 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::solver::MotorParameters;
 use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
 use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
-use crate::utils::{WBasis, WReal};
+use crate::utils::{SimdBasis, SimdRealCopy};
 
 #[cfg(feature = "dim3")]
 use crate::dynamics::SphericalJoint;
@@ -121,7 +121,7 @@ pub struct JointLimits<N> {
     pub impulse: N,
 }
 
-impl<N: WReal> Default for JointLimits<N> {
+impl<N: SimdRealCopy> Default for JointLimits<N> {
     fn default() -> Self {
         Self {
             min: -N::splat(Real::MAX),
@@ -131,6 +131,16 @@ impl<N: WReal> Default for JointLimits<N> {
     }
 }
 
+impl<N: SimdRealCopy> From<[N; 2]> for JointLimits<N> {
+    fn from(value: [N; 2]) -> Self {
+        Self {
+            min: value[0],
+            max: value[1],
+            impulse: N::splat(0.0),
+        }
+    }
+}
+
 /// A joint’s motor along one of its degrees of freedom.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -210,14 +220,23 @@ pub struct GenericJoint {
     /// The degrees-of-freedoms motorised by this joint.
     pub motor_axes: JointAxesMask,
     /// The coupled degrees of freedom of this joint.
+    ///
+    /// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors.
+    /// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first
+    /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized
+    /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF.
     pub coupled_axes: JointAxesMask,
     /// The limits, along each degrees of freedoms of this joint.
     ///
     /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask.
+    /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis`
+    /// bitmask is applied to the coupled linear (resp. angular) axes.
     pub limits: [JointLimits<Real>; SPATIAL_DIM],
     /// The motors, along each degrees of freedoms of this joint.
     ///
-    /// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
+    /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask.
+    /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
+    /// bitmask is applied to the coupled linear (resp. angular) axes.
     pub motors: [JointMotor; SPATIAL_DIM],
     /// Are contacts between the attached rigid-bodies enabled?
     pub contacts_enabled: bool,
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index f21f950..67f8949 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -214,7 +214,8 @@ impl ImpulseJointSet {
     //     //     .map(|e| &mut e.weight)
     // }
 
-    #[cfg(not(feature = "parallel"))]
+    // #[cfg(not(feature = "parallel"))]
+    #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism.
     pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
         &mut self.joint_graph.graph.edges[..]
     }
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index 93cb0ab..423d4c2 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -6,6 +6,7 @@ pub use self::multibody_joint::*;
 pub use self::prismatic_joint::*;
 pub use self::revolute_joint::*;
 pub use self::rope_joint::*;
+pub use self::spring_joint::*;
 
 #[cfg(feature = "dim3")]
 pub use self::spherical_joint::*;
@@ -21,3 +22,4 @@ mod rope_joint;
 
 #[cfg(feature = "dim3")]
 mod spherical_joint;
+mod spring_joint;
diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs
index a701350..c789004 100644
--- a/src/dynamics/joint/multibody_joint/mod.rs
+++ b/src/dynamics/joint/multibody_joint/mod.rs
@@ -2,7 +2,9 @@
 
 pub use self::multibody::Multibody;
 pub use self::multibody_joint::MultibodyJoint;
-pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
+pub use self::multibody_joint_set::{
+    MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
+};
 pub use self::multibody_link::MultibodyLink;
 pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};
 
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index 63e87e2..c65a6ff 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -1,16 +1,13 @@
 use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
 use super::multibody_workspace::MultibodyWorkspace;
-use crate::dynamics::{
-    solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet,
-    RigidBodyType, RigidBodyVelocity,
-};
+use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
 #[cfg(feature = "dim3")]
 use crate::math::Matrix;
 use crate::math::{
     AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
 };
 use crate::prelude::MultibodyJoint;
-use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
+use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
 use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU};
 
 #[repr(C)]
@@ -372,6 +369,7 @@ impl Multibody {
 
         self.accelerations.fill(0.0);
 
+        // Eqn 42 to 45
         for i in 0..self.links.len() {
             let link = &self.links[i];
             let rb = &bodies[link.rigid_body];
@@ -400,7 +398,7 @@ impl Multibody {
             }
 
             acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23));
-            acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
+            acc.linvel += acc.angvel.gcross(link.shift23);
 
             self.workspace.accs[i] = acc;
 
@@ -728,7 +726,7 @@ impl Multibody {
 
     /// The generalized velocity at the multibody_joint of the given link.
     #[inline]
-    pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
+    pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
         let ndofs = link.joint().ndofs();
         DVectorView::from_slice(
             &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
@@ -829,8 +827,10 @@ impl Multibody {
         }
     }
 
+    // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
+    //       (i.e. just something used by the velocity solver’s small steps).
     /// Apply forward-kinematics to this multibody and its related rigid-bodies.
-    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
+    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
         // Special case for the root, which has no parent.
         {
             let link = &mut self.links[0];
@@ -839,7 +839,7 @@ impl Multibody {
 
             if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
                 rb.pos.next_position = link.local_to_world;
-                if update_mass_props {
+                if update_rb_mass_props {
                     rb.mprops.update_world_mass_properties(&link.local_to_world);
                 }
             }
@@ -873,7 +873,7 @@ impl Multibody {
                 "A rigid-body that is not at the root of a multibody must be dynamic."
             );
 
-            if update_mass_props {
+            if update_rb_mass_props {
                 link_rb
                     .mprops
                     .update_world_mass_properties(&link.local_to_world);
@@ -943,6 +943,7 @@ impl Multibody {
 
     #[cfg(feature = "parallel")]
     #[inline]
+    #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism.
     pub(crate) fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) {
         let num_constraints: usize = self
             .links
@@ -951,40 +952,4 @@ impl Multibody {
             .sum();
         (num_constraints, num_constraints)
     }
-
-    #[inline]
-    pub(crate) fn generate_internal_constraints(
-        &self,
-        params: &IntegrationParameters,
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
-        out: &mut Vec<AnyJointVelocityConstraint>,
-        mut insert_at: Option<usize>,
-    ) {
-        if !cfg!(feature = "parallel") {
-            let num_constraints: usize = self
-                .links
-                .iter()
-                .map(|l| l.joint().num_velocity_constraints())
-                .sum();
-
-            let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
-            if jacobians.nrows() < required_jacobian_len {
-                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
-            }
-        }
-
-        for link in self.links.iter() {
-            link.joint().velocity_constraints(
-                params,
-                self,
-                link,
-                0,
-                j_id,
-                jacobians,
-                out,
-                &mut insert_at,
-            );
-        }
-    }
 }
diff --git a/src/d