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

6 files changed, 159 insertions, 117 deletions

diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index 617d447..753fa8f 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -1,11 +1,7 @@
 use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
 use super::multibody_workspace::MultibodyWorkspace;
-use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity};
-#[cfg(feature = "dim3")]
-use crate::math::Matrix;
-use crate::math::{
-    AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
-};
+use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, Velocity};
+use crate::math::*;
 use crate::prelude::MultibodyJoint;
 use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix};
 use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU};
@@ -13,12 +9,12 @@ use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMat
 #[repr(C)]
 #[derive(Copy, Clone, Debug, Default)]
 struct Force {
-    linear: Vector<Real>,
-    angular: AngVector<Real>,
+    linear: Vector,
+    angular: AngVector,
 }
 
 impl Force {
-    fn new(linear: Vector<Real>, angular: AngVector<Real>) -> Self {
+    fn new(linear: Vector, angular: AngVector) -> Self {
         Self { linear, angular }
     }
 
@@ -28,10 +24,7 @@ impl Force {
 }
 
 #[cfg(feature = "dim2")]
-fn concat_rb_mass_matrix(
-    mass: Vector<Real>,
-    inertia: Real,
-) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
+fn concat_rb_mass_matrix(mass: Vector, inertia: Real) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
     let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
     result[(0, 0)] = mass.x;
     result[(1, 1)] = mass.y;
@@ -40,17 +33,14 @@ fn concat_rb_mass_matrix(
 }
 
 #[cfg(feature = "dim3")]
-fn concat_rb_mass_matrix(
-    mass: Vector<Real>,
-    inertia: Matrix<Real>,
-) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
+fn concat_rb_mass_matrix(mass: Vector, inertia: Matrix) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
     let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
     result[(0, 0)] = mass.x;
     result[(1, 1)] = mass.y;
     result[(2, 2)] = mass.z;
     result
         .fixed_view_mut::<ANG_DIM, ANG_DIM>(DIM, DIM)
-        .copy_from(&inertia);
+        .copy_from(&inertia.into());
     result
 }
 
@@ -375,7 +365,7 @@ impl Multibody {
             let link = &self.links[i];
             let rb = &bodies[link.rigid_body];
 
-            let mut acc = RigidBodyVelocity::zero();
+            let mut acc = Velocity::zero();
 
             if i != 0 {
                 let parent_id = link.parent_internal_id;
@@ -388,7 +378,7 @@ impl Multibody {
                 acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel);
                 #[cfg(feature = "dim3")]
                 {
-                    acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel);
+                    acc.angvel += parent_rb.vels.angvel.cross(link.joint_velocity.angvel);
                 }
 
                 acc.linvel += parent_rb
@@ -411,7 +401,7 @@ impl Multibody {
 
             #[cfg(feature = "dim3")]
             {
-                gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel));
+                gyroscopic = rb.vels.angvel.cross(rb_inertia * rb.vels.angvel);
             }
             #[cfg(feature = "dim2")]
             {
@@ -500,7 +490,7 @@ impl Multibody {
                     let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
 
                     let shift_tr = (link.shift02).gcross_matrix_tr();
-                    link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
+                    link_j_v.gemm(1.0, &shift_tr.into(), &parent_j_w, 1.0);
                 }
             } else {
                 self.body_jacobians[i].fill(0.0);
@@ -522,7 +512,7 @@ impl Multibody {
                 let (mut link_j_v, link_j_w) =
                     link_j.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
                 let shift_tr = link.shift23.gcross_matrix_tr();
-                link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
+                link_j_v.gemm(1.0, &shift_tr.into(), &link_j_w, 1.0);
             }
         }
     }
