Merge pull request #116 from EmbarkStudios/corrective-velocity-solve

Optional violation correction in velocity solver

src/dynamics/integration_parameters.rs

@@ -27,6 +27,19 @@ pub struct IntegrationParameters {
     /// 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,
+
+    /// 0-1: how much of the velocity to dampen out in the constraint solver?
+    /// (default `1.0`).
+    pub velocity_solve_fraction: Real,
+
+    /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
+    /// will be compensated for during the velocity solve.
+    /// If zero, you need to enable the positional solver.
+    /// 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,
+
     /// Amount of penetration the engine wont attempt to correct (default: `0.005m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
@@ -121,17 +134,12 @@ impl IntegrationParameters {
             max_stabilization_multiplier,
             max_velocity_iterations,
             max_position_iterations,
-            // 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.
-            // However we don't want it to be too small and end up with
-            // tons of islands, reducing SIMD parallelism opportunities.
-            min_island_size: 128,
             max_ccd_position_iterations,
             max_ccd_substeps,
             return_after_ccd_substep,
             multiple_ccd_substep_sensor_events_enabled,
             ccd_on_penetration_enabled,
+            ..Default::default()
         }
     }
 
@@ -173,6 +181,12 @@ impl IntegrationParameters {
             self.dt = 1.0 / inv_dt
         }
     }
