Merge pull request #79 from dimforge/split_geom

Move most of the geometric code to another crate.

39 files changed, 1982 insertions, 2933 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index 9ddf791..c920b69 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -1,35 +1,7 @@
 use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
-use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
-pub(crate) fn categorize_position_contacts(
-    bodies: &RigidBodySet,
-    manifolds: &[&mut ContactManifold],
-    manifold_indices: &[ContactManifoldIndex],
-    out_point_point_ground: &mut Vec<ContactManifoldIndex>,
-    out_plane_point_ground: &mut Vec<ContactManifoldIndex>,
-    out_point_point: &mut Vec<ContactManifoldIndex>,
-    out_plane_point: &mut Vec<ContactManifoldIndex>,
-) {
-    for manifold_i in manifold_indices {
-        let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
-
-        if !rb1.is_dynamic() || !rb2.is_dynamic() {
-            match manifold.kinematics.category {
-                KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
-                KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
-            }
-        } else {
-            match manifold.kinematics.category {
-                KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
-                KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
-            }
-        }
-    }
-}
-
-pub(crate) fn categorize_velocity_contacts(
+pub(crate) fn categorize_contacts(
     bodies: &RigidBodySet,
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
@@ -38,8 +10,8 @@ pub(crate) fn categorize_velocity_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
             out_ground.push(*manifold_i)
diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
index c4a424b..378d302 100644
--- a/src/dynamics/solver/delta_vel.rs
+++ b/src/dynamics/solver/delta_vel.rs
@@ -3,7 +3,7 @@ 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 04acaaf..21cc642 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -12,7 +12,7 @@ pub(crate) trait PairInteraction {
 
 impl<'a> PairInteraction for &'a mut ContactManifold {
     fn body_pair(&self) -> BodyPair {
-        self.body_pair
+        self.data.body_pair
     }
 }
 
@@ -338,7 +338,7 @@ impl InteractionGroups {
         let mut occupied_mask = 0u128;
         let max_interaction_points = interaction_indices
             .iter()
-            .map(|i| interactions[*i].num_active_contacts())
+            .map(|i| interactions[*i].data.num_active_contacts())
             .max()
             .unwrap_or(1);
 
@@ -351,12 +351,12 @@ impl InteractionGroups {
 
                 // FIXME: how could we avoid iterating
                 // on each interaction at every iteration on k?
-                if interaction.num_active_contacts() != k {
+                if interaction.data.num_active_contacts() != k {
                     continue;
                 }
 
-                let body1 = &bodies[interaction.body_pair.body1];
-                let body2 = &bodies[interaction.body_pair.body2];
+                let body1 = &bodies[interaction.data.body_pair.body1];
+                let body2 = &bodies[interaction.data.body_pair.body2];
                 let is_static1 = !body1.is_dynamic();
                 let is_static2 = !body2.is_dynamic();
 
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index b548c6b..deed8c2 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,9 +1,15 @@
 use super::{PositionSolver, VelocitySolver};
 use crate::counters::Counters;
+use crate::dynamics::solver::{
+    AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
+    AnyVelocityConstraint, SolverConstraints,
+};
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub struct IslandSolver {
+    contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
+    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
     velocity_solver: VelocitySolver,
     position_solver: PositionSolver,
 }
@@ -11,6 +17,8 @@ pub struct IslandSolver {
 impl IslandSolver {
     pub fn new() -> Self {
         Self {
+            contact_constraints: SolverConstraints::new(),
+            joint_constraints: SolverConstraints::new(),
             velocity_solver: VelocitySolver::new(),
             position_solver: PositionSolver::new(),
         }
@@ -29,33 +37,23 @@ impl IslandSolver {
     ) {
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.velocity_assembly_time.resume();
-            self.velocity_solver.init_constraints(
-                island_id,
-                params,
-                bodies,
-                manifolds,
-                &manifold_indices,
-                joints,
-                &joint_indices,
-            );
+            self.contact_constraints
+                .init(island_id, params, bodies, manifolds, manifold_indices);
+            self.joint_constraints
+                .init(island_id, params, bodies, joints, joint_indices);
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
-            self.velocity_solver
-                .solve_constraints(island_id, params, bodies, manifolds, joints);
-            counters.solver.velocity_resolution_time.pause();
-
-            counters.solver.position_assembly_time.resume();
-            self.position_solver.init_constraints(
+            self.velocity_solver.solve(
                 island_id,
                 params,
                 bodies,
                 manifolds,
-                &manifold_indices,
                 joints,
-                &joint_indices,
+                &mut self.contact_constraints.velocity_constraints,
+                &mut self.joint_constraints.velocity_constraints,
             );
-            counters.solver.position_assembly_time.pause();
+            counters.solver.velocity_resolution_time.pause();
         }
 
         counters.solver.velocity_update_time.resume();
@@ -64,8 +62,13 @@ impl IslandSolver {
 
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.position_resolution_time.resume();
-            self.position_solver
-                .solve_constraints(island_id, params, bodies);
+            self.position_solver.solve(
+                island_id,
+                params,
+                bodies,
+                &self.contact_constraints.position_constraints,
+                &self.joint_constraints.position_constraints,
+            );
             counters.solver.position_resolution_time.pause();
         }
     }
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs
index 21a537e..744c00d 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 {
@@ -27,10 +27,10 @@ impl BallPositionConstraint {
         Self {
             local_com1: rb1.mass_properties.local_com,
             local_com2: rb2.mass_properties.local_com,
-            im1: rb1.mass_properties.inv_mass,
-            im2: rb2.mass_properties.inv_mass,
-            ii1: rb1.world_inv_inertia_sqrt.squared(),
-            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            im1: rb1.effective_inv_mass,
+            im2: rb2.effective_inv_mass,
+            ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
             position1: rb1.active_set_offset,
@@ -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 {
@@ -115,8 +115,8 @@ impl BallPositionGroundConstraint {
             // already been flipped by the caller.
             Self {
                 anchor1: rb1.predicted_position * cparams.local_anchor2,
-                im2: rb2.mass_properties.inv_mass,
-                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                im2: rb2.effective_inv_mass,
+                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
                 position2: rb2.active_set_offset,
                 local_com2: rb2.mass_properties.local_com,
@@ -124,8 +124,8 @@ impl BallPositionGroundConstraint {
         } else {
             Self {
                 anchor1: rb1.predicted_position * cparams.local_anchor1,
-                im2: rb2.mass_properties.inv_mass,
-                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                im2: rb2.effective_inv_mass,
+                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
                 position2: rb2.active_set_offset,
                 local_com2: rb2.mass_properties.local_com,
@@ -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 c552d57..043eea7 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, SimdFloat, SIMD_WIDTH};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use simba::simd::SimdValue;
 
@@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint {
     position1: [usize; SIMD_WIDTH],
     position2: [usize; SIMD_WIDTH],
 
-    local_com1: Point<SimdFloat>,
-    local_com2: Point<SimdFloat>,
+    local_com1: Point<SimdReal>,
+    local_com2: Point<SimdReal>,
 
-    im1: SimdFloat,
-    im2: SimdFloat,
+    im1: SimdReal,
+    im2: SimdReal,
 
-    ii1: AngularInertia<SimdFloat>,
-    ii2: AngularInertia<SimdFloat>,
+    ii1: AngularInertia<SimdReal>,
+    ii2: AngularInertia<SimdReal>,
 
-    local_anchor1: Point<SimdFloat>,
-    local_anchor2: Point<SimdFloat>,
+    local_anchor1: Point<SimdReal>,
+    local_anchor2: Point<SimdReal>,
 }
 
 impl WBallPositionConstraint {
@@ -31,14 +31,14 @@ impl WBallPositionConstraint {
     ) -> Self {
         let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
         let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
-        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
-        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
-        let ii1 = AngularInertia::<SimdFloat>::from(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1 = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
-        let ii2 = AngularInertia::<SimdFloat>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let ii2 = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
@@ -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]);
 
@@ -97,7 +97,7 @@ impl WBallPositionConstraint {
         };
 
         let inv_lhs = lhs.inverse_unchecked();
-        let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
+        let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
 
         position1.translation.vector += impulse * self.im1;
         position2.translation.vector -= impulse * self.im2;
@@ -120,11 +120,11 @@ impl WBallPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct WBallPositionGroundConstraint {
     position2: [usize; SIMD_WIDTH],
-    anchor1: Point<SimdFloat>,
-    im2: SimdFloat,
-    ii2: AngularInertia<SimdFloat>,
-    local_anchor2: Point<SimdFloat>,
-    local_com2: Point<SimdFloat>,
+    anchor1: Point<SimdReal>,
+    im2: SimdReal,
+    ii2: AngularInertia<SimdReal>,
+    local_anchor2: Point<SimdReal>,
+    local_com2: Point<SimdReal>,
 }
 
 impl WBallPositionGroundConstraint {
@@ -141,9 +141,9 @@ impl WBallPositionGroundConstraint {
             } else {
                 cparams[ii].local_anchor1
             }; SIMD_WIDTH]);
-        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
-        let ii2 = AngularInertia::<SimdFloat>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2 = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
@@ -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;
@@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint {
         };
 
         let inv_lhs = lhs.inverse_unchecked();
-        let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
+        let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
         position2.translation.vector -= impulse * self.im2;
 
         let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index 97ba244..e75f978 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::{AngularInertia, Real, SdpMatrix, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -12,16 +12,19 @@ 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>,
+    r1: Vector<Real>,
+    r2: Vector<Real>,
 
-    inv_lhs: SdpMatrix<f32>,
+    inv_lhs: SdpMatrix<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
+
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl BallVelocityConstraint {
@@ -37,8 +40,8 @@ impl BallVelocityConstraint {
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
 
         let rhs = -(vel1 - vel2);
         let lhs;
@@ -49,12 +52,12 @@ impl BallVelocityConstraint {
         #[cfg(feature = "dim3")]
         {
             lhs = rb2
-                .world_inv_inertia_sqrt
+                .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
                 .add_diagonal(im2)
                 + rb1
-                    .world_inv_inertia_sqrt
+                    .effective_world_inv_inertia_sqrt
                     .squared()
                     .quadform(&cmat1)
                     .add_diagonal(im1);
@@ -64,17 +67,14 @@ impl BallVelocityConstraint {
         // it's just easier that way.
         #[cfg(feature = "dim2")]
         {
-            let ii1 = rb1.world_inv_inertia_sqrt.squared();
-            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
             let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
             lhs = SdpMatrix::new(m11, m12, m22)
         }
 
-        let gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1);
-        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
-
         let inv_lhs = lhs.inverse_unchecked();
 
         BallVelocityConstraint {
@@ -84,42 +84,46 @@ impl BallVelocityConstraint {
             im1,
             im2,
             impulse: cparams.impulse * params.warmstart_coeff,
-            gcross1,
-            gcross2,
+            r1: anchor1,
+            r2: anchor2,
             rhs,
             inv_lhs,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
 
-    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];
 
         mj_lambda1.linear += self.im1 * self.impulse;
-        mj_lambda1.angular += self.gcross1.gcross(self.impulse);
+        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         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];
 
-        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
-        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
+        let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let dvel = -vel1 + vel2 + self.rhs;
 
         let impulse = self.inv_lhs * dvel;
         self.impulse += impulse;
 
         mj_lambda1.linear += self.im1 * impulse;
-        mj_lambda1.angular += self.gcross1.gcross(impulse);
+        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
 
         mj_lambda2.linear -= self.im2 * impulse;
-        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -137,11 +141,12 @@ 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>,
+    r2: Vector<Real>,
+    inv_lhs: SdpMatrix<Re