From 81cd09f68a9a316774299d42ca88522c3032c08e Mon Sep 17 00:00:00 2001
From: Vedran Maric <valinion@gmail.com>
Date: Sat, 30 Aug 2025 01:48:21 +0200
Subject: [PATCH] Fix unsound use of `get_unchecked_mut`

---
 src/data/graph.rs                             |  3 +--
 .../joint/multibody_joint/multibody_link.rs   | 20 +++++++++----------
 src/utils.rs                                  | 20 ++++---------------
 3 files changed, 14 insertions(+), 29 deletions(-)

diff --git a/src/data/graph.rs b/src/data/graph.rs
index 4620b8850..7314faee0 100644
--- a/src/data/graph.rs
+++ b/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)
         }
     }
diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs
index 611cb49db..478aefe27 100644
--- a/src/dynamics/joint/multibody_joint/multibody_link.rs
+++ b/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)
     }
 }
 
diff --git a/src/utils.rs b/src/utils.rs
index 9917cb5b1..4b7cba624 100644
--- a/src/utils.rs
+++ b/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)
     }
 }