+
+    /// Convenience: `velocity_based_erp / dt`
+    #[inline]
+    pub(crate) fn velocity_based_erp_inv_dt(&self) -> Real {
+        self.velocity_based_erp * self.inv_dt()
+    }
 }
 
 impl Default for IntegrationParameters {
@@ -183,6 +197,8 @@ impl Default for IntegrationParameters {
             return_after_ccd_substep: false,
             erp: 0.2,
             joint_erp: 0.2,
+            velocity_solve_fraction: 1.0,
+            velocity_based_erp: 0.0,
             warmstart_coeff: 1.0,
             allowed_linear_error: 0.005,
             prediction_distance: 0.002,

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -40,17 +40,20 @@ impl BallVelocityConstraint {
         rb2: &RigidBody,
         joint: &BallJoint,
     ) -> Self {
-        let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com;
+        let anchor_world1 = rb1.position * joint.local_anchor1;
-        let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com;
+        let anchor_world2 = rb2.position * joint.local_anchor2;
+        let anchor1 = anchor_world1 - rb1.world_com;
+        let anchor2 = anchor_world2 - rb2.world_com;
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
 
-        let rhs = -(vel1 - vel2);
+        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
-        let lhs;
+            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
 
+        let lhs;
         let cmat1 = anchor1.gcross_matrix();
         let cmat2 = anchor2.gcross_matrix();
 
@@ -271,22 +274,27 @@ impl BallVelocityGroundConstraint {
         joint: &BallJoint,
         flipped: bool,
     ) -> Self {
-        let (anchor1, anchor2) = if flipped {
+        let (anchor_world1, anchor_world2) = if flipped {
             (
-                rb1.position * joint.local_anchor2 - rb1.world_com,
+                rb1.position * joint.local_anchor2,
-                rb2.position * joint.local_anchor1 - rb2.world_com,
+                rb2.position * joint.local_anchor1,
             )
         } else {
             (
-                rb1.position * joint.local_anchor1 - rb1.world_com,
+                rb1.position * joint.local_anchor1,
-                rb2.position * joint.local_anchor2 - rb2.world_com,
+                rb2.position * joint.local_anchor2,
             )
         };
 
+        let anchor1 = anchor_world1 - rb1.world_com;
+        let anchor2 = anchor_world2 - rb2.world_com;
+
         let im2 = rb2.effective_inv_mass;
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
-        let rhs = vel2 - vel1;
+
+        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+            + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
 
         let cmat2 = anchor2.gcross_matrix();
 

src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs

@@ -62,12 +62,15 @@ impl WBallVelocityConstraint {
         let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
         let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
 
-        let anchor1 = position1 * local_anchor1 - world_com1;
+        let anchor_world1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2 - world_com2;
+        let anchor_world2 = position2 * local_anchor2;
+        let anchor1 = anchor_world1 - world_com1;
+        let anchor2 = anchor_world2 - world_com2;
 
         let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
         let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
-        let rhs = -(vel1 - vel2);
+        let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
+            + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
         let lhs;
 
         let cmat1 = anchor1.gcross_matrix();
@@ -239,12 +242,15 @@ impl WBallVelocityGroundConstraint {
         );
         let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
 
-        let anchor1 = position1 * local_anchor1 - world_com1;
+        let anchor_world1 = position1 * local_anchor1;
-        let anchor2 = position2 * local_anchor2 - world_com2;
+        let anchor_world2 = position2 * local_anchor2;
+        let anchor1 = anchor_world1 - world_com1;
+        let anchor2 = anchor_world2 - world_com2;
 
         let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
         let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
-        let rhs = vel2 - vel1;
+        let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction)
+            + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt());
         let lhs;
 
         let cmat2 = anchor2.gcross_matrix();

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -103,12 +103,33 @@ impl FixedVelocityConstraint {
         let ang_dvel = -rb1.angvel + rb2.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs =
+            Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
 
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
+            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                let ang_err = ang_err.angle();
+                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err = ang_err.scaled_axis();
+                rhs += Vector6::new(
+                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+                ) * velocity_based_erp_inv_dt;
+            }
+        }
 
         FixedVelocityConstraint {
             joint_id,
@@ -293,11 +314,32 @@ impl FixedVelocityGroundConstraint {
         let ang_dvel = rb2.angvel - rb1.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs =
+            Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * params.velocity_solve_fraction;
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
+            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                let ang_err = ang_err.angle();
+                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err = ang_err.scaled_axis();
+                rhs += Vector6::new(
+                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+                ) * velocity_based_erp_inv_dt;
+            }
+        }
 
         FixedVelocityGroundConstraint {
             joint_id,

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -10,7 +10,7 @@ use crate::math::{
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix6, Vector6, U3};
+use na::{Cholesky, Matrix6, Vector3, Vector6, U3};
 #[cfg(feature = "dim2")]
 use {
     na::{Matrix3, Vector3},
@@ -132,13 +132,38 @@ impl WFixedVelocityConstraint {
         let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
         let ang_dvel = -angvel1 + angvel2;
 
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
+
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
 
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
+            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                let ang_err = ang_err.angle();
+                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err =
+                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                rhs += Vector6::new(
+                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+                ) * velocity_based_erp_inv_dt;
+            }
+        }
 
         WFixedVelocityConstraint {
             joint_id,
@@ -373,12 +398,38 @@ impl WFixedVelocityGroundConstraint {
         let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
         let ang_dvel = angvel2 - angvel1;
 
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
+
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let mut rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel) * velocity_solve_fraction;
+
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let mut rhs = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
-        );
+        ) * velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            let lin_err = anchor2.translation.vector - anchor1.translation.vector;
+            let ang_err = anchor2.rotation * anchor1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                let ang_err = ang_err.angle();
+                rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err =
+                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                rhs += Vector6::new(
+                    lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+                ) * velocity_based_erp_inv_dt;
+            }
+        }
 
         WFixedVelocityGroundConstraint {
             joint_id,

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -48,10 +48,15 @@ pub(crate) struct PrismaticVelocityConstraint {
     motor_inv_lhs: Real,
     motor_max_impulse: Real,
 
+    limits_active: bool,
     limits_impulse: Real,
-    limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
+    /// World-coordinate direction of the limit force on rb2.
+    /// The force direction on rb1 is opposite (Newton's third law)..
+    limits_forcedir2: Vector<Real>,
     limits_rhs: Real,
     limits_inv_lhs: Real,
+    /// min/max applied impulse due to limits
+    limits_impulse_limits: (Real, Real),
 
     #[cfg(feature = "dim2")]
     basis1: Vector2<Real>,
@@ -135,13 +140,39 @@ impl PrismaticVelocityConstraint {
         #[cfg(feature = "dim3")]
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let ang_rhs = rb2.angvel - rb1.angvel;
+        let angvel_err = rb2.angvel - rb1.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
         #[cfg(feature = "dim3")]
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            angvel_err.x,
+            angvel_err.y,
+            angvel_err.z,
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
+
+            let frame1 = rb1.position * joint.local_frame1();
+            let frame2 = rb2.position * joint.local_frame2();
+            let ang_err = frame2.rotation * frame1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err = ang_err.scaled_axis();
+                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
+                    * velocity_based_erp_inv_dt;
+            }
+        }
 
         /*
          * Setup motor.
@@ -176,31 +207,35 @@ impl PrismaticVelocityConstraint {
             joint.motor_max_impulse,
         );
 
-        /*
+        // Setup limit constraint.
-         * Setup limit constraint.
+        let mut limits_active = false;
-         */
+        let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2
-        let mut limits_forcedirs = None;
         let mut limits_rhs = 0.0;
         let mut limits_impulse = 0.0;
         let mut limits_inv_lhs = 0.0;
+        let mut limits_impulse_limits = (0.0, 0.0);
 
         if joint.limits_enabled {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // TODO: we should allow both limits to be active at
+            // TODO: we should allow predictive constraint activation.
-            // the same time, and allow predictive constraint activation.
+
-            if dist < joint.limits[0] {
+            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
-                limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
+            let min_enabled = dist < min_limit;
-                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
+            let max_enabled = max_limit < dist;
-                limits_impulse = joint.limits_impulse;
+
-            } else if dist > joint.limits[1] {
+            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
-                limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
+            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
-                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
+
-                limits_impulse = joint.limits_impulse;
+            limits_active = min_enabled || max_enabled;
-            }
+            if limits_active {
+                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
+                    * params.velocity_solve_fraction;
+
+                limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
+                    * velocity_based_erp_inv_dt;
 
-            if dist < joint.limits[0] || dist > joint.limits[1] {
                 let gcross1 = r1.gcross(*axis1);
                 let gcross2 = r2.gcross(*axis2);
                 limits_inv_lhs = crate::utils::inv(
@@ -208,6 +243,11 @@ impl PrismaticVelocityConstraint {
                         + gcross1.gdot(ii1.transform_vector(gcross1))
                         + gcross2.gdot(ii2.transform_vector(gcross2)),
                 );
+
+                limits_impulse = joint
+                    .limits_impulse
+                    .max(limits_impulse_limits.0)
+                    .min(limits_impulse_limits.1);
             }
         }
 
@@ -220,10 +260,12 @@ impl PrismaticVelocityConstraint {
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: joint.impulse * params.warmstart_coeff,
+            limits_active,
             limits_impulse: limits_impulse * params.warmstart_coeff,
-            limits_forcedirs,
+            limits_forcedir2,
             limits_rhs,
             limits_inv_lhs,
+            limits_impulse_limits,
             motor_rhs,
             motor_inv_lhs,
             motor_impulse,
@@ -263,10 +305,9 @@ impl PrismaticVelocityConstraint {
         mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse);
 
         // Warmstart limits.
-        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+        if self.limits_active {
-            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
+            let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
-            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
+            let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
-
             mj_lambda1.linear += self.im1 * limit_impulse1;
             mj_lambda1.angular += self
                 .ii1_sqrt
@@ -313,14 +354,19 @@ impl PrismaticVelocityConstraint {
     }
 
     fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+        if self.limits_active {
+            let limits_forcedir1 = -self.limits_forcedir2;
+            let limits_forcedir2 = self.limits_forcedir2;
+
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
             let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
                 + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs).max(0.0);
+            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs)
+                .max(self.limits_impulse_limits.0)
+                .min(self.limits_impulse_limits.1);
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
@@ -396,8 +442,12 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
     #[cfg(feature = "dim3")]
     impulse: Vector5<Real>,
 
+    limits_active: bool,
+    limits_forcedir2: Vector<Real>,
     limits_impulse: Real,
     limits_rhs: Real,
+    /// min/max applied impulse due to limits
+    limits_impulse_limits: (Real, Real),
 
     axis2: Vector<Real>,
     motor_impulse: Real,
@@ -409,7 +459,6 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
     basis1: Vector2<Real>,
     #[cfg(feature = "dim3")]
     basis1: Matrix3x2<Real>,
-    limits_forcedir2: Option<Vector<Real>>,
 
     im2: Real,
     ii2_sqrt: AngularInertia<Real>,
@@ -520,13 +569,45 @@ impl PrismaticVelocityGroundConstraint {
         #[cfg(feature = "dim3")]
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let ang_rhs = rb2.angvel - rb1.angvel;
+        let angvel_err = rb2.angvel - rb1.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
         #[cfg(feature = "dim3")]
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            angvel_err.x,
+            angvel_err.y,
+            angvel_err.z,
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
+
+            let (frame1, frame2);
+            if flipped {
+                frame1 = rb1.position * joint.local_frame2();
+                frame2 = rb2.position * joint.local_frame1();
+            } else {
+                frame1 = rb1.position * joint.local_frame1();
+                frame2 = rb2.position * joint.local_frame2();
+            }
+
+            let ang_err = frame2.rotation * frame1.rotation.inverse();
+            #[cfg(feature = "dim2")]
+            {
+                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err = ang_err.scaled_axis();
+                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
+                    * velocity_based_erp_inv_dt;
+            }
+        }
 
         /*
          * Setup motor.
@@ -564,25 +645,37 @@ impl PrismaticVelocityGroundConstraint {
         /*
          * Setup limit constraint.
          */
-        let mut limits_forcedir2 = None;
+        let mut limits_active = false;
+        let limits_forcedir2 = axis2.into_inner();
         let mut limits_rhs = 0.0;
         let mut limits_impulse = 0.0;
+        let mut limits_impulse_limits = (0.0, 0.0);
 
         if joint.limits_enabled {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // TODO: we should allow both limits to be active at
+            // TODO: we should allow predictive constraint activation.
-            // the same time.
+
-            // TODO: allow predictive constraint activation.
+            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
-            if dist < joint.limits[0] {
+            let min_enabled = dist < min_limit;
-                limits_forcedir2 = Some(axis2.into_inner());
+            let max_enabled = max_limit < dist;
-                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
+
-                limits_impulse = joint.limits_impulse;
+            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
-            } else if dist > joint.limits[1] {
+            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
-                limits_forcedir2 = Some(-axis2.into_inner());
+
-                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
+            limits_active = min_enabled || max_enabled;
-                limits_impulse = joint.limits_impulse;
+            if limits_active {
+                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
+                    * params.velocity_solve_fraction;
+
+                limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0))
+                    * velocity_based_erp_inv_dt;
+
+                limits_impulse = joint
+                    .limits_impulse
+                    .max(limits_impulse_limits.0)
+                    .min(limits_impulse_limits.1);
             }
         }
 
@@ -592,7 +685,11 @@ impl PrismaticVelocityGroundConstraint {
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: joint.impulse * params.warmstart_coeff,
+            limits_active,
+            limits_forcedir2,
             limits_impulse: limits_impulse * params.warmstart_coeff,
+            limits_rhs,
+            limits_impulse_limits,
             motor_rhs,
             motor_inv_lhs,
             motor_impulse,
@@ -602,8 +699,6 @@ impl PrismaticVelocityGroundConstraint {
             rhs,
             r2,
             axis2: axis2.into_inner(),
-            limits_forcedir2,
-            limits_rhs,
         }
     }
 
@@ -625,9 +720,7 @@ impl PrismaticVelocityGroundConstraint {
         mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse);
 
         // Warmstart limits.
-        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+        mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse);
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
-        }
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
@@ -657,16 +750,20 @@ impl PrismaticVelocityGroundConstraint {
     }
 
     fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+        if self.limits_active {
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
-            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+            let lin_dvel = self
+                .limits_forcedir2
+                .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
                 + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0);
+            let new_impulse = (self.limits_impulse - lin_dvel / self.im2)
+                .max(self.limits_impulse_limits.0)
+                .min(self.limits_impulse_limits.1);
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+            mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
         }
     }
 

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -8,8 +8,10 @@ use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
+
 #[cfg(feature = "dim3")]
