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

Fixes #735
2025-01-08 16:47:13 +00:00
parent dc4bd24da8
commit 564f7b7348
1 changed files with 2 additions and 2 deletions
--- 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")]