diff options
4 files changed, 73 insertions, 54 deletions
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 022779d..e1e1441 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/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, } } diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs index dd6035d..c2c9ebd 100644 --- a/src/dynamics/joint/spring_model.rs +++ b/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. /// diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 10a042c..ef3a1ba 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -15,8 +15,8 @@ pub(crate) struct RevoluteVelocityConstraint { joint_id: JointIndex, - r1_mat: Matrix3<Real>, - r2_mat: Matrix3<Real>, + r1: Vector<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, - r2_mat, + r1, + 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) - - (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; @@ -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 ang_rhs = basis2.tr_mul(&(rb2.angvel - 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 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); 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 { |