Fix Nans from RevoluteJoint::angle() (#736)

Fixes #735
dimforge · Jan 8, 2025 · 564f7b7 · 564f7b7
1 parent dc4bd24
commit 564f7b7
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
@@ -87,9 +87,9 @@ impl RevoluteJoint {
 
         #[cfg(feature = "dim3")]
         if joint_rot1.dot(&joint_rot2) < 0.0 {
-            -ang_err.i.asin() * 2.0
+            -ang_err.i.clamp(-1.0, 1.0).asin() * 2.0
         } else {
-            ang_err.i.asin() * 2.0
+            ang_err.i.clamp(-1.0, 1.0).asin() * 2.0
         }
 
         #[cfg(feature = "dim2")]