Make revolute joint actuation work properly even when SIMD is enabled.

5 files changed, 144 insertions, 93 deletions

diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 21cc642..0f01798 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -214,6 +214,12 @@ impl InteractionGroups {
                 continue;
             }
 
+            if !interaction.supports_simd_constraints() {
+                // This joint does not support simd constraints yet.
+                self.nongrouped_interactions.push(*interaction_i);
+                continue;
+            }
+
             let ijoint = interaction.params.type_id();
             let i1 = body1.active_set_offset;
             let i2 = body2.active_set_offset;
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index ebc3c4a..ed27e51 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -7,8 +7,7 @@ use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 use super::{
     WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
-    WFixedVelocityGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
-    WGenericVelocityConstraint, WGenericVelocityGroundConstraint, WPrismaticVelocityConstraint,
+    WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
     WPrismaticVelocityGroundConstraint,
 };
 #[cfg(feature = "dim3")]
@@ -122,12 +121,12 @@ impl AnyJointVelocityConstraint {
                     params, joint_id, rbs1, rbs2, joints,
                 ))
             }
-            JointParams::GenericJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
-                AnyJointVelocityConstraint::WGenericConstraint(
-                    WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
-                )
-            }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointVelocityConstraint::WGenericConstraint(
+            //         WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+            //     )
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -221,14 +220,14 @@ impl AnyJointVelocityConstraint {
                     ),
                 )
             }
-            JointParams::GenericJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
-                AnyJointVelocityConstraint::WGenericGroundConstraint(
-                    WGenericVelocityGroundConstraint::from_params(
-                        params, joint_id, rbs1, rbs2, joints, flipped,
-                    ),
-                )
-            }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointVelocityConstraint::WGenericGroundConstraint(
+            //         WGenericVelocityGroundConstraint::from_params(
+            //             params, joint_id, rbs1, rbs2, joints, flipped,
+            //         ),
+            //     )
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
index 40f5d5a..f0c374e 100644
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
@@ -10,8 +10,8 @@ use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 use super::{
     WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
-    WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
-    WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
+    WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
+    WPrismaticPositionGroundConstraint,
 };
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
@@ -99,12 +99,12 @@ impl AnyJointPositionConstraint {
                     rbs1, rbs2, joints,
                 ))
             }