-use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5, U2, U3};
+
 #[cfg(feature = "dim2")]
 use {
     na::{Matrix2, Vector2},
@@ -18,6 +20,7 @@ use {
 
 #[cfg(feature = "dim2")]
 type LinImpulseDim = na::U1;
+
 #[cfg(feature = "dim3")]
 type LinImpulseDim = na::U2;
 
@@ -45,10 +48,12 @@ pub(crate) struct WPrismaticVelocityConstraint {
     #[cfg(feature = "dim2")]
     impulse: Vector2<SimdReal>,
 
+    limits_active: bool,
     limits_impulse: SimdReal,
-    limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
+    limits_forcedir2: Vector<SimdReal>,
     limits_rhs: SimdReal,
     limits_inv_lhs: SimdReal,
+    limits_impulse_limits: (SimdReal, SimdReal),
 
     #[cfg(feature = "dim2")]
     basis1: Vector2<SimdReal>,
@@ -180,46 +185,91 @@ impl WPrismaticVelocityConstraint {
         #[cfg(feature = "dim3")]
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let ang_rhs = angvel2 - angvel1;
+        let angvel_err = angvel2 - angvel1;
+
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction;
         #[cfg(feature = "dim3")]
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            angvel_err.x,
+            angvel_err.y,
+            angvel_err.z,
+        ) * velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
+
+            let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]);
+            let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]);
+
+            let frame1 = position1 * local_frame1;
+            let frame2 = position2 * local_frame2;
+            let ang_err = frame2.rotation * frame1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err =
+                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
+                    * velocity_based_erp_inv_dt;
+            }
+        }
+
+        // Setup limit constraint.
+        let zero: SimdReal = na::zero();
+        let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
+        let mut limits_active = false;
+        let mut limits_rhs = zero;
+        let mut limits_impulse = zero;
+        let mut limits_inv_lhs = zero;
+        let mut limits_impulse_limits = (zero, zero);
 
