Use newtypes for collider, rigid-body and joint handles.

5 files changed, 98 insertions, 93 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 96fcbf9..a4de108 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
+use crate::dynamics::{MassProperties, RigidBodyHandle};
 use crate::geometry::InteractionGroups;
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector};
 use cdl::bounding_volume::AABB;
@@ -333,7 +333,7 @@ pub struct Collider {
 
 impl Collider {
     pub(crate) fn reset_internal_references(&mut self) {
-        self.parent = RigidBodySet::invalid_handle();
+        self.parent = RigidBodyHandle::invalid();
         self.proxy_index = crate::INVALID_USIZE;
     }
 
@@ -708,7 +708,7 @@ impl ColliderBuilder {
             restitution: self.restitution,
             delta: self.delta,
             is_sensor: self.is_sensor,
-            parent: RigidBodySet::invalid_handle(),
+            parent: RigidBodyHandle::invalid(),
             position: Isometry::identity(),
             predicted_position: Isometry::identity(),
             proxy_index: crate::INVALID_USIZE,
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index eb09322..60ce007 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -2,10 +2,42 @@ use crate::data::arena::Arena;
 use crate::data::pubsub::PubSub;
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
 use crate::geometry::Collider;
+use cdl::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
 
 /// The unique identifier of a collider added to a collider set.
-pub type ColliderHandle = crate::data::arena::Index;
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct ColliderHandle(pub(crate) crate::data::arena::Index);
+
+impl ColliderHandle {
+    pub fn into_raw_parts(self) -> (usize, u64) {
+        self.0.into_raw_parts()
+    }
+
+    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid collider handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_USIZE,
+            crate::INVALID_U64,
+        ))
+    }
+}
+
+impl IndexedData for ColliderHandle {
+    fn default() -> Self {
+        Self(IndexedData::default())
+    }
+
+    fn index(&self) -> usize {
+        self.0.index()
+    }
+}
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -38,7 +70,7 @@ impl ColliderSet {
 
     /// Iterate through all the colliders on this set.
     pub fn iter(&self) -> impl ExactSizeIterator<Item = (ColliderHandle, &Collider)> {
-        self.colliders.iter()
+        self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c))
     }
 
     /// The number of colliders on this set.
