From 5b80c4efbf93ad1294c9d3d390d8c8f090681b0e Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Wed, 10 Feb 2021 11:56:51 +0100
Subject: Start experimenting with a generic joint implementation for joint
 drives.

---
 .../generic_position_constraint_wide.rs            | 51 ++++++++++++++++++++++
 1 file changed, 51 insertions(+)
 create mode 100644 src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs

(limited to 'src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs')

diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
new file mode 100644
index 0000000..9ceea67
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
@@ -0,0 +1,51 @@
+use super::{GenericPositionConstraint, GenericPositionGroundConstraint};
+use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WGenericPositionConstraint {
+    constraints: [GenericPositionConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WGenericPositionGroundConstraint {
+    constraints: [GenericPositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
-- 
cgit