Fix lever-arm handling in the revolute joint.

src/dynamics/joint/revolute_joint.rs

@@ -72,7 +72,7 @@ impl RevoluteJoint {
             motor_max_impulse: Real::MAX,
             motor_impulse: 0.0,
             prev_axis1: *local_axis1,
-            motor_model: SpringModel::VelocityBased,
+            motor_model: SpringModel::default(),
             motor_last_angle: 0.0,
         }
     }

src/dynamics/joint/spring_model.rs

@@ -21,6 +21,12 @@ pub enum SpringModel {
     ForceBased,
 }
 
+impl Default for SpringModel {
+    fn default() -> Self {
+        SpringModel::VelocityBased
+    }
+}
+
 impl SpringModel {
     /// Combines the coefficients used for solving the spring equation.
     ///

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -15,8 +15,8 @@ pub(crate) struct RevoluteVelocityConstraint {
 
     joint_id: JointIndex,
 
-    r1_mat: Matrix3<Real>,
+    r1: Vector<Real>,
-    r2_mat: Matrix3<Real>,
+    r2: Vector<Real>,
 
     inv_lhs: Matrix5<Real>,
     rhs: Vector5<Real>,
@@ -56,13 +56,12 @@ impl RevoluteVelocityConstraint {
             rb1.position * joint.basis1[0],
             rb1.position * joint.basis1[1],
         ]);
-        let basis_projection1 = basis1 * basis1.transpose();
 
-        let basis_projection_half2 = Matrix3x2::from_columns(&[
+        let basis2 = Matrix3x2::from_columns(&[
             rb2.position * joint.basis2[0],
             rb2.position * joint.basis2[1],
         ]);
-        let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose();
+        let basis_projection2 = basis2 * basis2.transpose();
         let basis2 = basis_projection2 * basis1;
 
         let im1 = rb1.effective_inv_mass;
@@ -70,13 +69,14 @@ impl RevoluteVelocityConstraint {
 
         let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
-        let r1_mat = basis_projection1 * r1.gcross_matrix();
+        let r1_mat = r1.gcross_matrix();
 
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r2 = anchor2 - rb2.world_com;
-        let r2_mat = basis_projection2 * r2.gcross_matrix();
+        let r2_mat = r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
+
         let lhs00 =
             ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
         let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat));
@@ -91,7 +91,7 @@ impl RevoluteVelocityConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = (rb2.linvel - r2_mat * rb2.angvel) - (rb1.linvel - r1_mat * rb1.angvel);
+        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
         let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
         let mut rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
@@ -162,8 +162,8 @@ impl RevoluteVelocityConstraint {
             impulse,
             inv_lhs,
             rhs,
-            r1_mat,
+            r1,
-            r2_mat,
+            r2,
             motor_rhs,
             motor_inv_lhs,
             motor_max_impulse,
@@ -186,12 +186,12 @@ impl RevoluteVelocityConstraint {
         mj_lambda1.linear += self.im1 * lin_impulse1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1);
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
         mj_lambda2.linear -= self.im2 * lin_impulse2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2);
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         /*
          * Motor
@@ -216,8 +216,8 @@ impl RevoluteVelocityConstraint {
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
-        let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2)
+        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
-            - (mj_lambda1.linear - self.r1_mat * ang_vel1);
+            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
         let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
@@ -231,12 +231,12 @@ impl RevoluteVelocityConstraint {
         mj_lambda1.linear += self.im1 * lin_impulse1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1);
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
         mj_lambda2.linear -= self.im2 * lin_impulse2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2);
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         /*
          * Motor.
@@ -313,24 +313,41 @@ impl RevoluteVelocityGroundConstraint {
     ) -> AnyJointVelocityConstraint {
         let anchor2;
         let anchor1;
+        let axis1;
+        let axis2;
+        let basis1;
         let basis2;
 
         if flipped {
+            axis1 = rb1.position * *joint.local_axis2;
+            axis2 = rb2.position * *joint.local_axis1;
             anchor1 = rb1.position * joint.local_anchor2;
             anchor2 = rb2.position * joint.local_anchor1;
+            basis1 = Matrix3x2::from_columns(&[
+                rb1.position * joint.basis2[0],
+                rb1.position * joint.basis2[1],
+            ]);
             basis2 = Matrix3x2::from_columns(&[
                 rb2.position * joint.basis1[0],
                 rb2.position * joint.basis1[1],
             ]);
         } else {
+            axis1 = rb1.position * *joint.local_axis1;
+            axis2 = rb2.position * *joint.local_axis2;
             anchor1 = rb1.position * joint.local_anchor1;
             anchor2 = rb2.position * joint.local_anchor2;
+            basis1 = Matrix3x2::from_columns(&[
+                rb1.position * joint.basis1[0],
+                rb1.position * joint.basis1[1],
+            ]);
             basis2 = Matrix3x2::from_columns(&[
                 rb2.position * joint.basis2[0],
                 rb2.position * joint.basis2[1],
             ]);
         };
 
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
         let im2 = rb2.effective_inv_mass;
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
@@ -351,15 +368,13 @@ impl RevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let ang_rhs = basis2.tr_mul(&(rb2.angvel - rb1.angvel));
+        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
         /*
          * Motor part.
          */
-        let motor_axis1 = rb1.position * *joint.local_axis1;
-        let motor_axis2 = rb2.position * *joint.local_axis2;
         let mut motor_rhs = 0.0;
         let mut motor_inv_lhs = 0.0;
         let mut motor_max_impulse = joint.motor_max_impulse;
@@ -377,13 +392,13 @@ impl RevoluteVelocityGroundConstraint {
         }
 
         if damping != 0.0 {
-            let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
+            let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
             motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
         }
 
         if stiffness != 0.0 || damping != 0.0 {
             motor_inv_lhs = if keep_lhs {
-                crate::utils::inv(motor_axis2.dot(&ii2.transform_vector(motor_axis2))) * gamma
+                crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2))) * gamma
             } else {
                 gamma
             };
