Merge pull request #79 from dimforge/split_geom

Move most of the geometric code to another crate.

5 files changed, 105 insertions, 80 deletions

diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index ec255d4..82e2a10 100644
--- a/src/dynamics/joint/ball_joint.rs
+++ b/src/dynamics/joint/ball_joint.rs
@@ -1,29 +1,29 @@
-use crate::math::{Point, Vector};
+use crate::math::{Point, Real, Vector};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A joint that removes all relative linear motion between a pair of points on two bodies.
 pub struct BallJoint {
     /// Where the ball joint is attached on the first body, expressed in the first body local frame.
-    pub local_anchor1: Point<f32>,
+    pub local_anchor1: Point<Real>,
     /// Where the ball joint is attached on the first body, expressed in the first body local frame.
-    pub local_anchor2: Point<f32>,
+    pub local_anchor2: Point<Real>,
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector<f32>,
+    pub impulse: Vector<Real>,
 }
 
 impl BallJoint {
     /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
-    pub fn new(local_anchor1: Point<f32>, local_anchor2: Point<f32>) -> Self {
+    pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
         Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
     }
 
     pub(crate) fn with_impulse(
-        local_anchor1: Point<f32>,
-        local_anchor2: Point<f32>,
-        impulse: Vector<f32>,
+        local_anchor1: Point<Real>,
+        local_anchor2: Point<Real>,
+        impulse: Vector<Real>,
     ) -> Self {
         Self {
             local_anchor1,
diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
index 0731cfb..359e14a 100644
--- a/src/dynamics/joint/fixed_joint.rs
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -1,4 +1,4 @@
-use crate::math::{Isometry, SpacialVector};
+use crate::math::{Isometry, Real, SpacialVector};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -8,22 +8,22 @@ use crate::math::{Isometry, SpacialVector};
 pub struct FixedJoint {
     /// The frame of reference for the first body affected by this joint, expressed in the local frame
     /// of the first body.
-    pub local_anchor1: Isometry<f32>,
+    pub local_anchor1: Isometry<Real>,
     /// The frame of reference for the second body affected by this joint, expressed in the local frame
     /// of the first body.
-    pub local_anchor2: Isometry<f32>,
+    pub local_anchor2: Isometry<Real>,
     /// The impulse applied to the first body affected by this joint.
     ///
     /// The impulse applied to the second body affected by this joint is given by `-impulse`.
     /// This combines both linear and angular impulses:
     /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
     /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
-    pub impulse: SpacialVector<f32>,
+    pub impulse: SpacialVector<Real>,
 }
 
 impl FixedJoint {
     /// Creates a new fixed joint from the frames of reference of both bodies.
-    pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self {
+    pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
         Self {
             local_anchor1,
             local_anchor2,
diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs
index 5144d97..a87532a 100644
--- a/src/dynamics/joint/joint_set.rs
+++ b/src/dynamics/joint/joint_set.rs
@@ -1,11 +1,36 @@
 use super::Joint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
-use crate::data::arena::{Arena, Index};
+use crate::data::arena::Arena;
 use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
 
 /// The unique identifier of a joint added to the joint set.
-pub type JointHandle = Index;
+/// 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))]
+#[repr(transparent)]
+pub struct JointHandle(pub(crate) crate::data::arena::Index);
+
+impl JointHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (usize, u64) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid joint handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_USIZE,
+            crate::INVALID_U64,
+        ))
+    }
+}
+
 pub(crate) type JointIndex = usize;
 pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
 
@@ -13,7 +38,7 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
 /// A set of joints that can be handled by a physics `World`.
 pub struct JointSet {
     joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
-    joint_graph: InteractionGraph<Joint>,
+    joint_graph: InteractionGraph<RigidBodyHandle, Joint>,
 }
 
 impl JointSet {
@@ -25,29 +50,24 @@ impl JointSet {
         }
     }
 
-    /// An always-invalid joint handle.
-    pub fn invalid_handle() -> JointHandle {
-        JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
-    }
-
     /// The number of joints on this set.
     pub fn len(&self) -> usize {
         self.joint_graph.graph.edges.len()
     }
 
     /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
-    pub fn joint_graph(&self) -> &InteractionGraph<Joint> {
+    pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, Joint> {
         &self.joint_graph
     }
 
     /// Is the given joint handle valid?
     pub fn contains(&self, handle: JointHandle) -> bool {
-        self.joint_ids.contains(handle)
+        self.joint_ids.contains(handle.0)
     }
 
     /// Gets the joint with the given handle.
     pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
-        let id = self.joint_ids.get(handle)?;
+        let id = self.joint_ids.get(handle.0)?;
         self.joint_graph.graph.edge_weight(*id)
     }
 
@@ -62,7 +82,10 @@ impl JointSet {
     /// suffer form the ABA problem.
     pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> {
         let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
-        Some((self.joint_graph.graph.edge_weight(*id)?, handle))
+        Some((
+            self.joint_graph.graph.edge_weight(*id)?,
+            JointHandle(handle),
+        ))
     }
 
     /// Iterates through all the joint on this set.
@@ -117,7 +140,7 @@ impl JointSet {
         let joint = Joint {
             body1,
             body2,
-            handle,
+            handle: JointHandle(handle),
             #[cfg(feature = "parallel")]
             constraint_index: 0,
             #[cfg(feature = "parallel")]
@@ -133,11 +156,13 @@ impl JointSet {
 
         // NOTE: the body won't have a graph index if it does not
         // have any joint attached.
-        if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) {
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index)
+        {
             rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
         }
 
-        if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) {
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index)
+        {
             rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
         }
 
@@ -146,7 +171,7 @@ impl JointSet {
             .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
 
         self.joint_ids[handle] = id;
-        handle
+        JointHandle(handle)
     }
 
     /// Retrieve all the joints happening between two active bodies.
@@ -191,7 +216,7 @@ impl JointSet {
         bodies: &mut RigidBodySet,
         wake_up: bool,
     ) -> Option<Joint> {
-        let id = self.joint_ids.remove(handle)?;
+        let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
         if wake_up {
@@ -207,7 +232,7 @@ impl JointSet {
         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] = id;
+            self.joint_ids[edge.handle.0] = id;
         }
 
         removed_joint
@@ -218,7 +243,7 @@ impl JointSet {
         deleted_id: RigidBodyGraphIndex,
         bodies: &mut RigidBodySet,
     ) {
-        if InteractionGraph::<()>::is_graph_index_valid(deleted_id) {
+        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.
             // - Update our Handle -> graph edge mapping.
@@ -229,12 +254,12 @@ impl JointSet {
                 .map(|e| (e.0, e.1, e.2.handle))
                 .collect();
             for (h1, h2, to_delete_handle) in to_delete {
-                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap();
+                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).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] = to_delete_edge_id;
+                    self.joint_ids[j.handle.0] = to_delete_edge_id;
                 }
 
                 // Wake up the attached bodies.
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index a6fd558..174ce79 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -1,4 +1,4 @@
-use crate::math::{Isometry, Point, Vector, DIM};
+use crate::math::{Isometry, Point, Real, Vector, DIM};
 use crate::utils::WBasis;
 use na::Unit;
 #[cfg(feature = "dim2")]
@@ -11,35 +11,35 @@ use na::Vector5;
 /// A joint that removes all relative motion between two bodies, except for the translations along one axis.
 pub struct PrismaticJoint {
     /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body.
-    pub local_anchor1: Point<f32>,
+    pub local_anchor1: Point<Real>,
     /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
-    pub local_anchor2: Point<f32>,
-    pub(crate) local_axis1: Unit<Vector<f32>>,
-    pub(crate) local_axis2: Unit<Vector<f32>>,
-    pub(crate) basis1: [Vector<f32>; DIM - 1],
-    pub(crate) basis2: [Vector<f32>; DIM - 1],
+    pub local_anchor2: Point<Real>,
+    pub(crate) local_axis1: Unit<Vector<Real>>,
+    pub(crate) local_axis2: Unit<Vector<Real>>,
+    pub(crate) basis1: [Vector<Real>; DIM - 1],
+    pub(crate) basis2: [Vector<Real>; DIM - 1],
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     #[cfg(feature = "dim3")]
-    pub impulse: Vector5<f32>,
+    pub impulse: Vector5<Real>,
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     #[cfg(feature = "dim2")]
-    pub impulse: Vector2<f32>,
+    pub impulse: Vector2<Real>,
     /// Whether or not this joint should enforce translational limits along its axis.
     pub limits_enabled: bool,
     /// The min an max relative position of the attached bodies along this joint's axis.
-    pub limits: [f32; 2],
+    pub limits: [Real; 2],
     /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
-    pub limits_impulse: f32,
+    pub limits_impulse: Real,
     // pub motor_enabled: bool,
-    // pub target_motor_vel: f32,
-    // pub max_motor_impulse: f32,
-    // pub motor_impulse: f32,
+    // pub target_motor_vel: Real,
+    // pub max_motor_impulse: Real,
+    // pub motor_impulse: Real,
 }
 
 impl PrismaticJoint {
@@ -47,10 +47,10 @@ impl PrismaticJoint {
     /// in the local-space of the affected bodies.
     #[cfg(feature = "dim2")]
     pub fn new(
-        local_anchor1: Point<f32>,
-        local_axis1: Unit<Vector<f32>>,
-        local_anchor2: Point<f32>,
-        local_axis2: Unit<Vector<f32>>,
+        local_anchor1: Point<Real>,
+        local_axis1: Unit<Vector<Real>>,
+        local_anchor2: Point<Real>,
+        local_axis2: Unit<Vector<Real>>,
     ) -> Self {
         Self {
             local_anchor1,
@@ -61,11 +61,11 @@ impl PrismaticJoint {
             basis2: local_axis2.orthonormal_basis(),
             impulse: na::zero(),
             limits_enabled: false,
-            limits: [-f32::MAX, f32::MAX],
+            limits: [-Real::MAX, Real::MAX],
             limits_impulse: 0.0,
             // motor_enabled: false,
             // target_motor_vel: 0.0,
-            // max_motor_impulse: f32::MAX,
+            // max_motor_impulse: Real::MAX,
             // motor_impulse: 0.0,
         }
     }
@@ -78,12 +78,12 @@ impl PrismaticJoint {
     /// computed arbitrarily.
     #[cfg(feature = "dim3")]
     pub fn new(
-        local_anchor1: Point<f32>,
-        local_axis1: Unit<Vector<f32>>,
-        local_tangent1: Vector<f32>,
-        local_anchor2: Point<f32>,
-        local_axis2: Unit<Vector<f32>>,
-        local_tangent2: Vector<f32>,
+        local_anchor1: Point<Real>,
+        local_axis1: Unit<Vector<Real>>,
+        local_tangent1: Vector<Real>,
+        local_anchor2: Point<Real>,
+        local_axis2: Unit<Vector<Real>>,
+        local_tangent2: Vector<Real>,
     ) -> Self {
         let basis1 = if let Some(local_bitangent1) =
             Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
@@ -116,28 +116,28 @@ impl PrismaticJoint {
             basis2,
             impulse: na::zero(),
             limits_enabled: false,
-            limits: [-f32::MAX, f32::MAX],
+            limits: [-Real::MAX, Real::MAX],
             limits_impulse: 0.0,
             // motor_enabled: false,
             // target_motor_vel: 0.0,
-            // max_motor_impulse: f32::MAX,
+            // max_motor_impulse: Real::MAX,
             // motor_impulse: 0.0,
         }
     }
 
     /// The local axis of this joint, expressed in the local-space of the first attached body.
-    pub fn local_axis1(&self) -> Unit<Vector<f32>> {
+    pub fn local_axis1(&self) -> Unit<Vector<Real>> {
         self.local_axis1
     }
 
     /// The local axis of this joint, expressed in the local-space of the second attached body.
-    pub fn local_axis2(&self) -> Unit<Vector<f32>> {
+    pub fn local_axis2(&self) -> Unit<Vector<Real>> {
         self.local_axis2
     }
 
     // FIXME: precompute this?
     #[cfg(feature = "dim2")]
-    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame1(&self) -> Isometry<Real> {
         use na::{Matrix2, Rotation2, UnitComplex};
 
         let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
@@ -149,7 +149,7 @@ impl PrismaticJoint {
 
     // FIXME: precompute this?
     #[cfg(feature = "dim2")]
-    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame2(&self) -> Isometry<Real> {
         use na::{Matrix2, Rotation2, UnitComplex};
 
         let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
@@ -161,7 +161,7 @@ impl PrismaticJoint {
 
     // FIXME: precompute this?
     #[cfg(feature = "dim3")]
-    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame1(&self) -> Isometry<Real> {
         use na::{Matrix3, Rotation3, UnitQuaternion};
 
         let mat = Matrix3::from_columns(&[
@@ -177,7 +177,7 @@ impl PrismaticJoint {
 
     // FIXME: precompute this?
     #[cfg(feature = "dim3")]
-    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame2(&self) -> Isometry<Real> {
         use na::{Matrix3, Rotation3, UnitQuaternion};
 
         let mat = Matrix3::from_columns(&[
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index cdb424b..ad7db0d 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -1,4 +1,4 @@
-use crate::math::{Point, Vector};
+use crate::math::{Point, Real, Vector};
 use crate::utils::WBasis;
 use na::{Unit, Vector5};
 
@@ -7,31 +7,31 @@ use na::{Unit, Vector5};
 /// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
 pub struct RevoluteJoint {
     /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
-    pub local_anchor1: Point<f32>,
+    pub local_anchor1: Point<Real>,
     /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
-    pub local_anchor2: Point<f32>,
+    pub local_anchor2: Point<Real>,
     /// The rotation axis of this revolute joint expressed in the local space of the first attached body.
-    pub local_axis1: Unit<Vector<f32>>,
+    pub local_axis1: Unit<Vector<Real>>,
     /// The rotation axis of this revolute joint expressed in the local space of the second attached body.
-    pub local_axis2: Unit<Vector<f32>>,
+    pub local_axis2: Unit<Vector<Real>>,
     /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
-    pub basis1: [Vector<f32>; 2],
+    pub basis1: [Vector<Real>; 2],
     /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
-    pub basis2: [Vector<f32>; 2],
+    pub basis2: [Vector<Real>; 2],
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector5<f32>,
+    pub impulse: Vector5<Real>,
 }
 
 impl RevoluteJoint {
     /// Creates a new revolute joint with the given point of applications and axis, all expressed
     /// in the local-space of the affected bodies.
     pub fn new(
-        local_anchor1: Point<f32>,
-        local_axis1: Unit<Vector<f32>>,
-        local_anchor2: Point<f32>,
-        local_axis2: Unit<Vector<f32>>,
+        local_anchor1: Point<Real>,
+        local_axis1: Unit<Vector<Real>>,
+        local_anchor2: Point<Real>,
+        local_axis2: Unit<Vector<Real>>,
     ) -> Self {
         Self {
             local_anchor1,