Implement multibody joints and the new solver

2 files changed, 32 insertions, 32 deletions

diff --git a/src/data/coarena.rs b/src/data/coarena.rs
index 124d85a..1f01c05 100644
--- a/src/data/coarena.rs
+++ b/src/data/coarena.rs
@@ -13,6 +13,14 @@ impl<T> Coarena<T> {
         Self { data: Vec::new() }
     }
 
+    pub fn iter(&self) -> impl Iterator<Item = (Index, &T)> {
+        self.data
+            .iter()
+            .enumerate()
+            .filter(|(_, elt)| elt.0 != u32::MAX)
+            .map(|(i, elt)| (Index::from_raw_parts(i as u32, elt.0), &elt.1))
+    }
+
     /// Gets a specific element from the coarena without specifying its generation number.
     ///
     /// It is strongly encouraged to use `Coarena::get` instead of this method because this method
@@ -23,12 +31,12 @@ impl<T> Coarena<T> {
 
     /// Deletes an element for the coarena and returns its value.
     ///
-    /// We can't really remove an element from the coarena. So instead of actually removing
-    /// it, this method will reset the value to the given `removed_value`.
+    /// This method will reset the value to the given `removed_value`.
     pub fn remove(&mut self, index: Index, removed_value: T) -> Option<T> {
         let (i, g) = index.into_raw_parts();
         let data = self.data.get_mut(i as usize)?;
         if g == data.0 {
+            data.0 = u32::MAX; // invalidate the generation number.
             Some(std::mem::replace(&mut data.1, removed_value))
         } else {
             None
diff --git a/src/data/graph.rs b/src/data/graph.rs
index de958c3..2dc7fbf 100644
--- a/src/data/graph.rs
+++ b/src/data/graph.rs
@@ -644,19 +644,24 @@ impl<'a, E> Iterator for Edges<'a, E> {
 
         // For iterate_over, "both" is represented as None.
         // For reverse, "no" is represented as None.
-        let (iterate_over, reverse) = (None, Some(self.direction.opposite()));
+        let (iterate_over, _reverse) = (None, Some(self.direction.opposite()));
 
         if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing {
             let i = self.next[0].index();
-            if let Some(Edge { node, weight, next }) = self.edges.get(i) {
+            if let Some(Edge {
+                node: _node,
+                weight,
+                next,
+            }) = self.edges.get(i)
+            {
                 self.next[0] = next[0];
                 return Some(EdgeReference {
                     index: EdgeIndex(i as u32),
-                    node: if reverse == Some(Direction::Outgoing) {
-                        swap_pair(*node)
-                    } else {
-                        *node
-                    },
+                    // node: if reverse == Some(Direction::Outgoing) {
+                    //     swap_pair(*node)
+                    // } else {
+                    //     *node
+                    // },
                     weight,
                 });
             }
@@ -674,11 +679,11 @@ impl<'a, E> Iterator for Edges<'a, E> {
 
                 return Some(EdgeReference {
                     index: edge_index,
-                    node: if reverse == Some(Direction::Incoming) {
-                        swap_pair(*node)
-                    } else {
-                        *node
-                    },
+                    // node: if reverse == Some(Direction::Incoming) {
+                    //     swap_pair(*node)
+                    // } else {
+                    //     *node
+                    // },
                     weight,
                 });
             }
@@ -688,10 +693,10 @@ impl<'a, E> Iterator for Edges<'a, E> {
     }
 }
 
-fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
-    x.swap(0, 1);
-    x
-}
+// fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
+//     x.swap(0, 1);
+//     x
+// }
 
 impl<'a, E> Clone for Edges<'a, E> {
     fn clone(&self) -> Self {
@@ -742,24 +747,11 @@ impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> {
     }
 }
 
-/// A “walker” object that can be used to step through the edge list of a node.
-///
-/// Created with [`.detach()`](struct.Neighbors.html#method.detach).
-///
-/// The walker does not borrow from the graph, so it lets you step through
-/// neighbors or incident edges while also mutating graph weights, as
-/// in the following example:
-#[derive(Clone)]
-pub struct WalkNeighbors {
-    skip_start: NodeIndex,
-    next: [EdgeIndex; 2],
-}
-
 /// Reference to a `Graph` edge.
 #[derive(Debug)]
 pub struct EdgeReference<'a, E: 'a> {
     index: EdgeIndex,
-    node: [NodeIndex; 2],
+    // node: [NodeIndex; 2],
     weight: &'a E,
 }