@@ -405,7 +420,7 @@ impl RevoluteVelocityGroundConstraint {
             r2,
             motor_inv_lhs,
             motor_impulse,
-            motor_axis2,
+            motor_axis2: axis2,
             motor_max_impulse,
             motor_rhs,
             motor_angle,
@@ -441,7 +456,6 @@ impl RevoluteVelocityGroundConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let ang_vel2 = ang_vel2 - self.motor_axis2 * ang_vel2.dot(&self.motor_axis2);
 
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let ang_dvel = self.basis2.tr_mul(&ang_vel2);

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -17,8 +17,8 @@ pub(crate) struct WRevoluteVelocityConstraint {
 
     joint_id: [JointIndex; SIMD_WIDTH],
 
-    r1_mat: Matrix3<SimdReal>,
+    r1: Vector<SimdReal>,
-    r2_mat: Matrix3<SimdReal>,
+    r2: Vector<SimdReal>,
 
     inv_lhs: Matrix5<SimdReal>,
     rhs: Vector5<SimdReal>,
@@ -79,26 +79,18 @@ impl WRevoluteVelocityConstraint {
         let anchor2 = position2 * local_anchor2;
         let basis1 =
             Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
-        let basis_projection1 = basis1 * basis1.transpose();
+        let basis2 =
-        let basis_projection_half2 =
             Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]);
-        let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose();
+        let basis_projection2 = basis2 * basis2.transpose();
         let basis2 = basis_projection2 * basis1;
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = r21 * basis1;
-        // NOTE: to simplify, we use basis2 = basis1.
-        // Though we may want to test if that does not introduce any instability.
         let ii1 = ii1_sqrt.squared();
         let r1 = anchor1 - world_com1;
-        let r1_mat = basis_projection1 * r1.gcross_matrix();
+        let r1_mat = r1.gcross_matrix();
 
         let ii2 = ii2_sqrt.squared();
         let r2 = anchor2 - world_com2;
-        let r2_mat = basis_projection2 * r2.gcross_matrix();
+        let r2_mat = r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
         let lhs00 =
@@ -154,8 +146,8 @@ impl WRevoluteVelocityConstraint {
             impulse,
             inv_lhs,
             rhs,
-            r1_mat,
+            r1,
-            r2_mat,
+            r2,
         }
     }
 
@@ -185,12 +177,12 @@ impl WRevoluteVelocityConstraint {
         mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1);
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
         mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2);
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -223,8 +215,8 @@ impl WRevoluteVelocityConstraint {
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
-        let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2)
+        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
-            - (mj_lambda1.linear - self.r1_mat * ang_vel1);
+            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
         let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
@@ -238,12 +230,12 @@ impl WRevoluteVelocityConstraint {
         mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1);
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
         mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2);
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -320,6 +312,16 @@ impl WRevoluteVelocityGroundConstraint {
         let local_anchor2 = Point::from(
             array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
         );
+        let basis1 = Matrix3x2::from_columns(&[
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
+                ),
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
+                ),
+        ]);
         let basis2 = Matrix3x2::from_columns(&[
             position2
                 * Vector::from(
@@ -330,15 +332,12 @@ impl WRevoluteVelocityGroundConstraint {
                     array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
                 ),
         ]);
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = /*r21 * */ basis1;
         let ii2 = ii2_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r2 = anchor2 - world_com2;
@@ -358,8 +357,8 @@ impl WRevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
-        let ang_rhs = basis2.tr_mul(&(angvel2 - angvel1));
+        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
         WRevoluteVelocityGroundConstraint {