Start experimenting with a generic joint implementation for joint drives.

1 files changed, 34 insertions, 3 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
index 97a81ba..29acac6 100644
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
@@ -1,6 +1,7 @@
 use super::{
     BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
-    FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+    FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint,
+    PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
 };
 #[cfg(feature = "dim3")]
 use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
@@ -10,8 +11,8 @@ use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 use super::{
     WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
-    WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
-    WPrismaticPositionGroundConstraint,
+    WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
+    WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
 };
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
 #[cfg(feature = "simd-is-enabled")]
@@ -31,6 +32,12 @@ pub(crate) enum AnyJointPositionConstraint {
     WFixedJoint(WFixedPositionConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedPositionGroundConstraint),
+    GenericJoint(GenericPositionConstraint),
+    GenericGroundConstraint(GenericPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WGenericJoint(WGenericPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WGenericGroundConstraint(WGenericPositionGroundConstraint),
     PrismaticJoint(PrismaticPositionConstraint),
     PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -61,6 +68,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
                 FixedPositionConstraint::from_params(rb1, rb2, p),
             ),
+            JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
+                GenericPositionConstraint::from_params(rb1, rb2, p),
+            ),
             JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
                 PrismaticPositionConstraint::from_params(rb1, rb2, p),
             ),
@@ -89,6 +99,12 @@ impl AnyJointPositionConstraint {
                     rbs1, rbs2, joints,
                 ))
             }
+            JointParams::GenericJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
+                    rbs1, rbs2, joints,
+                ))
+            }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -123,6 +139,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
                 FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
             ),
+            JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
+                GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointPositionConstraint::PrismaticGroundConstraint(
                     PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -161,6 +180,12 @@ impl AnyJointPositionConstraint {
                     WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
+            JointParams::GenericJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WGenericGroundConstraint(
+                    WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                )
+            }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -193,6 +218,12 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]