Merge branch 'master' into collider-builder-debug

3 files changed, 105 insertions, 38 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
index 00bead1..40613c1 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
@@ -198,7 +198,7 @@ impl JointOneBodyConstraintBuilder {
 
         if flipped {
             std::mem::swap(&mut handle1, &mut handle2);
-            std::mem::swap(&mut joint_data.local_frame1, &mut joint_data.local_frame2);
+            joint_data.flip();
         };
 
         let rb1 = &bodies[handle1];
@@ -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_generic_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
index 34256a2..9876195 100644
--- a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
@@ -319,7 +319,6 @@ pub struct JointGenericVelocityOneBodyExternalConstraintBuilder {
     joint_id: JointIndex,
     joint: GenericJoint,
     j_id: usize,
-    flipped: bool,
     constraint_id: usize,
     // These are solver body for both joints, except that
     // the world_com is actually in local-space.
@@ -337,21 +336,20 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
         jacobians: &mut DVector<Real>,
         out_constraint_id: &mut usize,
     ) {
+        let mut joint_data = joint.data;
         let mut handle1 = joint.body1;
         let mut handle2 = joint.body2;
         let flipped = !bodies[handle2].is_dynamic();
 
-        let local_frame1 = if flipped {
+        if flipped {
             std::mem::swap(&mut handle1, &mut handle2);
-            joint.data.local_frame2
-        } else {
-            joint.data.local_frame1
-        };
+            joint_data.flip();
+        }
 
         let rb1 = &bodies[handle1];
         let rb2 = &bodies[handle2];
 
-        let frame1 = rb1.pos.position * local_frame1;
+        let frame1 = rb1.pos.position * joint_data.local_frame1;
 
         let starting_j_id = *j_id;
 
@@ -394,11 +392,10 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
             body1,
             link2,
             joint_id,
-            joint: joint.data,
+            joint: joint_data,
             j_id: starting_j_id,
             frame1,
             local_body2,
-            flipped,
             constraint_id: *out_constraint_id,
         });
 
@@ -417,12 +414,7 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
         //       constraints. Could we make this more incremental?
         let mb2 = &multibodies[self.link2.multibody];
         let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world;
-        let frame2 = pos2
-            * if self.flipped {
-                self.joint.local_frame1
-            } else {
-                self.joint.local_frame2
-            };
+        let frame2 = pos2 * self.joint.local_frame2;
 
         let joint_body2 = JointSolverBody {
             world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.
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(