Implement multibody joints and the new solver

1 files changed, 22 insertions, 2 deletions

diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
index b457020..9160f2e 100644
--- a/src/dynamics/solver/delta_vel.rs
+++ b/src/dynamics/solver/delta_vel.rs
@@ -1,14 +1,34 @@
-use crate::math::{AngVector, Vector};
+use crate::math::{AngVector, Vector, SPATIAL_DIM};
+use na::{DVectorSlice, DVectorSliceMut};
 use na::{Scalar, SimdRealField};
 use std::ops::AddAssign;
 
 #[derive(Copy, Clone, Debug)]
+#[repr(C)]
 //#[repr(align(64))]
-pub(crate) struct DeltaVel<N: Scalar + Copy> {
+pub struct DeltaVel<N: Scalar + Copy> {
     pub linear: Vector<N>,
     pub angular: AngVector<N>,
 }
 
+impl<N: Scalar + Copy> DeltaVel<N> {
+    pub fn as_slice(&self) -> &[N; SPATIAL_DIM] {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] {
+        unsafe { std::mem::transmute(self) }
+    }
+
+    pub fn as_vector_slice(&self) -> DVectorSlice<N> {
+        DVectorSlice::from_slice(&self.as_slice()[..], SPATIAL_DIM)
+    }
+
+    pub fn as_vector_slice_mut(&mut self) -> DVectorSliceMut<N> {
+        DVectorSliceMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM)
+    }
+}
+
 impl<N: SimdRealField + Copy> DeltaVel<N> {
     pub fn zero() -> Self {
         Self {