fix: implement linear-coupled-motor constraint between two dynamic bodies

Fix #602

3 files changed, 98 insertions, 22 deletions

diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
index b5fdb6d..4d88949 100644
--- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
+++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
@@ -25,6 +25,7 @@ use {
     crate::math::SIMD_WIDTH,
 };
 
+#[derive(Debug)]
 pub struct ConstraintsCounts {
     pub num_constraints: usize,
     pub num_jacobian_lines: usize,
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
index 00bead1..44716b2 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
@@ -551,6 +551,82 @@ impl<N: SimdRealCopy> JointTwoBodyConstraintHelper<N> {
         constraint
     }
 
+    pub fn motor_linear_coupled<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &JointSolverBody<N, LANES>,
+        body2: &JointSolverBody<N, LANES>,
+        coupled_axes: u8,
+        motor_params: &MotorParameters<N>,
+        limits: Option<[N; 2]>,
+        writeback_id: WritebackId,
+    ) -> JointTwoBodyConstraint<N, LANES> {
+        let inv_dt = N::splat(params.inv_dt());
+
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+
+        for i in 0..DIM {
+            if coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+            }
+        }
+
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let mut rhs_wo_bias = N::zero();
+        if motor_params.erp_inv_dt != N::zero() {
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
+        }
+
+        let mut target_vel = motor_params.target_vel;
+        if let Some(limits) = limits {
+            target_vel =
+                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
+        };
+
+        rhs_wo_bias += -target_vel;
+
+        ang_jac1 = body1.sqrt_ii * ang_jac1;
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        JointTwoBodyConstraint {
+            joint_id,
+            solver_vel1: body1.solver_vel,
+            solver_vel2: body2.solver_vel,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
+            rhs: rhs_wo_bias,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
     pub fn lock_linear<const LANES: usize>(
         &self,
         params: &IntegrationParameters,
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
index e184e3d..60c42d3 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
@@ -217,28 +217,26 @@ impl JointTwoBodyConstraint<Real, 1> {
         }
 
         if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
-            // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 {
-            //     let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 {
-            //         Some([
-            //             joint.limits[first_coupled_lin_axis_id].min,
-            //             joint.limits[first_coupled_lin_axis_id].max,
-            //         ])
-            //     } else {
-            //         None
-            //     };
-            //
-            //     out[len] = builder.motor_linear_coupled
-            //         params,
-            //         [joint_id],
-            //         body1,
-            //         body2,
-            //         coupled_axes,
-            //         &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
-            //         limits,
-            //         WritebackId::Motor(first_coupled_lin_axis_id),
-            //     );
-            //     len += 1;
-            // }
+            let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
+                Some([
+                    joint.limits[first_coupled_lin_axis_id].min,
+                    joint.limits[first_coupled_lin_axis_id].max,
+                ])
+            } else {
+                None
+            };
+
+            out[len] = builder.motor_linear_coupled(
+                params,
+                [joint_id],
+                body1,
+                body2,
+                coupled_axes,
+                &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
+                limits,
+                WritebackId::Motor(first_coupled_lin_axis_id),
+            );
+            len += 1;
         }
 
         JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
@@ -350,6 +348,7 @@ impl JointTwoBodyConstraint<Real, 1> {
         }
     }
 }
+
 #[cfg(feature = "simd-is-enabled")]
 impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
     pub fn lock_axes(