From 9b87f06a856c4d673642e210f8b0986cfdbac3af Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <sebcrozet@dimforge.com>
Date: Sun, 21 Jan 2024 21:02:23 +0100
Subject: feat: implement new "small-steps" solver + joint improvements

---
 src/dynamics/solver/solver_vel.rs | 59 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)
 create mode 100644 src/dynamics/solver/solver_vel.rs

(limited to 'src/dynamics/solver/solver_vel.rs')

diff --git a/src/dynamics/solver/solver_vel.rs b/src/dynamics/solver/solver_vel.rs
new file mode 100644
index 0000000..48c76b8
--- /dev/null
+++ b/src/dynamics/solver/solver_vel.rs
@@ -0,0 +1,59 @@
+use crate::math::{AngVector, Vector, SPATIAL_DIM};
+use crate::utils::SimdRealCopy;
+use na::{DVectorView, DVectorViewMut, Scalar};
+use std::ops::{AddAssign, Sub};
+
+#[derive(Copy, Clone, Debug, Default)]
+#[repr(C)]
+//#[repr(align(64))]
+pub struct SolverVel<N: Scalar + Copy> {
+    // The linear velocity of a solver body.
+    pub linear: Vector<N>,
+    // The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body.
+    pub angular: AngVector<N>,
+}
+
+impl<N: Scalar + Copy> SolverVel<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) -> DVectorView<N> {
+        DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM)
+    }
+
+    pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut<N> {
+        DVectorViewMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM)
+    }
+}
+
+impl<N: SimdRealCopy> SolverVel<N> {
+    pub fn zero() -> Self {
+        Self {
+            linear: na::zero(),
+            angular: na::zero(),
+        }
+    }
+}
+
+impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
+    fn add_assign(&mut self, rhs: Self) {
+        self.linear += rhs.linear;
+        self.angular += rhs.angular;
+    }
+}
+
+impl<N: SimdRealCopy> Sub for SolverVel<N> {
+    type Output = Self;
+
+    fn sub(self, rhs: Self) -> Self {
+        SolverVel {
+            linear: self.linear - rhs.linear,
+            angular: self.angular - rhs.angular,
+        }
+    }
+}
-- 
cgit