Add support of 64-bits reals.

31 files changed, 282 insertions, 279 deletions

diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
index c4a424b..4614ed7 100644
--- a/src/dynamics/solver/delta_vel.rs
+++ b/src/dynamics/solver/delta_vel.rs
@@ -1,9 +1,9 @@
-use crate::math::{AngVector, Vector};
+use crate::math::{AngVector, Real, Vector};
 use na::{Scalar, SimdRealField};
 
 #[derive(Copy, Clone, Debug)]
 //#[repr(align(64))]
-pub(crate) struct DeltaVel<N: Scalar> {
+pub(crate) struct DeltaVel<N: Scalar + Copy> {
     pub linear: Vector<N>,
     pub angular: AngVector<N>,
 }
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index b5f8173..b409f98 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -2,7 +2,7 @@ use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use {
-    crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
+    crate::math::{Real, SIMD_LAST_INDEX, SIMD_WIDTH},
     vec_map::VecMap,
 };
 
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 21a537e..8bc9072 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
 #[cfg(feature = "dim2")]
 use crate::math::SdpMatrix;
-use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint {
     position1: usize,
     position2: usize,
 
-    local_com1: Point<f32>,
-    local_com2: Point<f32>,
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    local_anchor1: Point<f32>,
-    local_anchor2: Point<f32>,
+    local_anchor1: Point<Real>,
+    local_anchor2: Point<Real>,
 }
 
 impl BallPositionConstraint {
@@ -38,7 +38,7 @@ impl BallPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -95,11 +95,11 @@ impl BallPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct BallPositionGroundConstraint {
     position2: usize,
-    anchor1: Point<f32>,
-    im2: f32,
-    ii2: AngularInertia<f32>,
-    local_anchor2: Point<f32>,
-    local_com2: Point<f32>,
+    anchor1: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    local_anchor2: Point<Real>,
+    local_com2: Point<Real>,
 }
 
 impl BallPositionGroundConstraint {
@@ -133,7 +133,7 @@ impl BallPositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
         let anchor2 = position2 * self.local_anchor2;
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
index f1ca4b6..2f2ffc3 100644
--- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
 #[cfg(feature = "dim2")]
 use crate::math::SdpMatrix;
-use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use simba::simd::SimdValue;
 
@@ -60,7 +60,7 @@ impl WBallPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
         let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
 
@@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
 
         let anchor2 = position2 * self.local_anchor2;
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index 97ba244..9f47624 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{SdpMatrix, Vector};
+use crate::math::{Real, SdpMatrix, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -12,16 +12,16 @@ pub(crate) struct BallVelocityConstraint {
 
     joint_id: JointIndex,
 
-    rhs: Vector<f32>,
-    pub(crate) impulse: Vector<f32>,
+    rhs: Vector<Real>,
+    pub(crate) impulse: Vector<Real>,
 
-    gcross1: Vector<f32>,
-    gcross2: Vector<f32>,
+    gcross1: Vector<Real>,
+    gcross2: Vector<Real>,
 
-    inv_lhs: SdpMatrix<f32>,
+    inv_lhs: SdpMatrix<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 }
 
 impl BallVelocityConstraint {
@@ -91,7 +91,7 @@ impl BallVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -104,7 +104,7 @@ impl BallVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -137,11 +137,11 @@ impl BallVelocityConstraint {
 pub(crate) struct BallVelocityGroundConstraint {
     mj_lambda2: usize,
     joint_id: JointIndex,
-    rhs: Vector<f32>,
-    impulse: Vector<f32>,
-    gcross2: Vector<f32>,
-    inv_lhs: SdpMatrix<f32>,
-    im2: f32,
+    rhs: Vector<Real>,
+    impulse: Vector<Real>,
+    gcross2: Vector<Real>,
+    inv_lhs: SdpMatrix<Real>,
+    im2: Real,
 }
 
 impl BallVelocityGroundConstraint {
@@ -206,14 +206,14 @@ impl BallVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
         mj_lambda2.linear -= self.im2 * self.impulse;
         mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
index bbb709e..2806233 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
@@ -3,7 +3,7 @@ use crate::dynamics::{
     BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
+    AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use simba::simd::SimdValue;
@@ -107,7 +107,7 @@ impl WBallVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -140,7 +140,7 @@ impl WBallVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -274,7 +274,7 @@ impl WBallVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -293,7 +293,7 @@ impl WBallVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
index 24001cd..cf9dcb7 100644
--- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
@@ -1,22 +1,22 @@
 use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
-use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
 use crate::utils::WAngularInertia;
 
 #[derive(Debug)]
 pub(crate) struct FixedPositionConstraint {
     position1: usize,
     position2: usize,
-    local_anchor1: Isometry<f32>,
-    local_anchor2: Isometry<f32>,
-    local_com1: Point<f32>,
-    local_com2: Point<f32>,
-    im1: f32,
-    im2: f32,
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
-
-    lin_inv_lhs: f32,
-    ang_inv_lhs: AngularInertia<f32>,
+    local_anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
+    im1: Real,
+    im2: Real,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
+
+    lin_inv_lhs: Real,
+    ang_inv_lhs: AngularInertia<Real>,
 }
 
 impl FixedPositionConstraint {
@@ -44,7 +44,7 @@ impl FixedPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -81,12 +81,12 @@ impl FixedPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct FixedPositionGroundConstraint {
     position2: usize,
-    anchor1: Isometry<f32>,
-    local_anchor2: Isometry<f32>,
-    local_com2: Point<f32>,
-    im2: f32,
-    ii2: AngularInertia<f32>,
-    impulse: f32,
+    anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com2: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    impulse: Real,
 }
 
 impl FixedPositionGroundConstraint {
@@ -118,7 +118,7 @@ impl FixedPositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
         // Angular correction.
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
index e4187c8..715b7dd 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{AngularInertia, Dim, SpacialVector, Vector};
+use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim2")]
 use na::{Matrix3, Vector3};
@@ -16,29 +16,29 @@ pub(crate) struct FixedVelocityConstraint {
 
     joint_id: JointIndex,
 
-    impulse: SpacialVector<f32>,
+    impulse: SpacialVector<Real>,
 
     #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim3")]
-    rhs: Vector6<f32>,
+    rhs: Vector6<Real>,
 
     #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim2")]
-    rhs: Vector3<f32>,
+    rhs: Vector3<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    ii1_sqrt: AngularInertia<f32>,
-    ii2_sqrt: AngularInertia<f32>,
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
 
-    r1: Vector<f32>,
-    r2: Vector<f32>,
+    r1: Vector<Real>,
+    r2: Vector<Real>,
 }
 
 impl FixedVelocityConstraint {
@@ -128,7 +128,7 @@ impl FixedVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -152,7 +152,7 @@ impl FixedVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -207,22 +207,22 @@ pub(crate) struct FixedVelocityGroundConstraint {
 
     joint_id: JointIndex,
 
-    impulse: SpacialVector<f32>,
+    impulse: SpacialVector<Real>,
 
     #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim3")]
-    rhs: Vector6<f32>,
+    rhs: Vector6<Real>,
 
     #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim2")]
-    rhs: Vector3<f32>,
+    rhs: Vector3<Real>,
 
-    im2: f32,
-    ii2: AngularInertia<f32>,
-    ii2_sqrt: AngularInertia<f32>,
-    r2: Vector<f32>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+    r2: Vector<Real>,
 }
 
 impl FixedVelocityGroundConstraint {
@@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -329,7 +329,7 @@ impl FixedVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
index 6584ea2..48a6cba 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
@@ -5,8 +5,8 @@ use crate::dynamics::{
     FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
 use crate::math::{
-    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector,
-    SIMD_WIDTH,
+    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
+    Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim3")]
@@ -158,7 +158,7 @@ impl WFixedVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -202,7 +202,7 @@ impl WFixedVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -393,7 +393,7 @@ impl WFixedVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -420,7 +420,7 @@ impl WFixedVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index 8b1f8bf..ed6d758 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -17,6 +17,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
 };
+use crate::math::Real;
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
 
@@ -220,7 +221,7 @@ impl AnyJointVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         match self {
             AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
@@ -254,7 +255,7 @@ impl AnyJointVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         match self {
             AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),
diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
index 4773687..0ebe88e 100644
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
@@ -7,9 +7,9 @@ use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
-use crate::math::Isometry;
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
+use crate::math::{Isometry, Real};
 
 pub(crate) enum AnyJointPositionConstraint {
     BallJoint(BallPositionConstraint),
@@ -147,7 +147,7 @@ impl AnyJointPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         match self {
             AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),
diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
index c3e4b98..2616f6b 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
-use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
 use crate::utils::WAngularInertia;
 use na::Unit;
 
@@ -8,22 +8,22 @@ pub(crate) struct PrismaticPositionConstraint {
     position1: usize,
     position2: usize,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    lin_inv_lhs: f32,
-    ang_inv_lhs: AngularInertia<f32>,
+    lin_inv_lhs: Real,
+    ang_inv_lhs: AngularInertia<Real>,
 
-    limits: [f32; 2],
+    limits: [Real; 2],
 
-    local_frame1: Isometry<f32>,
-    local_frame2: Isometry<f32>,
+    local_frame1: Isometry<Real>,
+    local_frame2: Isometry<Real>,
 
-    local_axis1: Unit<Vector<f32>>,
-    local_axis2: Unit<Vector<f32>>,
+    local_axis1: Unit<Vector<Real>>,
+    local_axis2: Unit<Vector<Real>>,
 }
 
 impl PrismaticPositionConstraint {
@@ -52,7 +52,7 @@ impl PrismaticPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -99,11 +99,11 @@ impl PrismaticPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct PrismaticPositionGroundConstraint {
     position2: usize,
-    frame1: Isometry<f32>,
-    local_frame2: Isometry<f32>,
-    axis1: Unit<Vector<f32>>,
-    local_axis2: Unit<Vector<f32>>,
-    limits: [f32; 2],
+    frame1: Isometry<Real>,
+    local_frame2: Isometry<Real>,
+    axis1: Unit<Vector<Real>>,
+    local_axis2: Unit<Vector<Real>>,
+    limits: [Real; 2],
 }
 
 impl PrismaticPositionGroundConstraint {
@@ -140,7 +140,7 @@ impl Prisma