feat: start experimenting with a glam/bevy versionbevy-glam

29 files changed, 1676 insertions, 1670 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index cef9c9e..8198ed6 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -50,8 +50,8 @@ pub(crate) fn categorize_joints(
 ) {
     for joint_i in joint_indices {
         let joint = &impulse_joints[*joint_i].weight;
-        let rb1 = &bodies[joint.body1.0];
-        let rb2 = &bodies[joint.body2.0];
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
 
         if multibody_joints.rigid_body_link(joint.body1).is_some()
             || multibody_joints.rigid_body_link(joint.body2).is_some()
diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
index b5fdb6d..7fecef7 100644
--- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
+++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
@@ -12,17 +12,13 @@ use crate::dynamics::{
     RigidBodySet,
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::{Real, MAX_MANIFOLD_POINTS};
+use crate::math::*;
 use na::DVector;
-use parry::math::DIM;
 
 #[cfg(feature = "simd-is-enabled")]
-use {
-    crate::dynamics::solver::contact_constraint::{
-        OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd,
-        TwoBodyConstraintSimd,
-    },
-    crate::math::SIMD_WIDTH,
+use crate::dynamics::solver::contact_constraint::{
+    OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd,
+    TwoBodyConstraintSimd,
 };
 
 pub struct ConstraintsCounts {
@@ -174,6 +170,7 @@ impl ContactConstraintsSet {
             self.simd_compute_one_body_constraints(bodies, manifolds);
         }
         self.compute_one_body_constraints(bodies, manifolds);
+
         self.compute_generic_one_body_constraints(
             bodies,
             multibody_joints,
@@ -438,6 +435,7 @@ impl ContactConstraintsSet {
 
             curr_start += num_to_add;
         }
+
         assert_eq!(curr_start, total_num_constraints);
     }
 
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
index 7b1f8ea..3967a5e 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -1,9 +1,9 @@
 use crate::dynamics::solver::OneBodyConstraint;
 use crate::dynamics::{
-    IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity,
+    IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, Velocity,
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
+use crate::math::*;
 use crate::utils::SimdCross;
 
 use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
@@ -54,7 +54,7 @@ impl GenericOneBodyConstraintBuilder {
             let rb1 = &bodies[handle1];
             (rb1.vels, rb1.mprops.world_com)
         } else {
-            (RigidBodyVelocity::zero(), Point::origin())
+            (Velocity::zero(), Point::origin())
         };
 
         let rb1 = handle1
@@ -131,7 +131,7 @@ impl GenericOneBodyConstraintBuilder {
                             #[cfg(feature = "dim2")]
                             na::vector!(torque_dir2),
                             #[cfg(feature = "dim3")]
-                            torque_dir2,
+                            torque_dir2.into(),
                             jacobian_id,
                             jacobians,
                         )
@@ -141,26 +141,26 @@ impl GenericOneBodyConstraintBuilder {
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
 
-                    let proj_vel1 = vel1.dot(&force_dir1);
-                    let proj_vel2 = vel2.dot(&force_dir1);
+                    let proj_vel1 = vel1.dot(force_dir1);
+                    let proj_vel2 = vel2.dot(force_dir1);
                     let dvel = proj_vel1 - proj_vel2;
                     // NOTE: we add proj_vel1 since it’s not accessible through solver_vel.
                     normal_rhs_wo_bias =
                         proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel;
 
                     constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart {
-                        gcross2: na::zero(), // Unused for generic constraints.
-                        rhs: na::zero(),
-                        rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
-                        total_impulse: na::zero(),
+                        gcross2: Default::default(), // Unused for generic constraints.
+                        rhs: Default::default(),
+                        rhs_wo_bias: Default::default(),
+                        impulse: Default::default(),
+                        total_impulse: Default::default(),
                         r,
                     };
                 }
 
                 // Tangent parts.
                 {
-                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse = Default::default();
 
                     for j in 0..DIM - 1 {
                         let torque_dir2 = dp2.gcross(-tangents1[j]);
@@ -171,7 +171,7 @@ impl GenericOneBodyConstraintBuilder {
                                 #[cfg(feature = "dim2")]
                                 na::vector![torque_dir2],
                                 #[cfg(feature = "dim3")]
-                                torque_dir2,
+                                torque_dir2.into(),
                                 jacobian_id,
                                 jacobians,
                             )
@@ -181,7 +181,7 @@ impl GenericOneBodyConstraintBuilder {
 
                         let rhs_wo_bias = (vel1
                             + flipped_multiplier * manifold_point.tangent_velocity)
-                            .dot(&tangents1[j]);
+                            .dot(tangents1[j]);
 
                         constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
                         constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
index 1003a9a..1f7b088 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
@@ -1,7 +1,8 @@
 use crate::dynamics::solver::{
     OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart,
 };
-use crate::math::{Real, DIM};
+use crate::math::*;
+use crate::utils::SimdCapMagnitude;
 use na::DVector;
 #[cfg(feature = "dim2")]
 use na::SimdPartialOrd;
@@ -21,12 +22,12 @@ impl OneBodyConstraintTangentPart<Real> {
         {
             let dvel_0 = jacobians
                 .rows(j_id2, ndofs2)
-                .dot(&solver_vels.rows(solver_vel2, ndofs2))
+                .dot(solver_vels.rows(solver_vel2, ndofs2))
                 + self.rhs[0];
 
-            let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
-            let dlambda = new_impulse - self.impulse[0];
-            self.impulse[0] = new_impulse;
+            let new_impulse = (self.impulse - self.r[0] * dvel_0).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
 
             solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
                 dlambda,
@@ -40,18 +41,18 @@ impl OneBodyConstraintTangentPart<Real> {
             let j_step = ndofs2 * 2;
             let dvel_0 = jacobians
                 .rows(j_id2, ndofs2)
-                .dot(&solver_vels.rows(solver_vel2, ndofs2))
+                .dot(solver_vels.rows(solver_vel2, ndofs2))
                 + self.rhs[0];
             let dvel_1 = jacobians
                 .rows(j_id2 + j_step, ndofs2)
                 .dot(&solver_vels.rows(solver_vel2, ndofs2))
                 + self.rhs[1];
 
-            let new_impulse = na::Vector2::new(
+            let new_impulse = Vector2::new(
                 self.impulse[0] - self.r[0] * dvel_0,
                 self.impulse[1] - self.r[1] * dvel_1,
             );
-            let new_impulse = new_impulse.cap_magnitude(limit);
+            let new_impulse = new_impulse.simd_cap_magnitude(limit);
 
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
index 073f585..6215c94 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint};
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
+use crate::math::*;
 use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
 
 use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
@@ -23,8 +23,8 @@ pub(crate) struct GenericTwoBodyConstraintBuilder {
 impl GenericTwoBodyConstraintBuilder {
     pub fn invalid() -> Self {
         Self {
-            handle1: RigidBodyHandle::invalid(),
-            handle2: RigidBodyHandle::invalid(),
+            handle1: RigidBodyHandle::PLACEHOLDER,
+            handle2: RigidBodyHandle::PLACEHOLDER,
             ccd_thickness: Real::MAX,
             inner: TwoBodyConstraintBuilder::invalid(),
         }
@@ -102,12 +102,12 @@ impl GenericTwoBodyConstraintBuilder {
             constraint.inner.im1 = if type1.is_dynamic() {
                 mprops1.effective_inv_mass
             } else {
-                na::zero()
+                Default::default()
             };
             constraint.inner.im2 = if type2.is_dynamic() {
                 mprops2.effective_inv_mass
             } else {
-                na::zero()
+                Default::default()
             };
             constraint.inner.solver_vel1 = solver_vel1;
             constraint.inner.solver_vel2 = solver_vel2;
@@ -141,14 +141,14 @@ impl GenericTwoBodyConstraintBuilder {
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(torque_dir1)
                     } else {
-                        na::zero()
+                        Default::default()
                     };
                     let gcross2 = if type2.is_dynamic() {
                         mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(torque_dir2)
                     } else {
-                        na::zero()
+                        Default::default()
                     };
 
                     let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
@@ -158,13 +158,13 @@ impl GenericTwoBodyConstraintBuilder {
                             #[cfg(feature = "dim2")]
                             na::vector!(torque_dir1),
                             #[cfg(feature = "dim3")]
-                            torque_dir1,
+                            torque_dir1.into(),
                             jacobian_id,
                             jacobians,
                         )
                         .0
                     } else if type1.is_dynamic() {
-                        force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+                        force_dir1.dot(mprops1.effective_inv_mass.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
                     } else {
                         0.0
@@ -177,13 +177,13 @@ impl GenericTwoBodyConstraintBuilder {
                             #[cfg(feature = "dim2")]
                             na::vector!(torque_dir2),
                             #[cfg(feature = "dim3")]
-                            torque_dir2,
+                            torque_dir2.into(),
                             jacobian_id,
                             jacobians,
                         )
                         .0
                     } else if type2.is_dynamic() {
-                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                        force_dir1.dot(mprops2.effective_inv_mass.component_mul(&force_dir1))
                             + gcross2.gdot(gcross2)
                     } else {
                         0.0
@@ -194,22 +194,22 @@ impl GenericTwoBodyConstraintBuilder {
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
 
                     normal_rhs_wo_bias =
-                        (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
+                        (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(force_dir1);
 
                     constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs: na::zero(),
-                        rhs_wo_bias: na::zero(),
-                        total_impulse: na::zero(),
-                        impulse: na::zero(),
+                        rhs: Default::default(),
+                        rhs_wo_bias: Default::default(),
+                        total_impulse: Default::default(),
+                        impulse: Default::default(),
                         r,
                     };
                 }
 
                 // Tangent parts.
                 {
-                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse = Default::default();
 
                     for j in 0..DIM - 1 {
                         let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -218,7 +218,7 @@ impl GenericTwoBodyConstraintBuilder {
                                 .effective_world_inv_inertia_sqrt
                                 .transform_vector(torque_dir1)
                         } else {
-                            na::zero()
+                            Default::default()
                         };
                         constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1;
 
@@ -228,7 +228,7 @@ impl GenericTwoBodyConstraintBuilder {
                                 .effective_world_inv_inertia_sqrt
                                 .transform_vector(torque_dir2)
                         } else {
-                            na::zero()
+                            Default::default()
                         };
                         constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2;
 
@@ -239,13 +239,13 @@ impl GenericTwoBodyConstraintBuilder {
                                 #[cfg(feature = "dim2")]
                                 na::vector![torque_dir1],
                                 #[cfg(feature = "dim3")]
-                                torque_dir1,
+                                torque_dir1.into(),
                                 jacobian_id,
                                 jacobians,
                             )
                             .0
                         } else if type1.is_dynamic() {
-                            force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+                            force_dir1.dot(mprops1.effective_inv_mass.component_mul(&force_dir1))
                                 + gcross1.gdot(gcross1)
                         } else {
                             0.0
@@ -258,20 +258,20 @@ impl GenericTwoBodyConstraintBuilder {
                                 #[cfg(feature = "dim2")]
                                 na::vector![torque_dir2],
                                 #[cfg(feature = "dim3")]
-                                torque_dir2,
+                                torque_dir2.into(),
                                 jacobian_id,
                                 jacobians,
                             )
                             .0
                         } else if type2.is_dynamic() {
-                            force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                            force_dir1.dot(mprops2.effective_inv_mass.component_mul(&force_dir1))
                                 + gcross2.gdot(gcross2)
                         } else {
                             0.0
                         };
 
                         let r = crate::utils::inv(inv_r1 + inv_r2);
-                        let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
+                        let rhs_wo_bias = manifold_point.tangent_velocity.dot(tangents1[j]);
 
                         constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
                         constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
index 40740a0..f073bfb 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
@@ -2,8 +2,8 @@ use crate::dynamics::solver::SolverVel;
 use crate::dynamics::solver::{
     TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart,
 };
-use crate::math::{AngVector, Real, Vector, DIM};
-use crate::utils::SimdDot;
+use crate::math::*;
+use crate::utils::{SimdCapMagnitude, SimdDot};
 use na::DVector;
 #[cfg(feature = "dim2")]
 use {crate::utils::SimdBasis, na::SimdPartialOrd};
@@ -46,16 +46,16 @@ impl GenericRhs {
         j_id: usize,
         ndofs: usize,
         jacobians: &DVector<Real>,
-        dir: &Vector<Real>,
-        gcross: &AngVector<Real>,
+        dir: &Vector,
+        gcross: &AngVector,
         solver_vels: &DVector<Real>,
     ) -> Real {
         match self {
-            GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
+            GenericRhs::SolverVel(rhs) => dir.dot(rhs.linear) + gcross.gdot(rhs.angular),
             GenericRhs::GenericId(solver_vel) => {
                 let j = jacobians.rows(j_id, ndofs);
                 let rhs = solver_vels.rows(*solver_vel, ndofs);
-                j.dot(&rhs)
+                j.dot(rhs)
             }
         }
     }
@@ -67,15 +67,15 @@ impl GenericRhs {
         ndofs: usize,
         impulse: Real,
         jacobians: &DVector<Real>,
-        dir: &Vector<Real>,
-        gcross: &AngVector<Real>,
+        dir: &Vector,
+        gcross: &AngVector,
         solver_vels: &mut DVector<Real>,
-        inv_mass: &Vector<Real>,
+        inv_mass: &Vector,
     ) {
         match self {
             GenericRhs::SolverVel(rhs) => {
                 rhs.linear += dir.component_mul(inv_mass) * impulse;
-                rhs.angular += gcross * impulse;
+                rhs.angular += *gcross * impulse;
             }
             GenericRhs::GenericId(solver_vel) => {
                 let wj_id = j_id + ndofs;
@@ -93,9 +93,9 @@ impl TwoBodyConstraintTangentPart<Real> {
         &mut self,
         j_id: usize,
         jacobians: &DVector<Real>,
-        tangents1: [&Vector<Real>; DIM - 1],
-        im1: &Vector<Real>,
-        im2: &Vector<Real>,
+        tangents1: [&Vector; DIM - 1],
+        im1: &Vector,
+        im2: &Vector,
         ndofs1: usize,
         ndofs2: usize,
         limit: Real,
@@ -121,14 +121,14 @@ impl TwoBodyConstraintTangentPart<Real> {
                 j_id2,
                 ndofs2,
                 jacobians,
-                &-tangents1[0],
+                &-*tangents1[0],
                 &self.gcross2[0],
                 solver_vels,
             ) + self.rhs[0];
 
-            let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
-            let dlambda = new_impulse - self.impulse[0];
-            self.impulse[0] = new_impulse;
+            let new_impulse = (self.impulse - self.r[0] * dvel_0).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
 
             solver_vel1.apply_impulse(
                 j_id1,
@@ -145,7 +145,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 ndofs2,
                 dlambda,
                 jacobians,
-                &-tangents1[0],
+                &-*tangents1[0],
                 &self.gcross2[0],
                 solver_vels,
                 im2,
@@ -165,7 +165,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 j_id2,
                 ndofs2,
                 jacobians,
-                &-tangents1[0],
+                &-*tangents1[0],
                 &self.gcross2[0],
                 solver_vels,
             ) + self.rhs[0];
@@ -180,16 +180,16 @@ impl TwoBodyConstraintTangentPart<Real> {
                 j_id2 + j_step,
                 ndofs2,
                 jacobians,
-                &-tangents1[1],
+                &-*tangents1[1],
                 &self.gcross2[1],
                 solver_vels,
             ) + self.rhs[1];
 
-            let new_impulse = na::Vector2::new(
+            let new_impulse = Vector2::new(
                 self.impulse[0] - self.r[0] * dvel_0,
                 self.impulse[1] - self.r[1] * dvel_1,
             );
-            let new_impulse = new_impulse.cap_magnitude(limit);
+            let new_impulse = new_impulse.simd_cap_magnitude(limit);
 
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
@@ -220,7 +220,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 ndofs2,
                 dlambda[0],
                 jacobians,
-                &-tangents1[0],
+                &-*tangents1[0],
                 &self.gcross2[0],
                 solver_vels,
                 im2,
@@ -230,7 +230,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 ndofs2,
                 dlambda[1],
                 jacobians,
-                &-tangents1[1],
+                &-*tangents1[1],
                 &self.gcross2[1],
                 solver_vels,
                 im2,
@@ -246,9 +246,9 @@ impl TwoBodyConstraintNormalPart<Real> {
         cfm_factor: Real,
         j_id: usize,
         jacobians: &DVector<Real>,
-        dir1: &Vector<Real>,
-        im1: &Vector<Real>,
-        im2: &Vector<Real>,
+        dir1: &Vector,
+        im1: &Vector,
+        im2: &Vector,
         ndofs1: usize,
         ndofs2: usize,
         solver_vel1: &mut GenericRhs,
@@ -259,7 +259,14 @@ impl TwoBodyConstraintNormalPart<Real> {
         let j_id2 = j_id2(j_id, ndofs1, ndofs2);
 
         let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, dir1, &self.gcross1, solver_vels)
-            + solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels)
+            + solver_vel2.dvel(
+                j_id2,
+                ndofs2,
+                jacobians,
+                &-*dir1,
+                &self.gcross2,
+                solver_vels,
+            )
             + self.rhs;
 
         let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
@@ -281,7 +288,7 @@ impl TwoBodyConstraintNormalPart<Real> {
             ndofs2,
             dlambda,
             jacobians,
-            &-dir1,
+            &-*dir1,
             &self.gcross2,
             solver_vels,
             im2,
@@ -295,10 +302,10 @@ impl TwoBodyConstraintElement<Real> {
         cfm_factor: Real,
         elements: &mut [Self],
         jacobians: &DVector<Real>,
-        dir1: &Vector<Real>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
-        im1: &Vector<Real>,
-        im2: &Vector<Real>,
+        dir1: &Vector,
+        #[cfg(feature = "dim3")] tangent1: &Vector,
+        im1: &Vector,
+        im2: &Vector,
         limit: Real,
         // ndofs is 0 for a non-multibody body, or a multibody with zero
         // degrees of freedom.
@@ -339,7 +346,7 @@ impl TwoBodyConstraintElement<Real> {
         // Solve friction.
         if solve_friction {
             #[cfg(feature = "dim3")]
-            let tangents1 = [tangent1, &dir1.cross(tangent1)];
+            let tangents1 = [tangent1, &dir1.cross(*tangent1)];
             #[cfg(feature = "dim2")]
             let tangents1 = [&dir1.orthonormal_vector()];
             let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index be108a4..3c80238 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -1,5 +1,5 @@
 use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
-use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::math::*;
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
@@ -7,36 +7,24 @@ use parry::math::Isometry;
 
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::SolverVel;
-use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
+use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, Velocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 // TODO: move this struct somewhere else.
-#[derive(Copy, Clone, Debug)]
+#[derive(Copy, Clone, Debug, Default)]
 pub struct ContactPointInfos<N: SimdRealCopy> {
-    pub tangent_vel: Vector<N>,
-    pub local_p1: Point<N>,
-    pub local_p2: Point<N>,
+    pub tangent_vel: N::Vector,
+    pub local_p1: N::Point,
+    pub local_p2: N::Point,
     pub dist: N,
     pub normal_rhs_wo_bias: N,
 }
 
-impl<N: SimdRealCopy> Default for ContactPointInfos<N> {
-    fn default() -> Self {
-        Self {
-            tangent_vel: Vector::zeros(),
-            local_p1: Point::origin(),
-            local_p2: Point::origin(),
-            dist: N::zero(),
-            normal_rhs_wo_bias: N::zero(),
-        }
-    }
-}
-
 #[derive(Copy, Clone, Debug)]
 pub(crate) struct OneBodyConstraintBuilder {
     // PERF: only store what’s necessary for the bias updates instead of the complete solver body.
     pub rb1: SolverBody,
-    pub vels1: RigidBodyVelocity,
+    pub vels1: Velocity,
     pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
 }
 
@@ -44,7 +32,7 @@ impl OneBodyConstraintBuilder {
     pub fn invalid() -> Self {
         Self {
             rb1: SolverBody::default(),
-            vels1: RigidBodyVelocity::zero(),
+            vels1: Velocity::zero(),
             infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
         }
     }
@@ -71,7 +59,7 @@ impl OneBodyConstraintBuilder {
             let rb1 = &bodies[handle1];
             (rb1.vels, rb1.mprops.world_com)
         } else {
-            (RigidBodyVelocity::zero(), Point::origin())
+            (Velocity::zero(), Point::origin())
         };
 
         let rb1 = handle1
@@ -131,14 +119,14 @@ impl OneBodyConstraintBuilder {
                         .transform_vector(dp2.gcross(-force_dir1));
 
                     let projected_mass = utils::inv(
-                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                        force_dir1.dot(mprops2.effective_inv_mass.component_mul(&force_dir1))
                             + gcross2.gdot(gcross2),
                     );
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
 
-                    let proj_vel1 = vel1.dot(&force_dir1);
-