@@ -48,7 +80,7 @@ impl ColliderSet {
 
     /// Is this collider handle valid?
     pub fn contains(&self, handle: ColliderHandle) -> bool {
-        self.colliders.contains(handle)
+        self.colliders.contains(handle.0)
     }
 
     /// Inserts a new collider to this set and retrieve its handle.
@@ -71,8 +103,8 @@ impl ColliderSet {
             .expect("Parent rigid body not found.");
         coll.position = parent.position * coll.delta;
         coll.predicted_position = parent.predicted_position * coll.delta;
-        let handle = self.colliders.insert(coll);
-        let coll = self.colliders.get(handle).unwrap();
+        let handle = ColliderHandle(self.colliders.insert(coll));
+        let coll = self.colliders.get(handle.0).unwrap();
         parent.add_collider(handle, &coll);
         handle
     }
@@ -87,7 +119,7 @@ impl ColliderSet {
         bodies: &mut RigidBodySet,
         wake_up: bool,
     ) -> Option<Collider> {
-        let collider = self.colliders.remove(handle)?;
+        let collider = self.colliders.remove(handle.0)?;
 
         /*
          * Delete the collider from its parent body.
@@ -125,7 +157,9 @@ impl ColliderSet {
     /// Using this is discouraged in favor of `self.get(handle)` which does not
     /// suffer form the ABA problem.
     pub fn get_unknown_gen(&self, i: usize) -> Option<(&Collider, ColliderHandle)> {
-        self.colliders.get_unknown_gen(i)
+        self.colliders
+            .get_unknown_gen(i)
+            .map(|(c, h)| (c, ColliderHandle(h)))
     }
 
     /// Gets a mutable reference to the collider with the given handle without a known generation.
@@ -138,17 +172,19 @@ impl ColliderSet {
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
     pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> {
-        self.colliders.get_unknown_gen_mut(i)
+        self.colliders
+            .get_unknown_gen_mut(i)
+            .map(|(c, h)| (c, ColliderHandle(h)))
     }
 
     /// Get the collider with the given handle.
     pub fn get(&self, handle: ColliderHandle) -> Option<&Collider> {
-        self.colliders.get(handle)
+        self.colliders.get(handle.0)
     }
 
     /// Gets a mutable reference to the collider with the given handle.
     pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
-        self.colliders.get_mut(handle)
+        self.colliders.get_mut(handle.0)
     }
 
     // pub(crate) fn get2_mut_internal(
@@ -177,12 +213,12 @@ impl Index<ColliderHandle> for ColliderSet {
     type Output = Collider;
 
     fn index(&self, index: ColliderHandle) -> &Collider {
-        &self.colliders[index]
+        &self.colliders[index.0]
     }
 }
 
 impl IndexMut<ColliderHandle> for ColliderSet {
     fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider {
-        &mut self.colliders[index]
+        &mut self.colliders[index.0]
     }
 }
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 546b127..bd18a5d 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::{BodyPair, RigidBodySet};
+use crate::dynamics::{BodyPair, RigidBodyHandle};
 use crate::geometry::{ColliderPair, ContactManifold};
 use crate::math::{Point, Real, Vector};
 use cdl::query::ContactManifoldsWorkspace;
@@ -113,10 +113,7 @@ pub struct SolverContact {
 impl Default for ContactManifoldData {
     fn default() -> Self {
         Self::new(
-            BodyPair::new(
-                RigidBodySet::invalid_handle(),
-                RigidBodySet::invalid_handle(),
-            ),
+            BodyPair::new(RigidBodyHandle::invalid(), RigidBodyHandle::invalid()),
             SolverFlags::empty(),
         )
     }
diff --git a/src/geometry/interaction_graph.rs b/src/geometry/interaction_graph.rs
index 54a19ce..657050c 100644
--- a/src/geometry/interaction_graph.rs
+++ b/src/geometry/interaction_graph.rs
@@ -1,5 +1,4 @@
 use crate::data::graph::{Direction, EdgeIndex, Graph, NodeIndex};
-use crate::geometry::ColliderHandle;
 
 /// Index of a node of the interaction graph.
 pub type ColliderGraphIndex = NodeIndex;
@@ -11,11 +10,11 @@ pub type TemporaryInteractionIndex = EdgeIndex;
 /// A graph where nodes are collision objects and edges are contact or proximity algorithms.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
-pub struct InteractionGraph<T> {
-    pub(crate) graph: Graph<ColliderHandle, T>,
+pub struct InteractionGraph<N, E> {
+    pub(crate) graph: Graph<N, E>,
 }
 
-impl<T> InteractionGraph<T> {
+impl<N: Copy, E> InteractionGraph<N, E> {
     /// Creates a new empty collection of collision objects.
     pub fn new() -> Self {
         InteractionGraph {
@@ -24,7 +23,7 @@ impl<T> InteractionGraph<T> {
     }
 
     /// The underlying raw graph structure of this interaction graph.
-    pub fn raw_graph(&self) -> &Graph<ColliderHandle, T> {
+    pub fn raw_graph(&self) -> &Graph<N, E> {
         &self.graph
     }
 
@@ -40,7 +39,7 @@ impl<T> InteractionGraph<T> {
         &mut self,
         index1: ColliderGraphIndex,
         index2: ColliderGraphIndex,
-        interaction: T,
+        interaction: E,
     ) -> TemporaryInteractionIndex {
         self.graph.add_edge(index1, index2, interaction)
     }
@@ -49,7 +48,7 @@ impl<T> InteractionGraph<T> {
         &mut self,
         index1: ColliderGraphIndex,
         index2: ColliderGraphIndex,
-    ) -> Option<T> {
+    ) -> Option<E> {
         let id = self.graph.find_edge(index1, index2)?;
         self.graph.remove_edge(id)
     }
@@ -69,20 +68,18 @@ impl<T> InteractionGraph<T> {
     /// }
     /// ```
     #[must_use = "The graph index of the collision object returned by this method has been changed to `id`."]
-    pub(crate) fn remove_node(&mut self, id: ColliderGraphIndex) -> Option<ColliderHandle> {
+    pub(crate) fn remove_node(&mut self, id: ColliderGraphIndex) -> Option<N> {
         let _ = self.graph.remove_node(id);
         self.graph.node_weight(id).cloned()
     }
 
     /// All the interactions on this graph.
-    pub fn interactions(&self) -> impl Iterator<Item = &T> {
+    pub fn interactions(&self) -> impl Iterator<Item = &E> {
         self.graph.raw_edges().iter().map(move |edge| &edge.weight)
     }
 
     /// All the interactions on this graph with the corresponding endpoint weights.
-    pub fn interactions_with_endpoints(
-        &self,
-    ) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
+    pub fn interactions_with_endpoints(&self) -> impl Iterator<Item = (N, N, &E)> {
         self.graph.raw_edges().iter().map(move |edge| {
             (
                 self.graph.raw_nodes()[edge.source().index()].weight,
@@ -97,7 +94,7 @@ impl<T> InteractionGraph<T> {
         &self,
         id1: ColliderGraphIndex,
         id2: ColliderGraphIndex,
-    ) -> Option<(ColliderHandle, ColliderHandle, &T)> {
+    ) -> Option<(N, N, &E)> {
         self.graph.find_edge(id1, id2).and_then(|edge| {
             let endpoints = self.graph.edge_endpoints(edge)?;
             let h1 = self.graph.node_weight(endpoints.0)?;
@@ -112,7 +109,7 @@ impl<T> InteractionGraph<T> {
         &mut self,
         id1: ColliderGraphIndex,
         id2: ColliderGraphIndex,
-    ) -> Option<(ColliderHandle, ColliderHandle, &mut T)> {
+    ) -> Option<(N, N, &mut E)> {
         let edge = self.graph.find_edge(id1, id2)?;
         let endpoints = self.graph.edge_endpoints(edge)?;
         let h1 = *self.graph.node_weight(endpoints.0)?;
@@ -122,10 +119,7 @@ impl<T> InteractionGraph<T> {
     }
 
     /// All the interaction involving the collision object with graph index `id`.
-    pub fn interactions_with(
-        &self,
-        id: ColliderGraphIndex,
-    ) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
+    pub fn interactions_with(&self, id: ColliderGraphIndex) -> impl Iterator<Item = (N, N, &E)> {
         self.graph.edges(id).filter_map(move |e| {
             let endpoints = self.graph.edge_endpoints(e.id()).unwrap();
             Some((self.graph[endpoints.0], self.graph[endpoints.1], e.weight()))
@@ -133,10 +127,7 @@ impl<T> InteractionGraph<T> {
     }
 
     /// Gets the interaction with the given index.
-    pub fn index_interaction(
-        &self,
-        id: TemporaryInteractionIndex,
-    ) -> Option<(ColliderHandle, ColliderHandle, &T)> {
+    pub fn index_interaction(&self, id: TemporaryInteractionIndex) -> Option<(N, N, &E)> {
         if let (Some(e), Some(endpoints)) =
             (self.graph.edge_weight(id), self.graph.edge_endpoints(id))
         {
@@ -150,14 +141,7 @@ impl<T> InteractionGraph<T> {
     pub fn interactions_with_mut(
         &mut self,
         id: ColliderGraphIndex,
-    ) -> impl Iterator<
-        Item = (
-            ColliderHandle,
-            ColliderHandle,
-            TemporaryInteractionIndex,
-            &mut T,
-        ),
-    > {
+    ) -> impl Iterator<Item = (N, N, TemporaryInteractionIndex, &mut E)> {
         let incoming_edge = self.graph.first_edge(id, Direction::Incoming);
         let outgoing_edge = self.graph.first_edge(id, Direction::Outgoing);
 
@@ -172,7 +156,7 @@ impl<T> InteractionGraph<T> {
     // pub fn colliders_interacting_with<'a>(
     //     &'a self,
     //     id: ColliderGraphIndex,
-    // ) -> impl Iterator<Item = ColliderHandle> + 'a {
+    // ) -> impl Iterator<Item = N> + 'a {
     //     self.graph.edges(id).filter_map(move |e| {
     //         let inter = e.weight();
     //
@@ -188,7 +172,7 @@ impl<T> InteractionGraph<T> {
     // pub fn colliders_in_contact_with<'a>(
     //     &'a self,
     //     id: ColliderGraphIndex,
-    // ) -> impl Iterator<Item = ColliderHandle> + 'a {
+    // ) -> impl Iterator<Item = N> + 'a {
     //     self.graph.edges(id).filter_map(move |e| {
     //         let inter = e.weight();
     //
@@ -209,7 +193,7 @@ impl<T> InteractionGraph<T> {
     // pub fn colliders_in_proximity_of<'a>(
     //     &'a self,
     //     id: ColliderGraphIndex,
-    // ) -> impl Iterator<Item = ColliderHandle> + 'a {
+    // ) -> impl Iterator<Item = N> + 'a {
     //     self.graph.edges(id).filter_map(move |e| {
     //         if let Interaction::Proximity(_, prox) = e.weight() {
     //             if *prox == Proximity::Intersecting {
@@ -226,29 +210,17 @@ impl<T> InteractionGraph<T> {
     // }
 }
 
-pub struct InteractionsWithMut<'a, T> {
-    graph: &'a mut Graph<ColliderHandle, T>,
+pub struct InteractionsWithMut<'a, N, E> {
+    graph: &'a mut Graph<N, E>,
     incoming_edge: Option<EdgeIndex>,
     outgoing_edge: Option<EdgeIndex>,
 }
 
-impl<'a, T> Iterator for InteractionsWithMut<'a, T> {
-    type Item = (
-        ColliderHandle,
-        ColliderHandle,
-        TemporaryInteractionIndex,
-        &'a mut T,
-    );
+impl<'a, N: Copy, E> Iterator for InteractionsWithMut<'a, N, E> {
+    type Item = (N, N, TemporaryInteractionIndex, &'a mut E);
 
     #[inline]
-    fn next(
-        &mut self,
-    ) -> Option<(
-        ColliderHandle,
-        ColliderHandle,
-        TemporaryInteractionIndex,
-        &'a mut T,
-    )> {
+    fn next(&mut self) -> Option<(N, N, TemporaryInteractionIndex, &'a mut E)> {
         if let Some(edge) = self.incoming_edge {
             self.incoming_edge = self.graph.next_edge(edge, Direction::Incoming);
             let endpoints = self.graph.edge_endpoints(edge).unwrap();
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 5171f1b..621a3b4 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -27,8 +27,8 @@ struct ColliderGraphIndices {
 impl ColliderGraphIndices {
     fn invalid() -> Self {
         Self {
-            contact_graph_index: InteractionGraph::<ContactPair>::invalid_graph_index(),
-            intersection_graph_index: InteractionGraph::<bool>::invalid_graph_index(),
+            contact_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
+            intersection_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
         }
     }
 }
@@ -42,8 +42,8 @@ pub struct NarrowPhase {
         serde(skip, default = "crate::geometry::default_persistent_query_dispatcher")
     )]
     query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>,
-    contact_graph: InteractionGraph<ContactPair>,
-    intersection_graph: InteractionGraph<bool>,
+    contact_graph: InteractionGraph<ColliderHandle, ContactPair>,
+    intersection_graph: InteractionGraph<ColliderHandle, bool>,
     graph_indices: Coarena<ColliderGraphIndices>,
     removed_colliders: Option<Subscription<RemovedCollider>>,
 }
@@ -71,12 +71,12 @@ impl NarrowPhase {
     }
 
     /// The contact graph containing all contact pairs and their contact information.
-    pub fn contact_graph(&self) -> &InteractionGraph<ContactPair> {
+    pub fn contact_graph(&self) -> &InteractionGraph<ColliderHandle, ContactPair> {
         &self.contact_graph
     }
 
     /// The intersection graph containing all intersection pairs and their intersection information.
-    pub fn intersection_graph(&self) -> &InteractionGraph<bool> {
+    pub fn intersection_graph(&self) -> &InteractionGraph<ColliderHandle, bool> {
         &self.intersection_graph
     }
 
@@ -85,7 +85,7 @@ impl NarrowPhase {
         &self,
         collider: ColliderHandle,
     ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>> {
-        let id = self.graph_indices.get(collider)?;
+        let id = self.graph_indices.get(collider.0)?;
         Some(self.contact_graph.interactions_with(id.contact_graph_index))
     }
 
@@ -94,7 +94,7 @@ impl NarrowPhase {
         &'a self,
         collider: ColliderHandle,
     ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, bool)> + 'a> {
-        let id = self.graph_indices.get(collider)?;
+        let id = self.graph_indices.get(collider.0)?;
         Some(
             self.intersection_graph
                 .interactions_with(id.intersection_graph_index)
@@ -112,8 +112,8 @@ impl NarrowPhase {
         collider1: ColliderHandle,
         collider2: ColliderHandle,
     ) -> Option<&ContactPair> {
-        let id1 = self.graph_indices.get(collider1)?;
-        let id2 = self.graph_indices.get(collider2)?;
+        let id1 = self.graph_indices.get(collider1.0)?;
+        let id2 = self.graph_indices.get(collider2.0)?;
         self.contact_graph
             .interaction_pair(id1.contact_graph_index, id2.contact_graph_index)
             .map(|c| c.2)
@@ -128,8 +128,8 @@ impl NarrowPhase {
         collider1: ColliderHandle,
         collider2: ColliderHandle,
     ) -> Option<bool> {
-        let id1 = self.graph_indices.get(collider1)?;
-        let id2 = self.graph_indices.get(collider2)?;
+        let id1 = self.graph_indices.get(collider1.0)?;
+        let id2 = self.graph_indices.get(collider2.0)?;
         self.intersection_graph
             .interaction_pair(id1.intersection_graph_index, id2.intersection_graph_index)
             .map(|c| *c.2)
@@ -173,7 +173,7 @@ impl NarrowPhase {
         while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
             // NOTE: if the collider does not have any graph indices currently, there is nothing
             // to remove in the narrow-phase for this collider.
-            if let Some(graph_idx) = self.graph_indices.get(collider.handle) {
+            if let Some(graph_idx) = self.graph_indices.get(collider.handle.0) {
                 let intersection_graph_id = prox_id_remap
                     .get(&collider.handle)
                     .copied()
@@ -223,7 +223,7 @@ impl NarrowPhase {
         // We have to manage the fact that one other collider will
         // have its graph index changed because of the node's swap-remove.
         if let Some(replacement) = self.intersection_graph.remove_node(intersection_graph_id) {
-            if let Some(replacement) = self.graph_indices.get_mut(replacement) {
+            if let Some(replacement) = self.graph_indices.get_mut(replacement.0) {
                 replacement.intersection_graph_index = intersection_graph_id;
             } else {
                 prox_id_remap.insert(replacement, intersection_graph_id);
@@ -231,7 +231,7 @@ impl NarrowPhase {
         }
 
         if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
-            if let Some(replacement) = self.graph_indices.get_mut(replacement) {
+            if let Some(replacement) = self.graph_indices.get_mut(replacement.0) {
                 replacement.contact_graph_index = contact_graph_id;
             } else {
                 contact_id_remap.insert(replacement, contact_graph_id);
@@ -258,22 +258,22 @@ impl NarrowPhase {
                         }
 
                         let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
-                            pair.collider1,
-                            pair.collider2,
+                            pair.collider1.0,
+                            pair.collider2.0,
                             ColliderGraphIndices::invalid(),
                         );
 
                         if co1.is_sensor() || co2.is_sensor() {
                             // NOTE: the collider won't have a graph index as long
                             // as it does not interact with anything.
-                            if !InteractionGraph::<bool>::is_graph_index_valid(
+                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
                                 gid1.intersection_graph_index,
                             ) {
                                 gid1.intersection_graph_index =
                                     self.intersection_graph.graph.add_node(pair.collider1);
                             }
 
-                            if !InteractionGraph::<bool>::is_graph_index_valid(
+                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
                                 gid2.intersection_graph_index,
                             ) {
                                 gid2.intersection_graph_index =
@@ -301,14 +301,14 @@ impl NarrowPhase {
 
                             // NOTE: the collider won't have a graph index as long
                             // as it does not interact with anything.
-                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(
+                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
                                 gid1.contact_graph_index,
                             ) {
                                 gid1.contact_graph_index =
                                     self.contact_graph.graph.add_node(pair.collider1);
                             }
 
-                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(
+                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
                                 gid2.contact_graph_index,
                             ) {
                                 gid2.contact_graph_index =
@@ -338,8 +338,8 @@ impl NarrowPhase {
                         // TODO: could we just unwrap here?
                         // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
                         if let (Some(gid1), Some(gid2)) = (
-                            self.graph_indices.get(pair.collider1),
-                            self.graph_indices.get(pair.collider2),
+                            self.graph_indices.get(pair.collider1.0),
+                            self.graph_indices.get(pair.collider2.0),
                         ) {
                             if co1.is_sensor() || co2.is_sensor() {
                                 let was_intersecting = self.intersection_graph.remove_edge(