Fix unsound uses of get_unchecked_mut

[vdrn]

There were multiple unsound uses of get_unchecked_mut.

That function does not allow you to get multiple mutable references into a slice.
It only skips bounds-checking, nothing else.

You can run this code with cargo miri run to see the issue:

fn main() {
    let mut v: Vec<i32> = vec![1, 2];
    unsafe {
        let a = v.get_unchecked_mut(0) as *mut i32;
        let b = v.get_unchecked_mut(1) as *mut i32;
        println!("{}, {}", *a, *b);
    }
}

Since 1.86, stable Rust provides the exact functions you need:
get_disjoint_mut and get_disjoint_unchecked_mut.

Notes:

• I've tried to make this pull request as non-intrusive as possible, but IMO IndexMut2 Trait is redundant and should probably be removed.
• In get_mut_with_parent, I've kept the same error message for "Internal error: circular rigid body dependency. ", but that probably should just be get_disjoint_mut(..).unwrap()