wip

8 files changed, 67 insertions, 3 deletions

diff --git a/build/rapier2d-f64/Cargo.toml b/build/rapier2d-f64/Cargo.toml
index d244d9b..913f873 100644
--- a/build/rapier2d-f64/Cargo.toml
+++ b/build/rapier2d-f64/Cargo.toml
@@ -20,6 +20,7 @@ default = [ "dim2", "f64", "default-sets" ]
 dim2    = [ ]
 f64     = [ ]
 default-sets = [ ]
+avoid-fe-exceptions = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml
index a149bd9..6fecbdb 100644
--- a/build/rapier2d/Cargo.toml
+++ b/build/rapier2d/Cargo.toml
@@ -20,6 +20,7 @@ default = [ "dim2", "f32", "default-sets" ]
 dim2    = [ ]
 f32     = [ ]
 default-sets = [ ]
+avoid-fe-exceptions = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
diff --git a/build/rapier3d-f64/Cargo.toml b/build/rapier3d-f64/Cargo.toml
index 4b6519c..2292e12 100644
--- a/build/rapier3d-f64/Cargo.toml
+++ b/build/rapier3d-f64/Cargo.toml
@@ -20,6 +20,7 @@ default = [ "dim3", "f64", "default-sets" ]
 dim3    = [ ]
 f64     = [ ]
 default-sets = [ ]
+avoid-fe-exceptions = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml
index 8a3877b..541c517 100644
--- a/build/rapier3d/Cargo.toml
+++ b/build/rapier3d/Cargo.toml
@@ -20,6 +20,7 @@ default = [ "dim3", "f32", "default-sets" ]
 dim3    = [ ]
 f32     = [ ]
 default-sets = [ ]
+avoid-fe-exceptions = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index e15f2d8..90c574c 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -385,7 +385,13 @@ where
     let relative_linvel = linvel1 - linvel2;
     let mut tangent_relative_linvel =
         relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
-    let tangent_linvel_norm = tangent_relative_linvel.normalize_mut();
+
+    let tangent_linvel_norm = {
+        let _disable_fe_except =
+            crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
+        tangent_relative_linvel.normalize_mut()
+    };
+
     let threshold: N::Element = na::convert(1.0e-4);
     let use_fallback = tangent_linvel_norm.simd_lt(N::splat(threshold));
     let tangent_fallback = force_dir1.orthonormal_vector();
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index 1324c35..d1ad6ac 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -99,7 +99,13 @@ impl<N: SimdRealField> VelocityConstraintTangentPart<N> {
                 self.impulse[0] - self.r[0] * dimpulse_0,
                 self.impulse[1] - self.r[1] * dimpulse_1,
             );
-            let new_impulse = new_impulse.simd_cap_magnitude(limit);
+            let new_impulse = {
+                let _disable_fe_except =
+                    crate::utils::DisableFloatingPointExceptionsFlags::
+                    disable_floating_point_exceptions();
+                new_impulse.simd_cap_magnitude(limit)
+            };
+
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 8bcdc82..c4c6393 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -78,7 +78,12 @@ impl<N: SimdRealField> VelocityGroundConstraintTangentPart<N> {
                 self.impulse[0] - self.r[0] * dimpulse_0,
                 self.impulse[1] - self.r[1] * dimpulse_1,
             );
-            let new_impulse = new_impulse.simd_cap_magnitude(limit);
+            let new_impulse = {
+                let _disable_fe_except =
+                    crate::utils::DisableFloatingPointExceptionsFlags::
+                    disable_floating_point_exceptions();
+                new_impulse.simd_cap_magnitude(limit)
+            };
             let dlambda = new_impulse - self.impulse;
 
             self.impulse = new_impulse;
diff --git a/src/utils.rs b/src/utils.rs
index c1eb31d..4ea2339 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -4,6 +4,7 @@ use na::{Matrix3, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
 use num::Zero;
 use simba::simd::SimdValue;
 use std::ops::IndexMut;
+use std::os::raw::c_int;
 
 use parry::utils::SdpMatrix3;
 use {
@@ -670,6 +671,48 @@ impl Drop for FlushToZeroDenormalsAreZeroFlags {
     }
 }
 
+// This is an RAII structure that disables floating point exceptions while
+// it is alive, so that operations which generate NaNs and infinite values
+// intentionally will not trip an exception when debugging problematic
+// code that is generating NaNs and infinite values erroneously.
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(crate) struct DisableFloatingPointExceptionsFlags {
+    original_flags: c_int,
+}
+
+#[cfg(feature = "avoid-fe-exceptions")]
+extern "C" {
+    fn feenableexcept(flags: c_int);
+    fn fecleareexcept(flags: c_int);
+    fn fetestexcept(flags: c_int) -> c_int;
+    static FE_ALL_EXCEPT: c_int;
+}
+
+impl DisableFloatingPointExceptionsFlags {
+    #[cfg(not(feature = "avoid-fe-exceptions"))]
+    pub fn disable_floating_point_exceptions() -> Self {
+        Self { original_flags: 0 }
+    }
+
+    #[cfg(feature = "avoid-fe-exceptions")]
+    pub fn disable_floating_point_exceptions() -> Self {
+        unsafe {
+            let original_flags = fetestexcept(FE_ALL_EXCEPT);
+            fecleareexcept(FE_ALL_EXCEPT);
+            Self { original_flags }
+        }
+    }
+}
+
+#[cfg(feature = "avoid-fe-exceptions")]
+impl Drop for DisableFloatingPointExceptionsFlags {
+    fn drop(&mut self) {
+        unsafe {
+            feenableexcept(self.original_flags);
+        }
+    }
+}
+
 pub(crate) fn select_other<T: PartialEq>(pair: (T, T), elt: T) -> T {
     if pair.0 == elt {
         pair.1