Implemented prismatic narrow. Needs testing and close review

1 files changed, 83 insertions, 8 deletions

diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
index ae3f3e2..9f19ace 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
@@ -135,13 +135,47 @@ 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 dpos = anchor2 - anchor1;
+            let limit_err = dpos.dot(&axis1);
+            let mut linear_err = dpos - *axis1 * limit_err;
+
+            let frame1 = rb1.position * cparams.local_frame1();
+            let frame2 = rb2.position * cparams.local_frame2();
+            let ang_err = frame2.rotation * frame1.rotation.inverse();
+
+            if limit_err < cparams.limits[0] {
+                linear_err += *axis1 * (limit_err - cparams.limits[0]);
+            } else if limit_err > cparams.limits[1] {
+                linear_err += *axis1 * (limit_err - cparams.limits[1]);
+            }
+
+            #[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.
@@ -520,13 +554,54 @@ 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 (frame1, frame2);
+            if flipped {
+                frame1 = rb1.position * cparams.local_frame2();
+                frame2 = rb2.position * cparams.local_frame1();
+            } else {
+                frame1 = rb1.position * cparams.local_frame1();
+                frame2 = rb2.position * cparams.local_frame2();
+            }
+
+            let dpos = anchor2 - anchor1;
+            let limit_err = dpos.dot(&axis1);
+            let mut linear_err = dpos - *axis1 * limit_err;
+
+            let ang_err = frame2.rotation * frame1.rotation.inverse();
+
+            if limit_err < cparams.limits[0] {
+                linear_err += *axis1 * (limit_err - cparams.limits[0]);
+            } else if limit_err > cparams.limits[1] {
+                linear_err += *axis1 * (limit_err - cparams.limits[1]);
+            }
+
+            #[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.