From 19a00885d69dba87d9745edec1722513909e282a Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Mon, 21 Feb 2022 21:16:38 +0100
Subject: Use a threshold for utils::inv and simd_inv

---
 src/utils.rs | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/utils.rs b/src/utils.rs
index 9aa4bf2..07f0626 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -19,16 +19,19 @@ pub trait WReal: SimdRealField<Element = Real> + Copy {}
 impl WReal for Real {}
 impl WReal for SimdReal {}
 
+const INV_EPSILON: Real = 1.0e-20;
+
 pub(crate) fn inv(val: Real) -> Real {
-    if val == 0.0 {
+    if val >= -INV_EPSILON && val <= INV_EPSILON {
         0.0
     } else {
         1.0 / val
     }
 }
 
-pub(crate) fn simd_inv<N: SimdRealField + Copy>(val: N) -> N {
-    N::zero().select(val.simd_eq(N::zero()), N::one() / val)
+pub(crate) fn simd_inv<N: WReal>(val: N) -> N {
+    let eps = N::splat(INV_EPSILON);
+    N::zero().select(val.simd_gt(-eps) & val.simd_lt(eps), N::one() / val)
 }
 
 /// Trait to copy the sign of each component of one scalar/vector/matrix to another.
-- 
cgit 


From 15b165893c718a5606622b05b36f8041a429e30c Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Mon, 21 Feb 2022 21:19:30 +0100
Subject: Use WReal instead of SimdRealField everywhere

---
 src/dynamics/solver/delta_vel.rs                   | 10 ++++----
 .../joint_constraint/joint_velocity_constraint.rs  | 11 ++++----
 .../joint_velocity_constraint_builder.rs           |  3 +--
 src/dynamics/solver/velocity_constraint.rs         |  9 +++----
 src/dynamics/solver/velocity_constraint_element.rs | 17 +++++-------
 .../solver/velocity_ground_constraint_element.rs   | 17 +++++-------
 src/utils.rs                                       | 30 +++++++++-------------
 7 files changed, 41 insertions(+), 56 deletions(-)

(limited to 'src')

diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
index 73f3c91..cfdb791 100644
--- a/src/dynamics/solver/delta_vel.rs
+++ b/src/dynamics/solver/delta_vel.rs
@@ -1,6 +1,6 @@
 use crate::math::{AngVector, Vector, SPATIAL_DIM};
-use na::{DVectorSlice, DVectorSliceMut};
-use na::{Scalar, SimdRealField};
+use crate::utils::WReal;
+use na::{DVectorSlice, DVectorSliceMut, Scalar};
 use std::ops::{AddAssign, Sub};
 
 #[derive(Copy, Clone, Debug, Default)]
@@ -29,7 +29,7 @@ impl<N: Scalar + Copy> DeltaVel<N> {
     }
 }
 
-impl<N: SimdRealField + Copy> DeltaVel<N> {
+impl<N: WReal> DeltaVel<N> {
     pub fn zero() -> Self {
         Self {
             linear: na::zero(),
@@ -38,14 +38,14 @@ impl<N: SimdRealField + Copy> DeltaVel<N> {
     }
 }
 
-impl<N: SimdRealField + Copy> AddAssign for DeltaVel<N> {
+impl<N: WReal> AddAssign for DeltaVel<N> {
     fn add_assign(&mut self, rhs: Self) {
         self.linear += rhs.linear;
         self.angular += rhs.angular;
     }
 }
 
-impl<N: SimdRealField + Copy> Sub for DeltaVel<N> {
+impl<N: WReal> Sub for DeltaVel<N> {
     type Output = Self;
 
     fn sub(self, rhs: Self) -> Self {
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
index a4ec313..d264313 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
@@ -3,7 +3,6 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
 use crate::utils::{WDot, WReal};
-use simba::simd::SimdRealField;
 
 #[cfg(feature = "simd-is-enabled")]
 use {
@@ -12,7 +11,7 @@ use {
 };
 
 #[derive(Copy, Clone, PartialEq, Debug)]
-pub struct MotorParameters<N: SimdRealField> {
+pub struct MotorParameters<N: WReal> {
     pub stiffness: N,
     pub damping: N,
     pub gamma: N,
@@ -22,7 +21,7 @@ pub struct MotorParameters<N: SimdRealField> {
     pub max_impulse: N,
 }
 
-impl<N: SimdRealField> Default for MotorParameters<N> {
+impl<N: WReal> Default for MotorParameters<N> {
     fn default() -> Self {
         Self {
             stiffness: N::zero(),
@@ -48,7 +47,7 @@ pub enum WritebackId {
 // the solver, to avoid fetching data from the rigid-body set
 // every time.
 #[derive(Copy, Clone)]
-pub struct SolverBody<N: SimdRealField, const LANES: usize> {
+pub struct SolverBody<N: WReal, const LANES: usize> {
     pub linvel: Vector<N>,
     pub angvel: AngVector<N>,
     pub im: Vector<N>,
@@ -58,7 +57,7 @@ pub struct SolverBody<N: SimdRealField, const LANES: usize> {
 }
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> {
+pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
     pub mj_lambda1: [usize; LANES],
     pub mj_lambda2: [usize; LANES],
 
@@ -339,7 +338,7 @@ impl JointVelocityConstraint<SimdReal, SIMD_WIDTH> {
 }
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointVelocityGroundConstraint<N: SimdRealField, const LANES: usize> {
+pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
     pub mj_lambda2: [usize; LANES],
 
     pub joint_id: [JointIndex; LANES],
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
index 55a112a..3aa00f2 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
@@ -7,10 +7,9 @@ use crate::dynamics::{IntegrationParameters, JointIndex};
 use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
 use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
 use na::SMatrix;
-use simba::simd::SimdRealField;
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointVelocityConstraintBuilder<N: SimdRealField> {
+pub struct JointVelocityConstraintBuilder<N: WReal> {
     pub basis: Matrix<N>,
     pub cmat1_basis: SMatrix<N, ANG_DIM, DIM>,
     pub cmat2_basis: SMatrix<N, ANG_DIM, DIM>,
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index bb00b66..5711518 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -5,7 +5,7 @@ use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
-use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot, WReal};
 
 use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
 
@@ -373,8 +373,7 @@ pub(crate) fn compute_tangent_contact_directions<N>(
     linvel2: &Vector<N>,
 ) -> [Vector<N>; DIM - 1]
 where
-    N: na::SimdRealField + Copy,
-    N::Element: na::RealField + Copy,
+    N: WReal,
     Vector<N>: WBasis,
 {
     use na::SimdValue;
@@ -392,8 +391,8 @@ where
         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));
+    const THRESHOLD: Real = 1.0e-4;
+    let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
     let tangent_fallback = force_dir1.orthonormal_vector();
 
     let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index 2d2221d..30afef5 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -1,10 +1,9 @@
 use super::DeltaVel;
 use crate::math::{AngVector, Vector, DIM};
-use crate::utils::{WBasis, WDot};
-use na::SimdRealField;
+use crate::utils::{WBasis, WDot, WReal};
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
+pub(crate) struct VelocityConstraintTangentPart<N: WReal> {
     pub gcross1: [AngVector<N>; DIM - 1],
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
@@ -18,7 +17,7 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
     pub r: [N; DIM],
 }
 
-impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
+impl<N: WReal> VelocityConstraintTangentPart<N> {
     fn zero() -> Self {
         Self {
             gcross1: [na::zero(); DIM - 1],
@@ -43,7 +42,6 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
         mj_lambda2: &mut DeltaVel<N>,
     ) where
         AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
     {
         #[cfg(feature = "dim2")]
         {
@@ -107,7 +105,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
 }
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraintNormalPart<N: SimdRealField + Copy> {
+pub(crate) struct VelocityConstraintNormalPart<N: WReal> {
     pub gcross1: AngVector<N>,
     pub gcross2: AngVector<N>,
     pub rhs: N,
@@ -116,7 +114,7 @@ pub(crate) struct VelocityConstraintNormalPart<N: SimdRealField + Copy> {
     pub r: N,
 }
 
-impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
+impl<N: WReal> VelocityConstraintNormalPart<N> {
     fn zero() -> Self {
         Self {
             gcross1: na::zero(),
@@ -157,12 +155,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
 }
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityConstraintElement<N: SimdRealField + Copy> {
+pub(crate) struct VelocityConstraintElement<N: WReal> {
     pub normal_part: VelocityConstraintNormalPart<N>,
     pub tangent_part: VelocityConstraintTangentPart<N>,
 }
 
-impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
+impl<N: WReal> VelocityConstraintElement<N> {
     pub fn zero() -> Self {
         Self {
             normal_part: VelocityConstraintNormalPart::zero(),
@@ -186,7 +184,6 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
     ) where
         Vector<N>: WBasis,
         AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
     {
         // Solve penetration.
         if solve_normal {
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 8057030..06a727a 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -1,10 +1,9 @@
 use super::DeltaVel;
 use crate::math::{AngVector, Vector, DIM};
-use crate::utils::{WBasis, WDot};
-use na::SimdRealField;
+use crate::utils::{WBasis, WDot, WReal};
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
+pub(crate) struct VelocityGroundConstraintTangentPart<N: WReal> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     #[cfg(feature = "dim2")]
@@ -17,7 +16,7 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
     pub r: [N; DIM],
 }
 
-impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
+impl<N: WReal> VelocityGroundConstraintTangentPart<N> {
     fn zero() -> Self {
         Self {
             gcross2: [na::zero(); DIM - 1],
@@ -39,7 +38,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
         mj_lambda2: &mut DeltaVel<N>,
     ) where
         AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
     {
         #[cfg(feature = "dim2")]
         {
@@ -89,7 +87,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
 }
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
+pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
     pub gcross2: AngVector<N>,
     pub rhs: N,
     pub rhs_wo_bias: N,
@@ -97,7 +95,7 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
     pub r: N,
 }
 
-impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
+impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
     fn zero() -> Self {
         Self {
             gcross2: na::zero(),
@@ -129,12 +127,12 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
 }
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
+pub(crate) struct VelocityGroundConstraintElement<N: WReal> {
     pub normal_part: VelocityGroundConstraintNormalPart<N>,
     pub tangent_part: VelocityGroundConstraintTangentPart<N>,
 }
 
-impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
+impl<N: WReal> VelocityGroundConstraintElement<N> {
     pub fn zero() -> Self {
         Self {
             normal_part: VelocityGroundConstraintNormalPart::zero(),
@@ -156,7 +154,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
     ) where
         Vector<N>: WBasis,
         AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
     {
         // Solve penetration.
         if solve_normal {
diff --git a/src/utils.rs b/src/utils.rs
index 07f0626..c9edf16 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -126,7 +126,7 @@ pub trait WBasis: Sized {
     fn orthonormal_vector(self) -> Self;
 }
 
-impl<N: SimdRealField + Copy> WBasis for Vector2<N> {
+impl<N: WReal> WBasis for Vector2<N> {
     type Basis = [Vector2<N>; 1];
     fn orthonormal_basis(self) -> [Vector2<N>; 1] {
         [Vector2::new(-self.y, self.x)]
@@ -136,7 +136,7 @@ impl<N: SimdRealField + Copy> WBasis for Vector2<N> {
     }
 }
 
-impl<N: SimdRealField + Copy + WSign<N>> WBasis for Vector3<N> {
+impl<N: WReal + WSign<N>> WBasis for Vector3<N> {
     type Basis = [Vector3<N>; 2];
     // Robust and branchless implementation from Pixar:
     // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
@@ -254,7 +254,7 @@ pub(crate) trait WCrossMatrix: Sized {
     fn gcross_matrix_tr(self) -> Self::CrossMatTr;
 }
 
-impl<N: SimdRealField + Copy> WCrossMatrix for Vector3<N> {
+impl<N: WReal> WCrossMatrix for Vector3<N> {
     type CrossMat = Matrix3<N>;
     type CrossMatTr = Matrix3<N>;
 
@@ -279,7 +279,7 @@ impl<N: SimdRealField + Copy> WCrossMatrix for Vector3<N> {
     }
 }
 
-impl<N: SimdRealField + Copy> WCrossMatrix for Vector2<N> {
+impl<N: WReal> WCrossMatrix for Vector2<N> {
     type CrossMat = RowVector2<N>;
     type CrossMatTr = Vector2<N>;
 
@@ -356,7 +356,7 @@ pub(crate) trait WDot<Rhs>: Sized {
     fn gdot(&self, rhs: Rhs) -> Self::Result;
 }
 
-impl<N: SimdRealField + Copy> WDot<Vector3<N>> for Vector3<N> {
+impl<N: WReal> WDot<Vector3<N>> for Vector3<N> {
     type Result = N;
 
     fn gdot(&self, rhs: Vector3<N>) -> Self::Result {
@@ -364,7 +364,7 @@ impl<N: SimdRealField + Copy> WDot<Vector3<N>> for Vector3<N> {
     }
 }
 
-impl<N: SimdRealField + Copy> WDot<Vector2<N>> for Vector2<N> {
+impl<N: WReal> WDot<Vector2<N>> for Vector2<N> {
     type Result = N;
 
     fn gdot(&self, rhs: Vector2<N>) -> Self::Result {
@@ -372,7 +372,7 @@ impl<N: SimdRealField + Copy> WDot<Vector2<N>> for Vector2<N> {
     }
 }
 
-impl<N: SimdRealField + Copy> WDot<Vector1<N>> for N {
+impl<N: WReal> WDot<Vector1<N>> for N {
     type Result = N;
 
     fn gdot(&self, rhs: Vector1<N>) -> Self::Result {
@@ -388,7 +388,7 @@ impl<N: WReal> WDot<N> for N {
     }
 }
 
-impl<N: SimdRealField + Copy> WDot<N> for Vector1<N> {
+impl<N: WReal> WDot<N> for Vector1<N> {
     type Result = N;
 
     fn gdot(&self, rhs: N) -> Self::Result {
@@ -428,26 +428,20 @@ pub trait WQuat<N> {
     fn diff_conj1_2(&self, rhs: &Self) -> Self::Result;
 }
 
-impl<N: SimdRealField + Copy> WQuat<N> for UnitComplex<N>
-where
-    <N as SimdValue>::Element: SimdRealField,
-{
+impl<N: WReal> WQuat<N> for UnitComplex<N> {
     type Result = Matrix1<N>;
 
     fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
-        let two: N = na::convert(2.0);
+        let two: N = N::splat(2.0);
         Matrix1::new((self.im * rhs.im + self.re * rhs.re) * two)
     }
 }
 
-impl<N: SimdRealField + Copy> WQuat<N> for UnitQuaternion<N>
-where
-    <N as SimdValue>::Element: SimdRealField,
-{
+impl<N: WReal> WQuat<N> for UnitQuaternion<N> {
     type Result = Matrix3<N>;
 
     fn diff_conj1_2(&self, rhs: &Self) -> Self::Result {
-        let half: N = na::convert(0.5);
+        let half = N::splat(0.5);
         let v1 = self.imag();
         let v2 = rhs.imag();
         let w1 = self.w;
-- 
cgit