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

Optional violation correction in velocity solver

13 files changed, 643 insertions, 182 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index caad9b5..8c0f26c 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/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,
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index a110fbb..508ac65 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/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 anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com;
+        let anchor_world1 = rb1.position * joint.local_anchor1;
+        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 lhs;
+        let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+            + (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,
-                rb2.position * joint.local_anchor1 - rb2.world_com,
+                rb1.position * joint.local_anchor2,
+                rb2.position * joint.local_anchor1,
             )
         } else {
             (
-                rb1.position * joint.local_anchor1 - rb1.world_com,
-                rb2.position * joint.local_anchor2 - rb2.world_com,
+                rb1.position * joint.local_anchor1,
+                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();
 
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
index 95b0bb5..ff5e001 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
+++ b/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 anchor2 = position2 * local_anchor2 - world_com2;
+        let anchor_world1 = position1 * local_anchor1;
+        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 anchor2 = position2 * local_anchor2 - world_com2;
+        let anchor_world1 = position1 * local_anchor1;
+        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();
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
index 2868d4b..5782d86 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/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,
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
index c423272..6b90343 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ b/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,
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
index ae3f3e2..9445896 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ b/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 ang_rhs = rb2.angvel - rb1.angvel;
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        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.
-         */
-        let mut limits_forcedirs = None;
+        // Setup limit constraint.
+        let mut limits_active = false;
+        let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2
         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
-            // the same time, and allow predictive constraint activation.
-            if dist < joint.limits[0] {
-                limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
-                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = joint.limits_impulse;
-            } else if dist > joint.limits[1] {
-                limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
-                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
-                limits_impulse = joint.limits_impulse;
-            }
+            // TODO: we should allow predictive constraint activation.
+
+            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
+            let min_enabled = dist < min_limit;
+            let max_enabled = max_limit < dist;
+
+            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
+            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
+
+            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 {
-            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
-            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
-
+        if self.limits_active {
+            let limit_impulse1 = -self.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 ang_rhs = rb2.angvel - rb1.angvel;
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        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
-            // the same time.
-            // TODO: allow predictive constraint activation.
-            if dist < joint.limits[0] {
-                limits_forcedir2 = Some(axis2.into_inner());
-                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = joint.limits_impulse;
-            } else if dist > joint.limits[1] {
-                limits_forcedir2 = Some(-axis2.into_inner());
-                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
-                limits_impulse = joint.limits_impulse;
+            // TODO: we should allow predictive constraint activation.
+
+            let (min_limit, max_limit) = (joint.limits[0], joint.limits[1]);
+            let min_enabled = dist < min_limit;
+            let max_enabled = max_limit < dist;
+
+            limits_impulse_limits.0 = if max_enabled { -Real::INFINITY } else { 0.0 };
+            limits_impulse_limits.1 = if min_enabled { Real::INFINITY } else { 0.0 };
+
+            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;
+
+                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 += limits_forcedir2 * (self.im2 * self.limits_impulse);
-        }
+        mj_lambda2.linear += self.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);
         }
     }
 
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
index 442393d..f21acee 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
+++ b/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 ang_rhs = angvel2 - angvel1;
+        let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        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);
-
-        /*
-         * 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]);
+        let mut rhs = Vector5::new(
+            linvel_err.x,
+            linvel_err.y,
+            angvel_err.x,
+            angvel_err.y,
+            angvel_err.z,
+        ) * velocity_solve_fraction;
+