@@ -608,27 +598,27 @@ impl Multibody {
 
                 // [c1 - c0].gcross() * (JDot + JDot/u * qdot)"
                 let shift_cross_tr = link.shift02.gcross_matrix_tr();
-                coriolis_v.gemm(1.0, &shift_cross_tr, parent_coriolis_w, 1.0);
+                coriolis_v.gemm(1.0, &shift_cross_tr.into(), parent_coriolis_w, 1.0);
 
                 // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
                 let dvel_cross = (rb.vels.angvel.gcross(link.shift02)
                     + 2.0 * link.joint_velocity.linvel)
                     .gcross_matrix_tr();
-                coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0);
+                coriolis_v.gemm(1.0, &dvel_cross.into(), &parent_j_w, 1.0);
 
                 // JDot/u * qdot
                 coriolis_v.gemm(
                     1.0,
-                    &link.joint_velocity.linvel.gcross_matrix_tr(),
+                    &link.joint_velocity.linvel.gcross_matrix_tr().into(),
                     &parent_j_w,
                     1.0,
                 );
-                coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr), &parent_j_w, 1.0);
+                coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr).into(), &parent_j_w, 1.0);
 
                 #[cfg(feature = "dim3")]
                 {
                     let vel_wrt_joint_w = link.joint_velocity.angvel.gcross_matrix();
-                    coriolis_w.gemm(-1.0, &vel_wrt_joint_w, &parent_j_w, 1.0);
+                    coriolis_w.gemm(-1.0, &vel_wrt_joint_w.into(), &parent_j_w, 1.0);
                 }
 
                 // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
@@ -644,13 +634,13 @@ impl Multibody {
                     );
 
                     let rb_joint_j_v = rb_joint_j.fixed_rows::<DIM>(0);
-                    coriolis_v_part.gemm(2.0, &parent_w, &rb_joint_j_v, 1.0);
+                    coriolis_v_part.gemm(2.0, &parent_w.into(), &rb_joint_j_v, 1.0);
 
                     #[cfg(feature = "dim3")]
                     {
                         let rb_joint_j_w = rb_joint_j.fixed_rows::<ANG_DIM>(DIM);
                         let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs);
-                        coriolis_w_part.gemm(1.0, &parent_w, &rb_joint_j_w, 1.0);
+                        coriolis_w_part.gemm(1.0, &parent_w.into(), &rb_joint_j_w, 1.0);
                     }
                 }
             } else {
@@ -664,16 +654,16 @@ impl Multibody {
             {
                 // [c3 - c2].gcross() * (JDot + JDot/u * qdot)
                 let shift_cross_tr = link.shift23.gcross_matrix_tr();
-                coriolis_v.gemm(1.0, &shift_cross_tr, coriolis_w, 1.0);
+                coriolis_v.gemm(1.0, &shift_cross_tr.into(), coriolis_w, 1.0);
 
                 // JDot
                 let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr();
-                coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0);
+                coriolis_v.gemm(1.0, &dvel_cross.into(), &rb_j_w, 1.0);
 
                 // JDot/u * qdot
                 coriolis_v.gemm(
                     1.0,
-                    &(rb.vels.angvel.gcross_matrix() * shift_cross_tr),
+                    &(rb.vels.angvel.gcross_matrix() * shift_cross_tr).into(),
                     &rb_j_w,
                     1.0,
                 );
@@ -690,7 +680,7 @@ impl Multibody {
                 i_coriolis_dt_v.copy_from(coriolis_v);
                 i_coriolis_dt_v
                     .column_iter_mut()
-                    .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt)));
+                    .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt).into()));
             }
 
             #[cfg(feature = "dim2")]
