Implement multibody joints and the new solver

76 files changed, 7809 insertions, 11227 deletions

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 `IntegrationParams::dt` value directly"]
-    pub fn dt(&self) -> Real {
-        self.dt
-    }
-
     /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
     ///
     /// This is zero if `self.dt` is zero.
@@ -136,10 +82,10 @@ impl IntegrationParameters {
         }
     }
 
-    /// Convenience: `velocity_based_erp / dt`
+    /// Convenience: `erp / dt`
     #[inline]
-    pub(crate) fn velocity_based_erp_inv_dt(&self) -> Real {
-        self.velocity_based_erp * self.inv_dt()
+    pub(crate) fn erp_inv_dt(&self) -> Real {
+        self.erp * self.inv_dt()
     }
 }
 
@@ -148,20 +94,14 @@ impl Default for IntegrationParameters {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            //        multithreading_enabled:             true,
-            erp: 0.2,
-            joint_erp: 0.2,
             velocity_solve_fraction: 1.0,
-            velocity_based_erp: 0.0,
-            warmstart_coeff: 1.0,
-            warmstart_correction_slope: 10.0,
-            allowed_linear_error: 0.005,
+            erp: 0.8,
+            allowed_linear_error: 0.001, // 0.005
             prediction_distance: 0.002,
-            allowed_angular_error: 0.001,
-            max_linear_correction: 0.2,
-            max_angular_correction: 0.2,
             max_velocity_iterations: 4,
-            max_position_iterations: 1,
+            max_velocity_friction_iterations: 8,
+            max_stabilization_iterations: 1,
+            interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
             // FIXME: 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 10ac0f8..55b47c9 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -1,7 +1,7 @@
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::{
-    JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds,
-    RigidBodyType, RigidBodyVelocity,
+    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
+    RigidBodyIds, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{ColliderParent, NarrowPhase};
 use crate::math::Real;
@@ -175,7 +175,8 @@ impl IslandManager {
         bodies: &mut Bodies,
         colliders: &Colliders,
         narrow_phase: &NarrowPhase,
-        joints: &JointSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
         min_island_size: usize,
     ) where
         Bodies: ComponentSetMut<RigidBodyIds>
@@ -302,11 +303,15 @@ impl IslandManager {
             // in contact or joined with this collider.
             push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack);
 
-            for inter in joints.joints_with(handle) {
+            for inter in impulse_joints.joints_with(handle) {
                 let other = crate::utils::select_other((inter.0, inter.1), handle);
                 self.stack.push(other);
             }
 
+            for other in multibody_joints.attached_bodies(handle) {
+                self.stack.push(other);
+            }
+
             bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
                 activation.wake_up(false);
             });
diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
deleted file mode 100644
index 0b626c0..0000000
--- a/src/dynamics/joint/ball_joint.rs
+++ /dev/null
@@ -1,148 +0,0 @@
-use crate::dynamics::SpringModel;
-use crate::math::{Point, Real, Rotation, UnitVector, Vector};
-
-#[derive(Copy, Clone, PartialEq)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A joint that removes all relative linear motion between a pair of points on two bodies.
-pub struct BallJoint {
-    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
-    pub local_anchor1: Point<Real>,
-    /// Where the ball joint is attached on the second body, expressed in the second body local frame.
-    pub local_anchor2: Point<Real>,
-    /// The impulse applied by this joint on the first body.
-    ///
-    /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector<Real>,
-
-    /// The target relative angular velocity the motor will attempt to reach.
-    #[cfg(feature = "dim2")]
-    pub motor_target_vel: Real,
-    /// The target relative angular velocity the motor will attempt to reach.
-    #[cfg(feature = "dim3")]
-    pub motor_target_vel: Vector<Real>,
-    /// The target angular position of this joint, expressed as an axis-angle.
-    pub motor_target_pos: Rotation<Real>,
-    /// The motor's stiffness.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_stiffness: Real,
-    /// The motor's damping.
-    /// See the documentation of `SpringModel` for more information on this parameter.
-    pub motor_damping: Real,
-    /// The maximal impulse the motor is able to deliver.
-    pub motor_max_impulse: Real,
-    /// The angular impulse applied by the motor.
-    #[cfg(feature = "dim2")]
-    pub motor_impulse: Real,
-    /// The angular impulse applied by the motor.
-    #[cfg(feature = "dim3")]
-    pub motor_impulse: Vector<Real>,
-    /// The spring-like model used by the motor to reach the target velocity and .
-    pub motor_model: SpringModel,
-
-    /// Are the limits enabled for this joint?
-    pub limits_enabled: bool,
-    /// The axis of the limit cone for this joint, if the local-space of the first body.
-    pub limits_local_axis1: UnitVector<Real>,
-    /// The axis of the limit cone for this joint, if the local-space of the first body.
-    pub limits_local_axis2: UnitVector<Real>,
-    /// The maximum angle allowed between the two limit axes in world-space.
-    pub limits_angle: Real,
-    /// The impulse applied to enforce joint limits.
-    pub limits_impulse: Real,
-}
-
-impl BallJoint {
-    /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
-    pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
-        Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
-    }
-
-    pub(crate) fn with_impulse(
-        local_anchor1: Point<Real>,
-        local_anchor2: Point<Real>,
-        impulse: Vector<Real>,
-    ) -> Self {
-        Self {
-            local_anchor1,
-            local_anchor2,
-            impulse,
-            motor_target_vel: na::zero(),
-            motor_target_pos: Rotation::identity(),
-            motor_stiffness: 0.0,
-            motor_damping: 0.0,
-            motor_impulse: na::zero(),
-            motor_max_impulse: Real::MAX,
-            motor_model: SpringModel::default(),
-            limits_enabled: false,
-            limits_local_axis1: Vector::x_axis(),
-            limits_local_axis2: Vector::x_axis(),
-            limits_angle: Real::MAX,
-            limits_impulse: 0.0,
-        }
-    }
-
-    /// Can a SIMD constraint be used for resolving this joint?
-    pub fn supports_simd_constraints(&self) -> bool {
-        // SIMD ball constraints don't support motors and limits right now.
-        !self.limits_enabled
-            && (self.motor_max_impulse == 0.0
-                || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
-    }
-
-    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
-    pub fn configure_motor_model(&mut self, model: SpringModel) {
-        self.motor_model = model;
-    }
-
-    /// Sets the target velocity and velocity correction factor this motor.
-    #[cfg(feature = "dim2")]
-    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
-        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
-    }
-
-    /// Sets the target velocity and velocity correction factor this motor.
-    #[cfg(feature = "dim3")]
-    pub fn configure_motor_velocity(&mut self, target_vel: Vector<Real>, factor: Real) {
-        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
-    }
-
-    /// Sets the target orientation this motor needs to reach.
-    pub fn configure_motor_position(
-        &mut self,
-        target_pos: Rotation<Real>,
-        stiffness: Real,
-        damping: Real,
-    ) {
-        self.configure_motor(target_pos, na::zero(), stiffness, damping)
-    }
-
-    /// Sets the target orientation this motor needs to reach.
-    #[cfg(feature = "dim2")]
-    pub fn configure_motor(
-        &mut self,
-        target_pos: Rotation<Real>,
-        target_vel: Real,
-        stiffness: Real,
-        damping: Real,
-    ) {
-        self.motor_target_vel = target_vel;
-        self.motor_target_pos = target_pos;
-        self.motor_stiffness = stiffness;
-        self.motor_damping = damping;
-    }
-
-    /// Configure both the target orientation and target velocity of the motor.
-    #[cfg(feature = "dim3")]
-    pub fn configure_motor(
-        &mut self,
-        target_pos: Rotation<Real>,
-        target_vel: Vector<Real>,
-        stiffness: Real,
-        damping: Real,
-    ) {
-        self.motor_target_vel = target_vel;
-        self.motor_target_pos = target_pos;
-        self.motor_stiffness = stiffness;
-        self.motor_damping = damping