diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs | 6 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs | 7 |
2 files changed, 8 insertions, 5 deletions
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 4156680..50a37e2 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -418,7 +418,8 @@ impl JointVelocityConstraintBuilder<Real> { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0*PI))/(2.0*PI) * motor_params.erp_inv_dt; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { @@ -802,7 +803,8 @@ impl JointVelocityConstraintBuilder<Real> { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs += ((s_ang_dist - s_target_ang) % (2.0*PI))/(2.0*PI) * motor_params.erp_inv_dt; + rhs += ((s_ang_dist - s_target_ang) % (2.0 * PI)) / (2.0 * PI) + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index e6cb872..2fae33a 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -357,7 +357,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi())/N::simd_two_pi() * motor_params.erp_inv_dt; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { @@ -365,7 +366,6 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> { let s_target_ang = motor_params.target_pos.simd_sin(); rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; } - } let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); @@ -805,7 +805,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> { { let s_ang_dist = self.ang_err.angle(); let s_target_ang = motor_params.target_pos; - rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi())/N::simd_two_pi() * motor_params.erp_inv_dt; + rhs_wo_bias += ((s_ang_dist - s_target_ang) % N::simd_two_pi()) / N::simd_two_pi() + * motor_params.erp_inv_dt; } #[cfg(feature = "dim3")] { |