Fix Nans from RevoluteJoint::angle()

Fixes dimforge#735
chrisdunelm · Sep 21, 2024 · 5629688 · 5629688
1 parent e7e196d
commit 5629688
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")]