From 86a257d4f12e120b3f44740889440d0f5fce48d8 Mon Sep 17 00:00:00 2001
From: "Ryan H. Code" <42245712+ryanhcode@users.noreply.github.com>
Date: Fri, 11 Jul 2025 09:32:59 -0400
Subject: [PATCH] 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
---
 .../joint_constraint/joint_constraint_builder.rs   | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
index 9948d3f..9309939 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
+++ b/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;