diff options
| -rw-r--r-- | src/dynamics/solver/joint_constraint/ball_position_constraint.rs | 27 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs | 26 |
2 files changed, 25 insertions, 28 deletions
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index e8760b3..0dfdb86 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -119,19 +119,18 @@ impl BallPositionConstraint { let axis2 = position2 * self.limits_local_axis2; #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); + let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); #[cfg(feature = "dim3")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); + let axis_angle = + Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle - { - + if let Some((axis, angle)) = axis_angle { if angle >= self.limits_angle { #[cfg(feature = "dim2")] - let axis = axis[0]; + let axis = axis[0]; #[cfg(feature = "dim3")] - let axis = axis.into_inner(); + let axis = axis.into_inner(); let ang_error = angle - self.limits_angle; let ang_impulse = self .inv_ii1_ii2 @@ -242,19 +241,19 @@ impl BallPositionGroundConstraint { let axis2 = position2 * self.limits_local_axis2; #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1).axis_angle(); + let axis_angle = + Rotation::rotation_between_axis(&axis2, &self.limits_axis1).axis_angle(); #[cfg(feature = "dim3")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1).and_then(|r| r.axis_angle()); + let axis_angle = Rotation::rotation_between_axis(&axis2, &self.limits_axis1) + .and_then(|r| r.axis_angle()); // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle - { - + if let Some((axis, angle)) = axis_angle { if angle >= self.limits_angle { #[cfg(feature = "dim2")] - let axis = axis[0]; + let axis = axis[0]; #[cfg(feature = "dim3")] - let axis = axis.into_inner(); + let axis = axis.into_inner(); let ang_error = angle - self.limits_angle; let ang_correction = axis * ang_error * params.joint_erp; position2.rotation = Rotation::new(ang_correction) * position2.rotation; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 3f61864..5abd726 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -183,19 +183,18 @@ impl BallVelocityConstraint { let axis2 = rb_pos2.position * joint.limits_local_axis2; #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); + let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); #[cfg(feature = "dim3")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); + let axis_angle = + Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle - { - + if let Some((axis, angle)) = axis_angle { if angle >= joint.limits_angle { #[cfg(feature = "dim2")] - let axis = axis[0]; + let axis = axis[0]; #[cfg(feature = "dim3")] - let axis = axis.into_inner(); + let axis = axis.into_inner(); limits_active = true; limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis)) @@ -526,19 +525,18 @@ impl BallVelocityGroundConstraint { }; #[cfg(feature = "dim2")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); + let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).axis_angle(); #[cfg(feature = "dim3")] - let axis_angle = Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); + let axis_angle = + Rotation::rotation_between_axis(&axis2, &axis1).and_then(|r| r.axis_angle()); // TODO: handle the case where dot(axis1, axis2) = -1.0 - if let Some((axis, angle)) = axis_angle - { - + if let Some((axis, angle)) = axis_angle { if angle >= joint.limits_angle { #[cfg(feature = "dim2")] - let axis = axis[0]; + let axis = axis[0]; #[cfg(feature = "dim3")] - let axis = axis.into_inner(); + let axis = axis.into_inner(); limits_active = true; limits_rhs = (rb_vels2.angvel.gdot(axis) - rb_vels1.angvel.gdot(axis)) |