From 564f7b73481e89eb144fc786aad39df1d866c61a Mon Sep 17 00:00:00 2001
From: Chris Bacon <chris@dunelm.org.uk>
Date: Wed, 8 Jan 2025 16:47:13 +0000
Subject: Fix Nans from RevoluteJoint::angle() (#736)

Fixes #735
---
 src/dynamics/joint/revolute_joint.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 4dfed5c..c7f0288 100644
--- 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")]
-- 
cgit