-        /*
-         * Setup limit constraint.
-         */
-        let mut limits_forcedirs = None;
-        let mut limits_rhs = na::zero();
-        let mut limits_impulse = na::zero();
-        let mut limits_inv_lhs = na::zero();
         let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
-
         if limits_enabled.any() {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // FIXME: we should allow both limits to be active at
+            // TODO: we should allow predictive constraint activation.
-            // the same time + allow predictive constraint activation.
+
             let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
             let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
-            let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
 
             let min_enabled = dist.simd_lt(min_limit);
             let max_enabled = dist.simd_gt(max_limit);
-            let _0: SimdReal = na::zero();
-            let _1: SimdReal = na::one();
-            let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
 
-            if sign != _0 {
+            limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
+            limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
+
+            limits_active = (min_enabled | max_enabled).any();
+            if limits_active {
                 let gcross1 = r1.gcross(axis1);
                 let gcross2 = r2.gcross(axis2);
 
-                limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
+                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
-                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
+                    * velocity_solve_fraction;
-                limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
+
+                limits_rhs += ((dist - max_limit).simd_max(zero)
+                    - (min_limit - dist).simd_max(zero))
+                    * SimdReal::splat(velocity_based_erp_inv_dt);
+
+                limits_impulse =
+                    SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
+                        .simd_max(limits_impulse_limits.0)
+                        .simd_min(limits_impulse_limits.1);
+
                 limits_inv_lhs = SimdReal::splat(1.0)
                     / (im1
                         + im2
@@ -236,11 +286,13 @@ impl WPrismaticVelocityConstraint {
             ii1_sqrt,
             im2,
             ii2_sqrt,
+            limits_active,
             impulse: impulse * SimdReal::splat(params.warmstart_coeff),
             limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
-            limits_forcedirs,
+            limits_forcedir2,
             limits_rhs,
             limits_inv_lhs,
+            limits_impulse_limits,
             basis1,
             inv_lhs,
             rhs,
@@ -284,9 +336,9 @@ impl WPrismaticVelocityConstraint {
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
         // Warmstart limits.
-        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+        if self.limits_active {
-            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
+            let limit_impulse1 = -self.limits_forcedir2 * self.limits_impulse;
-            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
+            let limit_impulse2 = self.limits_forcedir2 * self.limits_impulse;
 
             mj_lambda1.linear += limit_impulse1 * self.im1;
             mj_lambda1.angular += self
@@ -348,15 +400,19 @@ impl WPrismaticVelocityConstraint {
         mj_lambda1: &mut DeltaVel<SimdReal>,
         mj_lambda2: &mut DeltaVel<SimdReal>,
     ) {
-        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+        if self.limits_active {
+            let limits_forcedir1 = -self.limits_forcedir2;
+            let limits_forcedir2 = self.limits_forcedir2;
+
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
             let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
                 + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.limits_rhs;
-            let new_impulse =
+            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs)
-                (self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero());
+                .simd_max(self.limits_impulse_limits.0)
+                .simd_min(self.limits_impulse_limits.1);
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
@@ -434,15 +490,17 @@ pub(crate) struct WPrismaticVelocityGroundConstraint {
     #[cfg(feature = "dim3")]
     impulse: Vector5<SimdReal>,
 
+    limits_active: bool,
+    limits_forcedir2: Vector<SimdReal>,
     limits_impulse: SimdReal,
     limits_rhs: SimdReal,
+    limits_impulse_limits: (SimdReal, SimdReal),
 
     axis2: Vector<SimdReal>,
     #[cfg(feature = "dim2")]
     basis1: Vector2<SimdReal>,
     #[cfg(feature = "dim3")]
     basis1: Matrix3x2<SimdReal>,
-    limits_forcedir2: Option<Vector<SimdReal>>,
 
     im2: SimdReal,
     ii2_sqrt: AngularInertia<SimdReal>,
@@ -553,40 +611,89 @@ impl WPrismaticVelocityGroundConstraint {
         #[cfg(feature = "dim3")]
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
-        let ang_rhs = angvel2 - angvel1;
+        let angvel_err = angvel2 - angvel1;
+
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        let mut rhs = Vector2::new(linvel_err.x, angvel_err) * velocity_solve_fraction;
         #[cfg(feature = "dim3")]
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            angvel_err.x,
+            angvel_err.y,
+            angvel_err.z,
+        ) * velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
+
+            let frame1 = position1
+                * Isometry::from(
+                    array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH],
+                );
+            let frame2 = position2
+                * Isometry::from(
+                    array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH],
+                );
+
+            let ang_err = frame2.rotation * frame1.rotation.inverse();
+
+            #[cfg(feature = "dim2")]
+            {
+                rhs += Vector2::new(linear_err.x, ang_err.angle()) * velocity_based_erp_inv_dt;
+            }
+
+            #[cfg(feature = "dim3")]
+            {
+                let ang_err =
+                    Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
+                rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
+                    * velocity_based_erp_inv_dt;
+            }
+        }
 
         // Setup limit constraint.
-        let mut limits_forcedir2 = None;
+        let zero: SimdReal = na::zero();
-        let mut limits_rhs = na::zero();
+        let limits_forcedir2 = axis2; // hopefully axis1 is colinear with axis2
-        let mut limits_impulse = na::zero();
+        let mut limits_active = false;
-        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+        let mut limits_rhs = zero;
+        let mut limits_impulse = zero;
+        let mut limits_impulse_limits = (zero, zero);
 
+        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
         if limits_enabled.any() {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // FIXME: we should allow both limits to be active at
+            // TODO: we should allow predictive constraint activation.
-            // the same time + allow predictive constraint activation.
             let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
             let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
-            let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
 
-            let use_min = dist.simd_lt(min_limit);
+            let min_enabled = dist.simd_lt(min_limit);
-            let use_max = dist.simd_gt(max_limit);
+            let max_enabled = dist.simd_gt(max_limit);
-            let _0: SimdReal = na::zero();
-            let _1: SimdReal = na::one();
-            let sign = _1.select(use_min, (-_1).select(use_max, _0));
 
-            if sign != _0 {
+            limits_impulse_limits.0 = SimdReal::splat(-Real::INFINITY).select(max_enabled, zero);
-                limits_forcedir2 = Some(axis2 * sign);
+            limits_impulse_limits.1 = SimdReal::splat(Real::INFINITY).select(min_enabled, zero);
-                limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign;
+
-                limits_impulse = lim_impulse.select(use_min | use_max, _0);
+            limits_active = (min_enabled | max_enabled).any();
+            if limits_active {
+                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1))
+                    * velocity_solve_fraction;
+
+                limits_rhs += ((dist - max_limit).simd_max(zero)
+                    - (min_limit - dist).simd_max(zero))
+                    * SimdReal::splat(velocity_based_erp_inv_dt);
+
+                limits_impulse =
+                    SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
+                        .simd_max(limits_impulse_limits.0)
+                        .simd_min(limits_impulse_limits.1);
             }
         }
 
@@ -596,14 +703,16 @@ impl WPrismaticVelocityGroundConstraint {
             im2,
             ii2_sqrt,
             impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            limits_active,
+            limits_forcedir2,
+            limits_rhs,
             limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
+            limits_impulse_limits,
             basis1,
             inv_lhs,
             rhs,
             r2,
             axis2,
-            limits_forcedir2,
-            limits_rhs,
         }
     }
 
@@ -628,9 +737,7 @@ impl WPrismaticVelocityGroundConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
-        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+        mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * self.limits_impulse);
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
-        }
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -663,18 +770,22 @@ impl WPrismaticVelocityGroundConstraint {
     }
 
     fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+        if self.limits_active {
             // FIXME: the transformation by ii2_sqrt could be avoided by
             // reusing some computations above.
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
-            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+            let lin_dvel = self
+                .limits_forcedir2
+                .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
                 + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero());
+            let new_impulse = (self.limits_impulse - lin_dvel / self.im2)
+                .simd_max(self.limits_impulse_limits.0)
+                .simd_min(self.limits_impulse_limits.1);
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+            mj_lambda2.linear += self.limits_forcedir2 * (self.im2 * dimpulse);
         }
     }
 

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -3,9 +3,8 @@ use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
 use crate::math::{AngularInertia, Real, Rotation, Vector};
