More experiments with the way the generic joint is stabilized.

1 files changed, 17 insertions, 0 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index 12b8f77..78332e8 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -333,6 +333,23 @@ impl AnyJointVelocityConstraint {
         }
     }
 
+    pub fn solve2(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        mj_lambdas_pos: &mut [DeltaVel<Real>],
+    ) {
+        match self {
+            AnyJointVelocityConstraint::GenericConstraint(c) => {
+                c.solve2(mj_lambdas, mj_lambdas_pos)
+            }
+            AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
+                c.solve2(mj_lambdas, mj_lambdas_pos)
+            }
+            AnyJointVelocityConstraint::Empty => unreachable!(),
+            _ => {}
+        }
+    }
+
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         match self {
             AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all),