-            JointParams::GenericJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
-                AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
-                    rbs1, rbs2, joints,
-                ))
-            }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
+            //         rbs1, rbs2, joints,
+            //     ))
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -180,12 +180,12 @@ impl AnyJointPositionConstraint {
                     WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
-            JointParams::GenericJoint(_) => {
-                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
-                AnyJointPositionConstraint::WGenericGroundConstraint(
-                    WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-                )
-            }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointPositionConstraint::WGenericGroundConstraint(
+            //         WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+            //     )
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
index 1219c39..10a042c 100644
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
@@ -56,24 +56,25 @@ impl RevoluteVelocityConstraint {
             rb1.position * joint.basis1[0],
             rb1.position * joint.basis1[1],
         ]);
-        let basis_filter1 = basis1 * basis1.transpose();
-        let basis2 = Matrix3x2::from_columns(&[
+        let basis_projection1 = basis1 * basis1.transpose();
+
+        let basis_projection_half2 = Matrix3x2::from_columns(&[
             rb2.position * joint.basis2[0],
             rb2.position * joint.basis2[1],
         ]);
-        let basis_filter2 = basis2 * basis2.transpose();
-        let basis2 = basis_filter2 * basis1;
+        let basis_projection2 = basis_projection_half2 * basis_projection_half2.transpose();
+        let basis2 = basis_projection2 * basis1;
 
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
 
         let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
-        let r1_mat = basis_filter1 * r1.gcross_matrix();
+        let r1_mat = basis_projection1 * r1.gcross_matrix();
 
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r2 = anchor2 - rb2.world_com;
-        let r2_mat = basis_filter2 * r2.gcross_matrix();
+        let r2_mat = basis_projection2 * r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
         let lhs00 =
@@ -318,15 +319,15 @@ impl RevoluteVelocityGroundConstraint {
             anchor1 = rb1.position * joint.local_anchor2;
             anchor2 = rb2.position * joint.local_anchor1;
             basis2 = Matrix3x2::from_columns(&[
-                rb2.position * joint.basis2[0],
-                rb2.position * joint.basis2[1],
+                rb2.position * joint.basis1[0],
+                rb2.position * joint.basis1[1],
             ]);
         } else {
             anchor1 = rb1.position * joint.local_anchor1;
             anchor2 = rb2.position * joint.local_anchor2;
             basis2 = Matrix3x2::from_columns(&[
-                rb2.position * joint.basis1[0],
-                rb2.position * joint.basis1[1],
+                rb2.position * joint.basis2[0],
+                rb2.position * joint.basis2[1],
             ]);
         };
 
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
index 047763d..5be1def 100644
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
@@ -4,9 +4,11 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
-use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
+};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
-use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, Vector5, U2, U3};
 
 #[derive(Debug)]
 pub(crate) struct WRevoluteVelocityConstraint {
@@ -15,14 +17,16 @@ pub(crate) struct WRevoluteVelocityConstraint {
 
     joint_id: [JointIndex; SIMD_WIDTH],
 
-    r1: Vector<SimdReal>,
-    r2: Vector<SimdReal>,
+    r1_mat: Matrix3<SimdReal>,
+    r2_mat: Matrix3<SimdReal>,
 
     inv_lhs: Matrix5<SimdReal>,
     rhs: Vector5<SimdReal>,
     impulse: Vector5<SimdReal>,
 
+    axis1: [Vector<Real>; SIMD_WIDTH],
     basis1: Matrix3x2<SimdReal>,
+    basis2: Matrix3x2<SimdReal>,
 
     im1: SimdReal,
     im2: SimdReal,
@@ -37,7 +41,7 @@ impl WRevoluteVelocityConstraint {
         joint_id: [JointIndex; SIMD_WIDTH],
         rbs1: [&RigidBody; SIMD_WIDTH],
         rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        joints: [&RevoluteJoint; SIMD_WIDTH],
     ) -> Self {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
@@ -59,18 +63,27 @@ impl WRevoluteVelocityConstraint {
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
         let local_basis1 = [
-            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
-            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
+        ];
+        let local_basis2 = [
+            Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
         ];
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
 
         let anchor1 = position1 * local_anchor1;
         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 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 basis2 = basis_projection2 * basis1;
 
         //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
         //            .unwrap_or_else(Rotation::identity)
@@ -81,19 +94,19 @@ impl WRevoluteVelocityConstraint {
         // 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 = r1.gcross_matrix();
+        let r1_mat = basis_projection1 * r1.gcross_matrix();
 
         let ii2 = ii2_sqrt.squared();
         let r2 = anchor2 - world_com2;
-        let r2_mat = r2.gcross_matrix();
+        let r2_mat = basis_projection2 * 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 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
-        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat));
+        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
 
-        // Note that cholesky won't read the upper-right part
+        // Note that Cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
         lhs.fixed_slice_mut::<U3, U3>(0, 0)
             .copy_from(&lhs00.into_matrix());
@@ -103,23 +116,46 @@ impl WRevoluteVelocityConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_rhs = basis1.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);
 
+        /*
+         * Adjust the warmstart impulse.
+         * If the velocity along the free axis is somewhat high,
+         * we need to adjust the angular warmstart impulse because it
+         * may have a direction that is too different than last frame,
+         * making it counter-productive.
+         */
+        let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
+        let mut impulse = impulse * warmstart_coeff;
+
+        let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
+        let rotated_impulse = Vector::from(array![|ii| {
+            let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
+                .unwrap_or_else(Rotation::identity);
+            axis_rot * joints[ii].world_ang_impulse
+        }; SIMD_WIDTH]);
+
+        let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
+        impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
+        impulse[4] = rotated_basis_impulse.y * warmstart_coeff;
+
         WRevoluteVelocityConstraint {
             joint_id,
             mj_lambda1,
             mj_lambda2,
             im1,
             ii1_sqrt,
+            axis1,
             basis1,
+            basis2,
             im2,
             ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            impulse,
             inv_lhs,
             rhs,
-            r1,
-            r2,
+            r1_mat,
+            r2_mat,
         }
     }
 
@@ -141,18 +177,20 @@ impl WRevoluteVelocityConstraint {
             ),
         };
 
-        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1);
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -184,26 +222,28 @@ 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 + ang_vel2.gcross(self.r2)
-            - mj_lambda1.linear
-            - ang_vel1.gcross(self.r1);
-        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
+
+        let lin_dvel = (mj_lambda2.linear - self.r2_mat * ang_vel2)
+            - (mj_lambda1.linear - self.r1_mat * ang_vel1);
+        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;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1_mat * lin_impulse1);
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2_mat * lin_impulse2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -216,10 +256,15 @@ impl WRevoluteVelocityConstraint {
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
+        let world_ang_impulse = self.basis1 * rot_part;
+
         for ii in 0..SIMD_WIDTH {
             let joint = &mut joints_all[self.joint_id[ii]].weight;
             if let JointParams::RevoluteJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii)
+                rev.impulse = self.impulse.extract(ii);
+                rev.world_ang_impulse = world_ang_impulse.extract(ii);
+                rev.prev_axis1 = self.axis1[ii];
             }
         }
     }
@@ -237,7 +282,7 @@ pub(crate) struct WRevoluteVelocityGroundConstraint {
     rhs: Vector5<SimdReal>,
     impulse: Vector5<SimdReal>,
 
-    basis1: Matrix3x2<SimdReal>,
+    basis2: Matrix3x2<SimdReal>,
 
     im2: SimdReal,
 
@@ -250,7 +295,7 @@ impl WRevoluteVelocityGroundConstraint {
         joint_id: [JointIndex; SIMD_WIDTH],
         rbs1: [&RigidBody; SIMD_WIDTH],
         rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        joints: [&RevoluteJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
@@ -267,22 +312,22 @@ impl WRevoluteVelocityGroundConstraint {
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
 
         let local_anchor1 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
         );
         let local_anchor2 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
         );
-        let basis1 = Matrix3x2::from_columns(&[
-            position1
+        let basis2 = Matrix3x2::from_columns(&[
+            position2
                 * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                    array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
                 ),
-            position1
+            position2
                 * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                    array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
                 ),
         ]);
 
@@ -301,8 +346,8 @@ impl WRevoluteVelocityGroundConstraint {
 
         let mut lhs = Matrix5::zeros();
         let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
 
         // Note that cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
@@ -314,7 +359,7 @@ impl WRevoluteVelocityGroundConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let ang_rhs = basis2.tr_mul(&(angvel2 - angvel1));
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
         WRevoluteVelocityGroundConstraint {
@@ -323,7 +368,7 @@ impl WRevoluteVelocityGroundConstraint {
             im2,
             ii2_sqrt,
             impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            basis1,
+            basis2,
             inv_lhs,
             rhs,
             r2,
@@ -341,7 +386,7 @@ impl WRevoluteVelocityGroundConstraint {
         };
 
         let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= lin_impulse * self.im2;
         mj_lambda2.angular -= self
@@ -366,13 +411,13 @@ impl WRevoluteVelocityGroundConstraint {
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
         let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= lin_impulse * self.im2;
         mj_lambda2.angular -= self