More experiments with the way the generic joint is stabilized.

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -14,6 +14,7 @@ use super::{
     WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
     WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
 };
+use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
@@ -241,4 +242,20 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::Empty => unreachable!(),
         }
     }
+
+    pub fn solve2(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<Real>],
+        dpos: &mut [DeltaVel<Real>],
+    ) {
+        match self {
+            AnyJointPositionConstraint::GenericJoint(c) => c.solve2(params, positions, dpos),
+            AnyJointPositionConstraint::GenericGroundConstraint(c) => {
+                c.solve2(params, positions, dpos)
+            }
+            _ => {}
+            AnyJointPositionConstraint::Empty => unreachable!(),
+        }
+    }
 }