Fix NaN resulting from non-clamped input to simd_asin in angular motor solver (#840)

* Fix non-clamped input to simd_asin in motor_angular

* Fix implementation & cleanup

* Cleanup

* Fix formatting

Author: ryanhcode

src/dynamics/solver/joint_constraint/joint_constraint_builder.rs

@@ -753,10 +753,20 @@ impl<N: SimdRealCopy> JointTwoBodyConstraintHelper<N> {
 
         let mut rhs_wo_bias = N::zero();
         if motor_params.erp_inv_dt != N::zero() {
+            let ang_dist;
+
             #[cfg(feature = "dim2")]
-            let ang_dist = self.ang_err.angle();
+            {
+                ang_dist = self.ang_err.angle();
+            }
+
             #[cfg(feature = "dim3")]
-            let ang_dist = self.ang_err.imag()[_motor_axis].simd_asin() * N::splat(2.0);
+            {
+                // Clamp the component from -1.0 to 1.0 to account for slight imprecision
+                let clamped_err = self.ang_err.imag()[_motor_axis].simd_clamp(-N::one(), N::one());
+                ang_dist = clamped_err.simd_asin() * N::splat(2.0);
+            }
+
             let target_ang = motor_params.target_pos;
             rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang)
                 * motor_params.erp_inv_dt;