feat: implement new "small-steps" solver + joint improvements

1 files changed, 59 insertions, 0 deletions

diff --git a/src/dynamics/solver/solver_body.rs b/src/dynamics/solver/solver_body.rs
new file mode 100644
index 0000000..297f28a
--- /dev/null
+++ b/src/dynamics/solver/solver_body.rs
@@ -0,0 +1,59 @@
+use crate::dynamics::{RigidBody, RigidBodyVelocity};
+use crate::math::{AngularInertia, Isometry, Point, Real, Vector};
+use crate::prelude::RigidBodyDamping;
+
+#[cfg(feature = "dim2")]
+use crate::num::Zero;
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct SolverBody {
+    pub position: Isometry<Real>,
+    pub integrated_vels: RigidBodyVelocity,
+    pub im: Vector<Real>,
+    pub sqrt_ii: AngularInertia<Real>,
+    pub world_com: Point<Real>,
+    pub ccd_thickness: Real,
+    pub damping: RigidBodyDamping,
+    pub local_com: Point<Real>,
+}
+
+impl Default for SolverBody {
+    fn default() -> Self {
+        Self {
+            position: Isometry::identity(),
+            integrated_vels: RigidBodyVelocity::zero(),
+            im: na::zero(),
+            sqrt_ii: AngularInertia::zero(),
+            world_com: Point::origin(),
+            ccd_thickness: 0.0,
+            damping: RigidBodyDamping::default(),
+            local_com: Point::origin(),
+        }
+    }
+}
+
+impl SolverBody {
+    pub fn from(rb: &RigidBody) -> Self {
+        Self {
+            position: rb.pos.position,
+            integrated_vels: RigidBodyVelocity::zero(),
+            im: rb.mprops.effective_inv_mass,
+            sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt,
+            world_com: rb.mprops.world_com,
+            ccd_thickness: rb.ccd.ccd_thickness,
+            damping: rb.damping,
+            local_com: rb.mprops.local_mprops.local_com,
+        }
+    }
+
+    pub fn copy_from(&mut self, rb: &RigidBody) {
+        self.position = rb.pos.position;
+        self.integrated_vels = RigidBodyVelocity::zero();
+        self.im = rb.mprops.effective_inv_mass;
+        self.sqrt_ii = rb.mprops.effective_world_inv_inertia_sqrt;
+        self.world_com = rb.mprops.world_com;
+        self.ccd_thickness = rb.ccd.ccd_thickness;
+        self.damping = rb.damping;
+        self.local_com = rb.mprops.local_mprops.local_com;
+    }
+}