Implement revolute narrow

2 files changed, 70 insertions, 22 deletions

diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
index 9f19ace..440b8ac 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
@@ -155,14 +155,14 @@ impl PrismaticVelocityConstraint {
             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 frame1 = rb1.position * joint.local_frame1();
+            let frame2 = rb2.position * joint.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]);
+            if limit_err < joint.limits[0] {
+                linear_err += *axis1 * (limit_err - joint.limits[0]);
+            } else if limit_err > joint.limits[1] {
+                linear_err += *axis1 * (limit_err - joint.limits[1]);
             }
 
             #[cfg(feature = "dim2")]
@@ -572,11 +572,11 @@ impl PrismaticVelocityGroundConstraint {
         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();
+                frame1 = rb1.position * joint.local_frame2();
+                frame2 = rb2.position * joint.local_frame1();
             } else {
-                frame1 = rb1.position * cparams.local_frame1();
-                frame2 = rb2.position * cparams.local_frame2();
+                frame1 = rb1.position * joint.local_frame1();
+                frame2 = rb2.position * joint.local_frame2();
             }
 
             let dpos = anchor2 - anchor1;
@@ -585,10 +585,10 @@ impl PrismaticVelocityGroundConstraint {
 
             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]);
+            if limit_err < joint.limits[0] {
+                linear_err += *axis1 * (limit_err - joint.limits[0]);
+            } else if limit_err > joint.limits[1] {
+                linear_err += *axis1 * (limit_err - joint.limits[1]);
             }
 
             #[cfg(feature = "dim2")]
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
index 61cb720..46f375b 100644
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
+++ b/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 ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+        let linvel_err =
+            (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
+        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 ang_err =
+                Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
+            let ang_err = ang_err.scaled_axis();
+
+            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,36 @@ 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 ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
-        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+        let linvel_err =
+            (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
+        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 ang_err =
+                Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
+            let ang_err = ang_err.scaled_axis();
+
+            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.