-use crate::na::UnitQuaternion;
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+use na::{Cholesky, Matrix3x2, Matrix5, UnitQuaternion, Vector5, U2, U3};
 
 #[derive(Debug)]
 pub(crate) struct RevoluteVelocityConstraint {
@@ -90,9 +89,31 @@ impl RevoluteVelocityConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
+        let linvel_err =
-        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
+            (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+        let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
+
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            linvel_err.z,
+            angvel_err.x,
+            angvel_err.y,
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let lin_err = anchor2 - anchor1;
+
+            let axis1 = rb1.position * joint.local_axis1;
+            let axis2 = rb2.position * joint.local_axis2;
+
+            let axis_error = axis1.cross(&axis2);
+            let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
+
+            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
+                * velocity_based_erp_inv_dt;
+        }
 
         /*
          * Motor.
@@ -371,9 +392,35 @@ impl RevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
+        let linvel_err =
-        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
+            (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+        let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            linvel_err.z,
+            angvel_err.x,
+            angvel_err.y,
+        ) * params.velocity_solve_fraction;
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let lin_err = anchor2 - anchor1;
+
+            let (axis1, axis2);
+            if flipped {
+                axis1 = rb1.position * joint.local_axis2;
+                axis2 = rb2.position * joint.local_axis1;
+            } else {
+                axis1 = rb1.position * joint.local_axis1;
+                axis2 = rb2.position * joint.local_axis2;
+            }
+            let axis_error = axis1.cross(&axis2);
+            let ang_err = basis2.tr_mul(&axis_error);
+
+            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
+                * velocity_based_erp_inv_dt;
+        }
 
         /*
          * Motor part.
@@ -472,6 +519,7 @@ impl RevoluteVelocityGroundConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
     }
+
     fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
         if self.motor_inv_lhs != 0.0 {
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -8,7 +8,7 @@ use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5, U2, U3};
 
 #[derive(Debug)]
 pub(crate) struct WRevoluteVelocityConstraint {
@@ -107,9 +107,38 @@ impl WRevoluteVelocityConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let linvel_err = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
+        let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            linvel_err.z,
+            angvel_err.x,
+            angvel_err.y,
+        ) * SimdReal::splat(params.velocity_solve_fraction);
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            let lin_err = anchor2 - anchor1;
+
+            let local_axis1 =
+                Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
+            let local_axis2 =
+                Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]);
+
+            let axis1 = position1 * local_axis1;
+            let axis2 = position2 * local_axis2;
+
+            let axis_error = axis1.cross(&axis2);
+            let ang_err =
+                (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * SimdReal::splat(0.5);
+
+            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
+                * velocity_based_erp_inv_dt;
+        }
 
         /*
          * Adjust the warmstart impulse.
@@ -357,9 +386,38 @@ impl WRevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
+        let linvel_err = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
-        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
+        let angvel_err = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            linvel_err.z,
+            angvel_err.x,
+            angvel_err.y,
+        ) * SimdReal::splat(params.velocity_solve_fraction);
+
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+        if velocity_based_erp_inv_dt != 0.0 {
+            let velocity_based_erp_inv_dt = SimdReal::splat(velocity_based_erp_inv_dt);
+
+            let lin_err = anchor2 - anchor1;
+
+            let local_axis1 = Unit::<Vector<_>>::from(
+                array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
+            );
+            let local_axis2 = Unit::<Vector<_>>::from(
+                array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH],
+            );
+            let axis1 = position1 * local_axis1;
+            let axis2 = position2 * local_axis2;
+
+            let axis_error = axis1.cross(&axis2);
+            let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error);
+
+            rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
+                * velocity_based_erp_inv_dt;
+        }
 
         WRevoluteVelocityGroundConstraint {
             joint_id,

src/dynamics/solver/velocity_constraint.rs

@@ -147,6 +147,8 @@ impl VelocityConstraint {
         assert_eq!(manifold.data.relative_dominance, 0);
 
         let inv_dt = params.inv_dt();
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+
         let rb1 = &bodies[manifold.data.body_pair.body1];
         let rb2 = &bodies[manifold.data.body_pair.body2];
         let mj_lambda1 = rb1.active_set_offset;
@@ -244,17 +246,19 @@ impl VelocityConstraint {
                             + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                    let is_resting = 1.0 - is_bouncy;
-                        * (vel1 - vel2).dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * inv_dt;
 
-                    let impulse = manifold_point.data.impulse * warmstart_coeff;
+                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1);
+                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
 
                     constraint.elements[k].normal_part = VelocityConstraintElementPart {
                         gcross1,
                         gcross2,
                         rhs,
-                        impulse,
+                        impulse: manifold_point.data.impulse * warmstart_coeff,
                         r,
                     };
                 }

src/dynamics/solver/velocity_constraint_wide.rs

@@ -72,6 +72,9 @@ impl WVelocityConstraint {
         }
 
         let inv_dt = SimdReal::splat(params.inv_dt());
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
+        let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
+
         let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
         let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
 
@@ -132,6 +135,7 @@ impl WVelocityConstraint {
                 let is_bouncy = SimdReal::from(
                     array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
                 );
+                let is_resting = SimdReal::splat(1.0) - is_bouncy;
                 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
                 let tangent_velocity =
@@ -158,8 +162,12 @@ impl WVelocityConstraint {
                     let r = SimdReal::splat(1.0)
                         / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
-                    let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity
+                    let mut rhs =
-                        + dist.simd_max(SimdReal::zero()) * inv_dt;
+                        (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
+                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+                    rhs *= is_bouncy + is_resting * velocity_solve_fraction;
+                    rhs +=
+                        dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
 
                     constraint.elements[k].normal_part = WVelocityConstraintElementPart {
                         gcross1,

src/dynamics/solver/velocity_ground_constraint.rs

@@ -64,6 +64,8 @@ impl VelocityGroundConstraint {
         push: bool,
     ) {
         let inv_dt = params.inv_dt();
+        let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
+
         let mut rb1 = &bodies[manifold.data.body_pair.body1];
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
         let flipped = manifold.data.relative_dominance < 0;
@@ -156,16 +158,18 @@ impl VelocityGroundConstraint {
                     let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                    let is_resting = 1.0 - is_bouncy;
-                        * (vel1 - vel2).dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * inv_dt;
 
-                    let impulse = manifold_point.data.impulse * warmstart_coeff;
+                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1);
+                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
                         gcross2,
                         rhs,
-                        impulse,
+                        impulse: manifold_point.data.impulse * warmstart_coeff,
                         r,
                     };
                 }

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -64,6 +64,9 @@ impl WVelocityGroundConstraint {
         push: bool,
     ) {
         let inv_dt = SimdReal::splat(params.inv_dt());
+        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
+        let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
+
         let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
         let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
         let mut flipped = [1.0; SIMD_WIDTH];
@@ -124,6 +127,7 @@ impl WVelocityGroundConstraint {
                 let is_bouncy = SimdReal::from(
                     array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
                 );
+                let is_resting = SimdReal::splat(1.0) - is_bouncy;
                 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
                 let tangent_velocity =
@@ -147,8 +151,12 @@ impl WVelocityGroundConstraint {
 
                     let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
-                    let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity
+                    let mut rhs =
-                        + dist.simd_max(SimdReal::zero()) * inv_dt;
+                        (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
+                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+                    rhs *= is_bouncy + is_resting * velocity_solve_fraction;
+                    rhs +=
+                        dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
 
                     constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
                         gcross2,