Fix unsound uses of get_unchecked_mut

src/data/graph.rs

@@ -147,8 +147,7 @@ fn index_twice<T>(arr: &mut [T], a: usize, b: usize) -> Pair<&mut T> {
     } else {
         // safe because a, b are in bounds and distinct
         unsafe {
-            let ar = &mut *(arr.get_unchecked_mut(a) as *mut _);
-            let br = &mut *(arr.get_unchecked_mut(b) as *mut _);
+            let [ar, br] = arr.get_disjoint_unchecked_mut([a, b]);
             Pair::Both(ar, br)
         }
     }

src/dynamics/joint/multibody_joint/multibody_link.rs

@@ -112,17 +112,15 @@ impl MultibodyLinkVec {
     pub fn get_mut_with_parent(&mut self, i: usize) -> (&mut MultibodyLink, &MultibodyLink) {
         let parent_id = self[i].parent_internal_id;
 
-        assert!(
-            parent_id != i,
-            "Internal error: circular rigid body dependency."
-        );
-        assert!(parent_id < self.len(), "Invalid parent index.");
-
-        unsafe {
-            let rb = &mut *(self.get_unchecked_mut(i) as *mut _);
-            let parent_rb = &*(self.get_unchecked(parent_id) as *const _);
-            (rb, parent_rb)
-        }
+        let [rb, parent_rb] = self.get_disjoint_mut([i, parent_id]).unwrap_or_else(|err| {
+            if parent_id == i {
+                panic!("Internal error: circular rigid body dependency. ({err:?})")
+            } else {
+                panic!("Invalid parent index. ({err:?})")
+            }
+        });
+
+        (rb, parent_rb)
     }
 }
 

src/utils.rs

@@ -619,28 +619,16 @@ pub trait IndexMut2<I>: IndexMut<I> {
 impl<T> IndexMut2<usize> for Vec<T> {
     #[inline]
     fn index_mut2(&mut self, i: usize, j: usize) -> (&mut T, &mut T) {
-        assert!(i != j, "Unable to index the same element twice.");
-        assert!(i < self.len() && j < self.len(), "Index out of bounds.");
-
-        unsafe {
-            let a = &mut *(self.get_unchecked_mut(i) as *mut _);
-            let b = &mut *(self.get_unchecked_mut(j) as *mut _);
-            (a, b)
-        }
+        let [a, b] = self.get_disjoint_mut([i, j]).unwrap();
+        (a, b)
     }
 }
 
 impl<T> IndexMut2<usize> for [T] {
     #[inline]
     fn index_mut2(&mut self, i: usize, j: usize) -> (&mut T, &mut T) {
-        assert!(i != j, "Unable to index the same element twice.");
-        assert!(i < self.len() && j < self.len(), "Index out of bounds.");
-
-        unsafe {
-            let a = &mut *(self.get_unchecked_mut(i) as *mut _);
-            let b = &mut *(self.get_unchecked_mut(j) as *mut _);
-            (a, b)
-        }
+        let [a, b] = self.get_disjoint_mut([i, j]).unwrap();
+        (a, b)
     }
 }