diff options
Diffstat (limited to 'src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs | 57 |
1 files changed, 28 insertions, 29 deletions
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 5be1def..d229457 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/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>, - r2_mat: Matrix3<SimdReal>, + r1: Vector<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 basis_projection_half2 = + let basis2 = 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, - r2_mat, + r1, + 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) - - (mj_lambda1.linear - self.r1_mat * ang_vel1); + let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2)) + - (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 ang_rhs = basis2.tr_mul(&(angvel2 - angvel1)); + let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1)); + 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 { |