@@ -702,7 +692,7 @@ impl Multibody {
             #[cfg(feature = "dim3")]
             {
                 let mut i_coriolis_dt_w = self.i_coriolis_dt.fixed_rows_mut::<ANG_DIM>(DIM);
-                i_coriolis_dt_w.gemm(dt, &rb_inertia, coriolis_w, 0.0);
+                i_coriolis_dt_w.gemm(dt, &rb_inertia.into(), coriolis_w, 0.0);
             }
 
             self.acc_augmented_mass
@@ -856,10 +846,15 @@ impl Multibody {
             {
                 let parent_rb = &bodies[parent_link.rigid_body];
                 let link_rb = &bodies[link.rigid_body];
-                let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
-                let c2 = link.local_to_world
-                    * Point::from(link.joint.data.local_frame2.translation.vector);
-                let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
+                let c0 = parent_link
+                    .local_to_world
+                    .transform_point(&parent_rb.mprops.local_mprops.local_com);
+                let c2 = link.local_to_world.transform_point(&Point::from(
+                    link.joint.data.local_frame2.translation.into_vector(),
+                ));
+                let c3 = link
+                    .local_to_world
+                    .transform_point(&link_rb.mprops.local_mprops.local_com);
 
                 link.shift02 = c2 - c0;
                 link.shift23 = c3 - c2;
@@ -896,7 +891,7 @@ impl Multibody {
     pub(crate) fn fill_jacobians(
         &self,
         link_id: usize,
-        unit_force: Vector<Real>,
+        unit_force: Vector,
         unit_torque: SVector<Real, ANG_DIM>,
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
@@ -908,10 +903,7 @@ impl Multibody {
         let wj_id = *j_id + self.ndofs;
         let force = Force {
             linear: unit_force,
-            #[cfg(feature = "dim2")]
-            angular: unit_torque[0],
-            #[cfg(feature = "dim3")]
-            angular: unit_torque,
+            angular: unit_torque.into(),
         };
 
         let link = &self.links[link_id];
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs
index 11ea890..491bdf7 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs
@@ -1,12 +1,9 @@
 use crate::dynamics::solver::JointGenericOneBodyConstraint;
 use crate::dynamics::{
     joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
-    RigidBodyVelocity,
-};
-use crate::math::{
-    Isometry, JacobianViewMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM,
-    SPATIAL_DIM,
+    Velocity,
 };
+use crate::math::*;
 use na::{DVector, DVectorViewMut};
 #[cfg(feature = "dim3")]
 use na::{UnitQuaternion, Vector3};
@@ -17,8 +14,8 @@ use na::{UnitQuaternion, Vector3};
 pub struct MultibodyJoint {
     /// The joint’s description.
     pub data: GenericJoint,
-    pub(crate) coords: SpacialVector<Real>,
-    pub(crate) joint_rot: Rotation<Real>,
+    pub(crate) coords: SpatialVector<Real>,
+    pub(crate) joint_rot: Rotation,
 }
 
 impl MultibodyJoint {
@@ -31,24 +28,24 @@ impl MultibodyJoint {
         }
     }
 
-    pub(crate) fn free(pos: Isometry<Real>) -> Self {
+    pub(crate) fn free(pos: Isometry) -> Self {
         let mut result = Self::new(GenericJoint::default());
         result.set_free_pos(pos);
         result
     }
 
-    pub(crate) fn fixed(pos: Isometry<Real>) -> Self {
+    pub(crate) fn fixed(pos: Isometry) -> Self {
         Self::new(FixedJointBuilder::new().local_frame1(pos).build().into())
     }
 
-    pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
+    pub(crate) fn set_free_pos(&mut self, pos: Isometry) {
         self.coords
             .fixed_rows_mut::<DIM>(0)
-            .copy_from(&pos.translation.vector);
+            .copy_from(&pos.translation.into_vector().into());
         self.joint_rot = pos.rotation;
     }
 
-    // pub(crate) fn local_joint_rot(&self) -> &Rotation<Real> {
+    // pub(crate) fn local_joint_rot(&self) -> &Rotation {
     //     &self.joint_rot
     // }
 
@@ -63,7 +60,7 @@ impl MultibodyJoint {
     }
 
     /// The position of the multibody link containing this multibody_joint relative to its parent.
-    pub fn body_to_parent(&self) -> Isometry<Real> {
+    pub fn body_to_parent(&self) -> Isometry {
         let locked_bits = self.data.locked_axes.bits();
         let mut transform = self.joint_rot * self.data.local_frame2.inverse();
 
@@ -97,12 +94,12 @@ impl MultibodyJoint {
                 self.coords[DIM + dof_id] += vels[curr_free_dof] * dt;
                 #[cfg(feature = "dim2")]
                 {
-                    self.joint_rot = Rotation::new(self.coords[DIM + dof_id]);
+                    self.joint_rot = Rotation::from_scaled_axis(self.coords[DIM + dof_id].into());
                 }
                 #[cfg(feature = "dim3")]
                 {
                     self.joint_rot = Rotation::from_axis_angle(
-                        &Vector::ith_axis(dof_id),
+                        Vector::ith_axis(dof_id),
                         self.coords[DIM + dof_id],
                     );
                 }
@@ -112,8 +109,8 @@ impl MultibodyJoint {
             }
             #[cfg(feature = "dim3")]
             3 => {
-                let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
-                let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
+                let angvel = Vector::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
+                let disp = Rotation::new_eps(angvel * dt, 0.0);
                 self.joint_rot = disp * self.joint_rot;
                 self.coords[3] += angvel[0] * dt;
                 self.coords[4] += angvel[1] * dt;
@@ -129,15 +126,15 @@ impl MultibodyJoint {
     }
 
     /// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`.
-    pub fn jacobian(&self, transform: &Rotation<Real>, out: &mut JacobianViewMut<Real>) {
+    pub fn jacobian(&self, transform: &Rotation, out: &mut JacobianViewMut<Real>) {
         let locked_bits = self.data.locked_axes.bits();
         let mut curr_free_dof = 0;
 
         for i in 0..DIM {
             if (locked_bits & (1 << i)) == 0 {
-                let transformed_axis = transform * Vector::ith(i, 1.0);
+                let transformed_axis = *transform * Vector::ith(i, 1.0);
                 out.fixed_view_mut::<DIM, 1>(0, curr_free_dof)
-                    .copy_from(&transformed_axis);
+                    .copy_from(&transformed_axis.into());
                 curr_free_dof += 1;
             }
         }
@@ -157,7 +154,7 @@ impl MultibodyJoint {
                     let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
                     let rotmat = transform.to_rotation_matrix().into_inner();
                     out.fixed_view_mut::<ANG_DIM, 1>(DIM, curr_free_dof)
-                        .copy_from(&rotmat.column(dof_id));
+                        .copy_from(&na::Vector3::from(rotmat.column(dof_id)));
                 }
             }
             2 => {
@@ -167,7 +164,7 @@ impl MultibodyJoint {
             3 => {
                 let rotmat = transform.to_rotation_matrix();
                 out.fixed_view_mut::<3, 3>(3, curr_free_dof)
-                    .copy_from(rotmat.matrix());
+                    .copy_from(&rotmat.into_inner().into());
             }
             _ => unreachable!(),
         }
@@ -175,9 +172,9 @@ impl MultibodyJoint {
 
     /// Multiply the multibody_joint jacobian by generalized velocities to obtain the
     /// relative velocity of the multibody link containing this multibody_joint.
-    pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
+    pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> Velocity {
         let locked_bits = self.data.locked_axes.bits();
-        let mut result = RigidBodyVelocity::zero();
+        let mut result = Velocity::zero();
         let mut curr_free_dof = 0;
 
         for i in 0..DIM {
@@ -207,7 +204,7 @@ impl MultibodyJoint {
             }
             #[cfg(feature = "dim3")]
             3 => {
-                let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
+                let angvel = Vector::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
                 result.angvel += angvel;
             }
             _ => unreachable!(),
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index fa0ffdb..e8d957a 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -1,22 +1,57 @@
 use crate::data::{Arena, Coarena, Index};
 use crate::dynamics::joint::MultibodyLink;
 use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle};
-use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
+use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
 use crate::parry::partitioning::IndexedData;
 
+#[cfg(feature = "bevy")]
+use bevy::prelude::{Component, Reflect, ReflectComponent};
+
 /// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[cfg_attr(feature = "bevy", derive(Component))]
+#[cfg(not(feature = "bevy"))]
 #[repr(transparent)]
 pub struct MultibodyJointHandle(pub Index);
 
+#[cfg(feature = "bevy")]
+pub type MultibodyJointHandle = bevy::prelude::Entity;
+
+#[cfg(not(feature = "bevy"))]
+impl From<MultibodyJointHandle> for crate::data::arena::Index {
+    fn from(value: MultibodyJointHandle) -> Self {
+        value.0
+    }
+}
+
+#[cfg(not(feature = "bevy"))]
+impl From<Index> for MultibodyJointHandle {
+    fn from(value: Index) -> Self {
+        Self(value)
+    }
+}
+
+#[cfg(not(feature = "bevy"))]
+impl From<RigidBodyHandle> for MultibodyJointHandle {
+    fn from(value: RigidBodyHandle) -> Self {
+        Self(value.0)
+    }
+}
+
 /// The temporary index of a multibody added to a `MultibodyJointSet`.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[repr(transparent)]
 pub struct MultibodyIndex(pub Index);
 
+#[cfg(not(feature = "bevy"))]
 impl MultibodyJointHandle {
+    pub const PLACEHOLDER: Self = Self(Index::from_raw_parts(
+        crate::INVALID_U32,
+        crate::INVALID_U32,
+    ));
+
     /// Converts this handle into its (index, generation) components.
     pub fn into_raw_parts(self) -> (u32, u32) {
         self.0.into_raw_parts()
@@ -36,12 +71,14 @@ impl MultibodyJointHandle {
     }
 }
 
+#[cfg(not(feature = "bevy"))]
 impl Default for MultibodyJointHandle {
     fn default() -> Self {
         Self::invalid()
     }
 }
 
+#[cfg(not(feature = "bevy"))]
 impl IndexedData for MultibodyJointHandle {
     fn default() -> Self {
         Self(IndexedData::default())
@@ -51,6 +88,13 @@ impl IndexedData for MultibodyJointHandle {
     }
 }
 
+#[cfg(not(feature = "bevy"))]
+impl From<MultibodyJointHandle> for RigidBodyHandle {
+    fn from(value: MultibodyJointHandle) -> Self {
+        RigidBodyHandle(value.0)
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 /// Indexes usable to get a multibody link from a `MultibodyJointSet`.
@@ -123,7 +167,12 @@ impl MultibodyJointSet {
             .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
             .map(|(h, link)| {
                 let mb = &self.multibodies[link.multibody.0];
-                (MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap())
+                (
+                    MultibodyJointHandle::from(h),
+                    link,
+                    mb,
+                    mb.link(link.id).unwrap(),
+                )
             })
     }
 
@@ -136,7 +185,7 @@ impl MultibodyJointSet {
         wake_up: bool,
     ) -> Option<MultibodyJointHandle> {
         let data = data.into();
-        let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
+        let link1 = self.rb2mb.get(body1.into()).copied().unwrap_or_else(|| {
             let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
             MultibodyLinkId {
                 graph_id: self.connectivity_graph.graph.add_node(body1),
@@ -145,7 +194,7 @@ impl MultibodyJointSet {
             }
         });
 
-        let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
+        let link2 = self.rb2mb.get(body2.into()).copied().unwrap_or_else(|| {
             let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
             MultibodyLinkId {
                 graph_id: self.connectivity_graph.graph.add_node(body2),
@@ -162,14 +211,14 @@ impl MultibodyJointSet {
         self.connectivity_graph
             .graph
             .add_edge(link1.graph_id, link2.graph_id, ());
-        self.rb2mb.insert(body1.0, link1);
-        self.rb2mb.insert(body2.0, link2);
+        self.rb2mb.insert(body1.into(), link1);
+        self.rb2mb.insert(body2.into(), link2);
 
         let mb2 = self.multibodies.remove(link2.multibody.0).unwrap();
         let multibody1 = &mut self.multibodies[link1.multibody.0];
 
         for mb_link2 in mb2.links() {
-            let link = self.rb2mb.get_mut(mb_link2.rigid_body.0).unwrap();
+            let link = self.rb2mb.get_mut(mb_link2.rigid_body.into()).unwrap();
             link.multibody = link1.multibody;
             link.id += multibody1.num_links();
         }
@@ -184,24 +233,24 @@ impl MultibodyJointSet {
         // Because each rigid-body can only have one parent link,
         // we can use the second rigid-body’s handle as the multibody_joint’s
         // handle.
-        Some(MultibodyJointHandle(body2.0))
+        Some(MultibodyJointHandle::from(body2))
     }
 
     /// Removes an multibody_joint from this set.
     pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) {
-        if let Some(removed) = self.rb2mb.get(handle.0).copied() {
+        if let Some(removed) = self.rb2mb.get(handle.into()).copied() {
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
 
             // Remove the edge from the connectivity graph.
             if let Some(parent_link) = multibody.link(removed.id).unwrap().parent_id() {
                 let parent_rb = multibody.link(parent_link).unwrap().rigid_body;
                 self.connectivity_graph.remove_edge(
-                    self.rb2mb.get(parent_rb.0).unwrap().graph_id,
+                    self.rb2mb.get(parent_rb.into()).unwrap().graph_id,
                     removed.graph_id,
                 );
 
                 if wake_up {
-                    self.to_wake_up.push(RigidBodyHandle(handle.0));
+                    self.to_wake_up.push(RigidBodyHandle::from(handle));
                     self.to_wake_up.push(parent_rb);
                 }
 
@@ -215,12 +264,12 @@ impl MultibodyJointSet {
                     if multibody.num_links() == 1 {
                         // We don’t have any multibody_joint attached to this body, remove it.
                         if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
-                            self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
+                            self.rb2mb.get_mut(other.into()).unwrap().graph_id = removed.graph_id;
                         }
                     } else {
                         let mb_id = self.multibodies.insert(multibody);
                         for link in self.multibodies[mb_id].links() {
-                            let ids = self.rb2mb.get_mut(link.rigid_body.0).unwrap();
+                            let ids = self.rb2mb.get_mut(link.rigid_body.into()).unwrap();
                             ids.multibody = MultibodyIndex(mb_id);
                             ids.id = link.internal_id;
                         }
@@ -232,7 +281,7 @@ impl MultibodyJointSet {
 
     /// Removes all the multibody_joints from the multibody the given rigid-body is part of.
     pub fn remove_multibody_articulations(&mut self, handle: RigidBodyHandle, wake_up: bool) {
-        if let Some(removed) = self.rb2mb.get(handle.0).copied() {
+        if let Some(removed) = self.rb2mb.get(handle.into()).copied() {
             // Remove the multibody.
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
             for link in multibody.links() {
@@ -243,10 +292,13 @@ impl MultibodyJointSet {
                 }
 
                 // Remove the rigid-body <-> multibody mapping for this link.
-                let removed = self.rb2mb.remove(rb_handle.0, Default::default()).unwrap();
+                let removed = self
+                    .rb2mb
+                    .remove(rb_handle.into(), Default::default())
+                    .unwrap();
                 // Remove the node (and all it’s edges) from the connectivity graph.
                 if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
-                    self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
+                    self.rb2mb.get_mut(other.into()).unwrap().graph_id = removed.graph_id;
                 }
             }
         }
@@ -255,14 +307,14 @@ impl MultibodyJointSet {
     /// Removes all the multibody joints attached to a rigid-body.
     pub fn remove_joints_attached_to_rigid_body(&mut self, rb_to_remove: RigidBodyHandle) {
         // TODO: optimize this.
-        if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
+        if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.into()).copied() {
             let mut articulations_to_remove = vec![];
             for (rb1, rb2, _) in self
                 .connectivity_graph
                 .interactions_with(link_to_remove.graph_id)
             {
                 // There is a multibody_joint handle is equal to the second rigid-body’s handle.
-                articulations_to_remove.push(MultibodyJointHandle(rb2.0));
+                articulations_to_remove.push(MultibodyJointHandle::from(rb2));
 
                 self.to_wake_up.push(rb1);
                 self.to_wake_up.push(rb2);
@@ -278,7 +330,7 @@ impl MultibodyJointSet {
     ///
     /// Returns `None` if `rb` isn’t part of any rigid-body.
     pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> {
-        self.rb2mb.get(rb.0)
+        self.rb2mb.get(rb.into())
     }
 
     /// Gets a reference to a multibody, based on its temporary index.
@@ -296,14 +348,14 @@ impl MultibodyJointSet {
 
     /// Gets a reference to the multibody identified by its `handle`.
     pub fn get(&self, handle: MultibodyJointHandle) -> Option<(&Multibody, usize)> {
-        let link = self.rb2mb.get(handle.0)?;
+        let link = self.rb2mb.get(handle.into())?;
         let multibody = self.multibodies.get(link.multibody.0)?;
         Some((multibody, link.id))
     }
 
     /// Gets a mutable reference to the multibody identified by its `handle`.
     pub fn get_mut(&mut self, handle: MultibodyJointHandle) -> Option<(&mut Multibody, usize)> {
-        let link = self.rb2mb.get(handle.0)?;
+        let link = self.rb2mb.get(handle.into())?;
         let multibody = self.multibodies.get_mut(link.multibody.0)?;
         Some((multibody, link.id))
     }
@@ -316,7 +368,7 @@ impl MultibodyJointSet {
         handle: MultibodyJointHandle,
     ) -> Option<(&mut Multibody, usize)> {
         // TODO: modification tracking?
-        let link = self.rb2mb.get(handle.0)?;
+        let link = self.rb2mb.get(handle.into())?;
         let multibody = self.multibodies.get_mut(link.multibody.0)?;
         Some((multibody, link.id))
     }
@@ -330,6 +382,7 @@ impl MultibodyJointSet {
     ///
     /// Using this is discouraged in favor of `self.get(handle)` which does not
     /// suffer form the ABA problem.
+    #[cfg(not(feature = "bevy"))]
     pub fn get_unknown_gen(&self, i: u32) -> Option<(&Multibody, usize, MultibodyJointHandle)> {
         let link = self.rb2mb.get_unknown_gen(i)?;
         let gen = self.rb2mb.get_gen(i)?;
@@ -347,8 +400,8 @@ impl MultibodyJointSet {
         rb1: RigidBodyHandle,
         rb2: RigidBodyHandle,
     ) -> Option<(MultibodyJointHandle, &Multibody, &MultibodyLink)> {
-        let id1 = self.rb2mb.get(rb1.0)?;
-        let id2 = self.rb2mb.get(rb2.0)?;
+        let id1 = self.rb2mb.get(rb1.into())?;
+        let id2 = self.rb2mb.get(rb2.into())?;
 
         // Both bodies must be part of the same multibody.
         if id1.multibody != id2.multibody {
@@ -363,13 +416,13 @@ impl MultibodyJointSet {
         let parent1 = link1.parent_id();
 
         if parent1 == Some(id2.id) {
-            Some((MultibodyJointHandle(rb1.0), mb, &link1))
+            Some((MultibodyJointHandle::from(rb1), mb, &link1))
         } else {
             let link2 = mb.link(id2.id)?;
             let parent2 = link2.parent_id();
 
             if parent2 == Some(id1.id) {
-                Some((MultibodyJointHandle(rb2.0), mb, &link2))
+                Some((MultibodyJointHandle::from(rb2), mb, &link2))
             } else {
                 None
             }
@@ -382,12 +435,12 @@ impl MultibodyJointSet {
         rb: RigidBodyHandle,
     ) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyHandle, MultibodyJointHandle)> + '_ {
         self.rb2mb
-            .get(rb.0)
+            .get(rb.into())
             .into_iter()
             .flat_map(move |link| self.connectivity_graph.interactions_with(link.graph_id))
             .map(|inter| {
                 // NOTE: the joint handle is always equal to the handle of the second rigid-body.
-                (inter.0, inter.1, MultibodyJointHandle(inter.1 .0))
+                (inter.0, inter.1, MultibodyJointHandle::from(inter.1))
             })
     }
 
@@ -398,7 +451,7 @@ impl MultibodyJointSet {
         body: RigidBodyHandle,
     ) -> impl Iterator<Item = RigidBodyHandle> + '_ {
         self.rb2mb
-            .get(body.0)
+            .get(body.into())
             .into_iter()
             .flat_map(move |id| self.connectivity_graph.interactions_with(id.graph_id))
             .map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs
index 7336a6c..932120b 100644
--- a/src/dynamics/joint/multibody_joint/multibody_link.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_link.rs
@@ -1,8 +1,8 @@
 use std::ops::{Deref, DerefMut};
 
 use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
-use crate::math::{Isometry, Real, Vector};
-use crate::prelude::RigidBodyVelocity;
+use crate::math::*;
+use crate::prelude::Velocity;
 
 /// One link of a multibody.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -21,13 +21,13 @@ pub struct MultibodyLink {
     /// The multibody joint of this link.
     pub joint: MultibodyJoint,
     // TODO: should this be removed in favor of the rigid-body position?
-    pub(crate) local_to_world: Isometry<Real>,
-    pub(crate) local_to_parent: Isometry<Real>,
-    pub(crate) shift02: Vector<Real>,
-    pub(crate) shift23: Vector<Real>,
+    pub(crate) local_to_world: Isometry,
+    pub(crate) local_to_parent: Isometry,
+    pub(crate) shift02: Vector,
+    pub(crate) shift23: Vector,
 
     /// The velocity added by the joint, in world-space.
-    pub(crate) joint_velocity: RigidBodyVelocity,
+    pub(crate) joint_velocity: Velocity,
 }
 
 impl MultibodyLink {
@@ -38,10 +38,10 @@ impl MultibodyLink {
         assembly_id: usize,
         parent_internal_id: usize,
         joint: MultibodyJoint,
-        local_to_world: Isometry<Real>,
-        local_to_parent: Isometry<Real>,
+        local_to_world: Isometry,
+        local_to_parent: Isometry,
     ) -> Self {
-        let joint_velocity = RigidBodyVelocity::zero();
+        let joint_velocity = Velocity::zero();
 
         MultibodyLink {
             internal_id,
@@ -50,8 +50,8 @@ impl MultibodyLink {
             joint,
             local_to_world,
             local_to_parent,
-            shift02: na::zero(),
-            shift23: na::zero(),
+            shift02: Default::default(),
+            shift23: Default::default(),
             joint_velocity,
             rigid_body,
         }
@@ -91,13 +91,13 @@ impl MultibodyLink {
 
     /// The world-space transform of the rigid-body attached to this link.
     #[inline]
-    pub fn local_to_world(&self) -> &Isometry<Real> {
+    pub fn local_to_world(&self) -> &Isometry {
         &self.local_to_world
     }
 
     /// The position of the rigid-body attached to this link relative to its parent.
     #[inline]
-    pub fn local_to_parent(&self) -> &Isometry<Real> {
+    pub fn local_to_parent(&self) -> &Isometry {
         &self.local_to_parent
     }
 }
diff --git a/src/dynamics/joint/multibody_joint/multibody_workspace.rs b/src/dynamics/joint/multibody_joint/multibody_workspace.rs
index 767d751..5732329 100644
--- a/src/dynamics/joint/multibody_joint/multibody_workspace.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_workspace.rs
@@ -1,12 +1,12 @@
-use crate::dynamics::RigidBodyVelocity;
-use crate::math::Real;
+use crate::dynamics::Velocity;
+use crate::math::*;
 use na::DVector;
 
 /// A temporary workspace for various updates of the multibody.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 pub(crate) struct MultibodyWorkspace {
-    pub accs: Vec<RigidBodyVelocity>,
+    pub accs: Vec<Velocity>,
     pub ndofs_vec: DVector<Real>,
 }
 
@@ -21,7 +21,7 @@ impl MultibodyWorkspace {
 
     /// Resize the workspace so it is enough for `nlinks` links.
     pub fn resize(&mut self, nlinks: usize, ndofs: usize) {
-        self.accs.resize(nlinks, RigidBodyVelocity::zero());
+        self.accs.resize(nlinks, Velocity::zero());
         self.ndofs_vec = DVector::zeros(ndofs)
     }
 }
diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
index d6efd12..e46024f 100644
--- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
+++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
@@ -3,7 +3,7 @@
 use crate::dynamics::joint::MultibodyLink;
 use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId};
 use crate