feat: implement new "small-steps" solver + joint improvements

1 files changed, 16 insertions, 14 deletions

diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs
index da650e6..62fc434 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::solver::AnyJointVelocityConstraint;
+use crate::dynamics::solver::JointGenericOneBodyConstraint;
 use crate::dynamics::{
     joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
     RigidBodyVelocity,
@@ -254,15 +254,15 @@ impl MultibodyJoint {
         params: &IntegrationParameters,
         multibody: &Multibody,
         link: &MultibodyLink,
-        dof_id: usize,
-        j_id: &mut usize,
+        mut j_id: usize,
         jacobians: &mut DVector<Real>,
-        constraints: &mut Vec<AnyJointVelocityConstraint>,
-        insert_at: &mut Option<usize>,
-    ) {
+        constraints: &mut [JointGenericOneBodyConstraint],
+    ) -> usize {
+        let j_id = &mut j_id;
         let locked_bits = self.data.locked_axes.bits();
         let limit_bits = self.data.limit_axes.bits();
         let motor_bits = self.data.motor_axes.bits();
+        let mut num_constraints = 0;
         let mut curr_free_dof = 0;
 
         for i in 0..DIM {
@@ -281,11 +281,11 @@ impl MultibodyJoint {
                         &self.data.motors[i],
                         self.coords[i],
                         limits,
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                 }
 
@@ -296,11 +296,11 @@ impl MultibodyJoint {
                         link,
                         [self.data.limits[i].min, self.data.limits[i].max],
                         self.coords[i],
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                 }
                 curr_free_dof += 1;
@@ -331,11 +331,11 @@ impl MultibodyJoint {
                         link,
                         limits,
                         self.coords[i],
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                     Some(limits)
                 } else {
@@ -350,15 +350,17 @@ impl MultibodyJoint {
                         &self.data.motors[i],
                         self.coords[i],
                         limits,
-                        dof_id + curr_free_dof,
+                        curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
-                        insert_at,
+                        &mut num_constraints,
                     );
                 }
                 curr_free_dof += 1;
             }
         }
+
+        num_constraints
     }
 }