16 files changed, 327 insertions, 258 deletions

diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
index 2bb2f64..f5a0250 100644
--- a/src/dynamics/joint/fixed_joint.rs
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask};
-use crate::math::{Isometry, Point, Real};
+use crate::math::*;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -37,48 +37,48 @@ impl FixedJoint {
 
     /// The joint’s frame, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_frame1(&self) -> &Isometry<Real> {
+    pub fn local_frame1(&self) -> &Isometry {
         &self.data.local_frame1
     }
 
     /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
-    pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+    pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self {
         self.data.set_local_frame1(local_frame);
         self
     }
 
     /// The joint’s frame, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_frame2(&self) -> &Isometry<Real> {
+    pub fn local_frame2(&self) -> &Isometry {
         &self.data.local_frame2
     }
 
     /// Sets joint’s frame, expressed in the second rigid-body’s local-space.
-    pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+    pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self {
         self.data.set_local_frame2(local_frame);
         self
     }
 
     /// The joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
-    pub fn local_anchor1(&self) -> Point<Real> {
+    pub fn local_anchor1(&self) -> Point {
         self.data.local_anchor1()
     }
 
     /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
-    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+    pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self {
         self.data.set_local_anchor1(anchor1);
         self
     }
 
     /// The joint’s anchor, expressed in the local-space of the second rigid-body.
     #[must_use]
-    pub fn local_anchor2(&self) -> Point<Real> {
+    pub fn local_anchor2(&self) -> Point {
         self.data.local_anchor2()
     }
 
     /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
-    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+    pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self {
         self.data.set_local_anchor2(anchor2);
         self
     }
@@ -110,28 +110,28 @@ impl FixedJointBuilder {
 
     /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
+    pub fn local_frame1(mut self, local_frame: Isometry) -> Self {
         self.0.set_local_frame1(local_frame);
         self
     }
 
     /// Sets joint’s frame, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
+    pub fn local_frame2(mut self, local_frame: Isometry) -> Self {
         self.0.set_local_frame2(local_frame);
         self
     }
 
     /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
-    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+    pub fn local_anchor1(mut self, anchor1: Point) -> Self {
         self.0.set_local_anchor1(anchor1);
         self
     }
 
     /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
     #[must_use]
-    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+    pub fn local_anchor2(mut self, anchor2: Point) -> Self {
         self.0.set_local_anchor2(anchor2);
         self
     }
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index 76a7fe1..8773b2c 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -2,7 +2,7 @@
 
 use crate::dynamics::solver::MotorParameters;
 use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
-use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
+use crate::math::*;
 use crate::utils::{SimdBasis, SimdRealCopy};
 
 #[cfg(feature = "dim3")]
@@ -212,9 +212,9 @@ pub enum JointEnabled {
 /// A generic joint.
 pub struct GenericJoint {
     /// The joint’s frame, expressed in the first rigid-body’s local-space.
-    pub local_frame1: Isometry<Real>,
+    pub local_frame1: Isometry,
     /// The joint’s frame, expressed in the second rigid-body’s local-space.
-    pub local_frame2: Isometry<Real>,
+    pub local_frame2: Isometry,
     /// The degrees-of-freedoms locked by this joint.
     pub locked_axes: JointAxesMask,
     /// The degrees-of-freedoms limited by this joint.
@@ -280,23 +280,20 @@ impl GenericJoint {
     }
 
     #[doc(hidden)]
-    pub fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
+    pub fn complete_ang_frame(axis: UnitVector) -> Rotation {
         let basis = axis.orthonormal_basis();
 
         #[cfg(feature = "dim2")]
         {
-            use na::{Matrix2, Rotation2, UnitComplex};
-            let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]);
-            let rotmat = Rotation2::from_matrix_unchecked(mat);
-            UnitComplex::from_rotation_matrix(&rotmat)
+            let mat = Matrix::from_columns(&[axis.into_inner(), basis[0]]);
+            Rotation::from_matrix_unchecked(mat)
         }
 
         #[cfg(feature = "dim3")]
         {
-            use na::{Matrix3, Rotation3, UnitQuaternion};
-            let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
-            let rotmat = Rotation3::from_matrix_unchecked(mat);
-            UnitQuaternion::from_rotation_matrix(&rotmat)
+            let mat = Matrix::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
+            let rotmat = RotationMatrix::from_matrix_unchecked(mat);
+            Rotation::from_rotation_matrix(&rotmat)
         }
     }
 
@@ -328,62 +325,62 @@ impl GenericJoint {
     }
 
     /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
-    pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+    pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self {
         self.local_frame1 = local_frame;
         self
     }
 
     /// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
-    pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+    pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self {
         self.local_frame2 = local_frame;
         self
     }
 
     /// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_axis1(&self) -> UnitVector<Real> {
-        self.local_frame1 * Vector::x_axis()
+    pub fn local_axis1(&self) -> UnitVector {
+        self.local_frame1.rotation * Vector::x_axis()
     }
 
     /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
-    pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
+    pub fn set_local_axis1(&mut self, local_axis: UnitVector) -> &mut Self {
         self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
         self
     }
 
     /// The principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_axis2(&self) -> UnitVector<Real> {
-        self.local_frame2 * Vector::x_axis()
+    pub fn local_axis2(&self) -> UnitVector {
+        self.local_frame2.rotation * Vector::x_axis()
     }
 
     /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
-    pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
+    pub fn set_local_axis2(&mut self, local_axis: UnitVector) -> &mut Self {
         self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
         self
     }
 
     /// The anchor of this joint, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_anchor1(&self) -> Point<Real> {
-        self.local_frame1.translation.vector.into()
+    pub fn local_anchor1(&self) -> Point {
+        self.local_frame1.translation.into_vector().into()
     }
 
     /// Sets anchor of this joint, expressed in the first rigid-body’s local-space.
-    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
-        self.local_frame1.translation.vector = anchor1.coords;
+    pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self {
+        *self.local_frame1.translation.as_vector_mut() = anchor1.into_vector();
         self
     }
 
     /// The anchor of this joint, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_anchor2(&self) -> Point<Real> {
-        self.local_frame2.translation.vector.into()
+    pub fn local_anchor2(&self) -> Point {
+        self.local_frame2.translation.into_vector().into()
     }
 
     /// Sets anchor of this joint, expressed in the second rigid-body’s local-space.
-    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
-        self.local_frame2.translation.vector = anchor2.coords;
+    pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self {
+        *self.local_frame2.translation.as_vector_mut() = anchor2.into_vector();
         self
     }
 
@@ -589,42 +586,42 @@ impl GenericJointBuilder {
 
     /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
+    pub fn local_frame1(mut self, local_frame: Isometry) -> Self {
         self.0.set_local_frame1(local_frame);
         self
     }
 
     /// Sets the joint’s frame, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
+    pub fn local_frame2(mut self, local_frame: Isometry) -> Self {
         self.0.set_local_frame2(local_frame);
         self
     }
 
     /// Sets the principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
+    pub fn local_axis1(mut self, local_axis: UnitVector) -> Self {
         self.0.set_local_axis1(local_axis);
         self
     }
 
     /// Sets the principal (local X) axis of this joint, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
+    pub fn local_axis2(mut self, local_axis: UnitVector) -> Self {
         self.0.set_local_axis2(local_axis);
         self
     }
 
     /// Sets the anchor of this joint, expressed in the first rigid-body’s local-space.
     #[must_use]
-    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+    pub fn local_anchor1(mut self, anchor1: Point) -> Self {
         self.0.set_local_anchor1(anchor1);
         self
     }
 
     /// Sets the anchor of this joint, expressed in the second rigid-body’s local-space.
     #[must_use]
-    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+    pub fn local_anchor2(mut self, anchor2: Point) -> Self {
         self.0.set_local_anchor2(anchor2);
         self
     }
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs
index 8d35c35..2be6223 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle};
-use crate::math::{Real, SpacialVector};
+use crate::math::*;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone, Debug, PartialEq)]
@@ -14,7 +14,7 @@ pub struct ImpulseJoint {
     pub data: GenericJoint,
 
     /// The impulses applied by this joint.
-    pub impulses: SpacialVector<Real>,
+    pub impulses: SpatialVector<Real>,
 
     // A joint needs to know its handle to simplify its removal.
     pub(crate) handle: ImpulseJointHandle,
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 3f9835e..7d4e651 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -2,17 +2,30 @@ use super::ImpulseJoint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
 use crate::data::arena::Arena;
-use crate::data::Coarena;
+use crate::data::{Coarena, Index};
 use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet};
 
+#[cfg(feature = "bevy")]
+use bevy::prelude::{Component, Reflect, ReflectComponent};
+
 /// The unique identifier of a joint added to the joint set.
 /// The unique identifier of a collider added to a collider set.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[cfg(not(feature = "bevy"))]
 #[repr(transparent)]
 pub struct ImpulseJointHandle(pub crate::data::arena::Index);
 
+#[cfg(feature = "bevy")]
+pub type ImpulseJointHandle = bevy::prelude::Entity;
+
+#[cfg(not(feature = "bevy"))]
 impl ImpulseJointHandle {
+    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()
@@ -25,10 +38,21 @@ impl ImpulseJointHandle {
 
     /// An always-invalid joint handle.
     pub fn invalid() -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(
-            crate::INVALID_U32,
-            crate::INVALID_U32,
-        ))
+        Self::PLACEHOLDER
+    }
+}
+
+#[cfg(not(feature = "bevy"))]
+impl From<ImpulseJointHandle> for crate::data::arena::Index {
+    fn from(value: ImpulseJointHandle) -> Self {
+        value.0
+    }
+}
+
+#[cfg(not(feature = "bevy"))]
+impl From<Index> for ImpulseJointHandle {
+    fn from(value: Index) -> Self {
+        Self(value)
     }
 }
 
@@ -40,7 +64,10 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<ImpulseJoint>;
 /// A set of impulse_joints that can be handled by a physics `World`.
 pub struct ImpulseJointSet {
     rb_graph_ids: Coarena<RigidBodyGraphIndex>,
+    #[cfg(not(feature = "bevy"))]
     joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
+    #[cfg(feature = "bevy")]
+    joint_ids: crate::data::EntityArena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
     joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
     pub(crate) to_wake_up: Vec<RigidBodyHandle>, // A set of rigid-body handles to wake-up during the next timestep.
 }
@@ -48,12 +75,7 @@ pub struct ImpulseJointSet {
 impl ImpulseJointSet {
     /// Creates a new empty set of impulse_joints.
     pub fn new() -> Self {
-        Self {
-            rb_graph_ids: Coarena::new(),
-            joint_ids: Arena::new(),
-            joint_graph: InteractionGraph::new(),
-            to_wake_up: vec![],
-        }
+        Self::default()
     }
 
     /// The number of impulse_joints on this set.
@@ -78,8 +100,8 @@ impl ImpulseJointSet {
         body2: RigidBodyHandle,
     ) -> impl Iterator<Item = (ImpulseJointHandle, &ImpulseJoint)> {
         self.rb_graph_ids
-            .get(body1.0)
-            .zip(self.rb_graph_ids.get(body2.0))
+            .get(body1.into())
+            .zip(self.rb_graph_ids.get(body2.into()))
             .into_iter()
             .flat_map(move |(id1, id2)| self.joint_graph.interaction_pair(*id1, *id2).into_iter())
             .map(|inter| (inter.2.handle, inter.2))
@@ -98,7 +120,7 @@ impl ImpulseJointSet {
         ),
     > {
         self.rb_graph_ids
-            .get(body.0)
+            .get(body.into())
             .into_iter()
             .flat_map(move |id| self.joint_graph.interactions_with(*id))
             .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2))
@@ -110,11 +132,14 @@ impl ImpulseJointSet {
         body: RigidBodyHandle,
         mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint),
     ) {
-        self.rb_graph_ids.get(body.0).into_iter().for_each(|id| {
-            for inter in self.joint_graph.interactions_with_mut(*id) {
-                (f)(inter.0, inter.1, inter.3.handle, inter.3)
-            }
-        })
+        self.rb_graph_ids
+            .get(body.into())
+            .into_iter()
+            .for_each(|id| {
+                for inter in self.joint_graph.interactions_with_mut(*id) {
+                    (f)(inter.0, inter.1, inter.3.handle, inter.3)
+                }
+            })
     }
 
     /// Iterates through all the enabled impulse joints attached to the given rigid-body.
@@ -135,18 +160,18 @@ impl ImpulseJointSet {
 
     /// Is the given joint handle valid?
     pub fn contains(&self, handle: ImpulseJointHandle) -> bool {
-        self.joint_ids.contains(handle.0)
+        self.joint_ids.contains(handle.into())
     }
 
     /// Gets the joint with the given handle.
     pub fn get(&self, handle: ImpulseJointHandle) -> Option<&ImpulseJoint> {
-        let id = self.joint_ids.get(handle.0)?;
+        let id = self.joint_ids.get(handle.into())?;
         self.joint_graph.graph.edge_weight(*id)
     }
 
     /// Gets a mutable reference to the joint with the given handle.
     pub fn get_mut(&mut self, handle: ImpulseJointHandle) -> Option<&mut ImpulseJoint> {
-        let id = self.joint_ids.get(handle.0)?;
+        let id = self.joint_ids.get(handle.into())?;
         self.joint_graph.graph.edge_weight_mut(*id)
     }
 
@@ -159,6 +184,7 @@ impl ImpulseJointSet {
     ///
     /// 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<(&ImpulseJoint, ImpulseJointHandle)> {
         let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
         Some((
@@ -176,6 +202,7 @@ impl ImpulseJointSet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
+    #[cfg(not(feature = "bevy"))]
     pub fn get_unknown_gen_mut(
         &mut self,
         i: u32,
@@ -231,39 +258,45 @@ impl ImpulseJointSet {
     /// automatically woken up during the next timestep.
     pub fn insert(
         &mut self,
+        #[cfg(feature = "bevy")] handle: RigidBodyHandle,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
         data: impl Into<GenericJoint>,
         wake_up: bool,
     ) -> ImpulseJointHandle {
         let data = data.into();
+
+        #[cfg(not(feature = "bevy"))]
         let handle = self.joint_ids.insert(0.into());
+        #[cfg(feature = "bevy")]
+        self.joint_ids.insert(handle, 0.into());
+
         let joint = ImpulseJoint {
             body1,
             body2,
             data,
             impulses: na::zero(),
-            handle: ImpulseJointHandle(handle),
+            handle: ImpulseJointHandle::from(handle),
         };
 
         let default_id = InteractionGraph::<(), ()>::invalid_graph_index();
         let mut graph_index1 = *self
             .rb_graph_ids
-            .ensure_element_exist(joint.body1.0, default_id);
+            .ensure_element_exist(joint.body1.into(), default_id);
         let mut graph_index2 = *self
             .rb_graph_ids
-            .ensure_element_exist(joint.body2.0, default_id);
+            .ensure_element_exist(joint.body2.into(), default_id);
 
         // NOTE: the body won't have a graph index if it does not
         // have any joint attached.
         if !InteractionGraph::<RigidBodyHandle, ImpulseJoint>::is_graph_index_valid(graph_index1) {
             graph_index1 = self.joint_graph.graph.add_node(joint.body1);
-            self.rb_graph_ids.insert(joint.body1.0, graph_index1);
+            self.rb_graph_ids.insert(joint.body1.into(), graph_index1);
         }
 
         if !InteractionGraph::<RigidBodyHandle, ImpulseJoint>::is_graph_index_valid(graph_index2) {
             graph_index2 = self.joint_graph.graph.add_node(joint.body2);
-            self.rb_graph_ids.insert(joint.body2.0, graph_index2);
+            self.rb_graph_ids.insert(joint.body2.into(), graph_index2);
         }
 
         self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint);
@@ -273,7 +306,7 @@ impl ImpulseJointSet {
             self.to_wake_up.push(body2);
         }
 
-        ImpulseJointHandle(handle)
+        ImpulseJointHandle::from(handle)
     }
 
     /// Retrieve all the enabled impulse joints happening between two active bodies.
@@ -315,7 +348,7 @@ impl ImpulseJointSet {
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up.
     pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option<ImpulseJoint> {
-        let id = self.joint_ids.remove(handle.0)?;
+        let id = self.joint_ids.remove(handle.into())?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
         if wake_up {
@@ -330,7 +363,7 @@ impl ImpulseJointSet {
         let removed_joint = self.joint_graph.graph.remove_edge(id);
 
         if let Some(edge) = self.joint_graph.graph.edge_weight(id) {
-            self.joint_ids[edge.handle.0] = id;
+            self.joint_ids[edge.handle.into()] = id;
         }
 
         removed_joint
@@ -347,10 +380,10 @@ impl ImpulseJointSet {
     ) -> Vec<ImpulseJointHandle> {
         let mut deleted = vec![];
 
-        if let Some(deleted_id) = self
-            .rb_graph_ids
-            .remove(handle.0, InteractionGraph::<(), ()>::invalid_graph_index())
-        {
+        if let Some(deleted_id) = self.rb_graph_ids.remove(
+            handle.into(),
+            InteractionGraph::<(), ()>::invalid_graph_index(),
+        ) {
             if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) {
                 // We have to delete each joint one by one in order to:
                 // - Wake-up the attached bodies.
@@ -363,12 +396,12 @@ impl ImpulseJointSet {
                     .collect();
                 for (h1, h2, to_delete_handle) in to_delete {
                     deleted.push(to_delete_handle);
-                    let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).unwrap();
+                    let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.into()).unwrap();
                     self.joint_graph.graph.remove_edge(to_delete_edge_id);
 
                     // Update the id of the edge which took the place of the deleted one.
                     if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) {
-                        self.joint_ids[j.handle.0] = to_delete_edge_id;
+                        self.joint_ids[j.handle.into()] = to_delete_edge_id;
                     }
 
                     // Wake up the attached bodies.
@@ -379,7 +412,7 @@ impl ImpulseJointSet {
                 if let Some(other) = self.joint_graph.remove_node(deleted_id) {
                     // One rigid-body joint graph index may have been invalidated
                     // so we need to update it.
-                    self.rb_graph_ids.insert(other.0, deleted_id);
+                    self.rb_graph_ids.insert(other.into(), deleted_id);
                 }
             }
         }
diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs
index 74b8dd3..65e1181 100644
--- a/src/dynamics/joint/motor_model.rs
+++ b/src/dynamics/joint/motor_model.rs
@@ -1,4 +1,4 @@
-use crate::math::Real;
+use crate::math::*;
 
 /// The spring-like model used for constraints resolution.
 #[derive(Default, Copy, Clone, Debug, PartialEq, Eq)]
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);
-