Make revolute joint actuation work properly even when SIMD is enabled.

5 files changed, 32 insertions, 0 deletions

diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index 82e2a10..f1e8fdf 100644
--- a/src/dynamics/joint/ball_joint.rs
+++ b/src/dynamics/joint/ball_joint.rs
@@ -31,4 +31,9 @@ impl BallJoint {
             impulse,
         }
     }
+
+    /// Can a SIMD constraint be used for resolving this joint?
+    pub fn supports_simd_constraints(&self) -> bool {
+        true
+    }
 }
diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
index 359e14a..2917757 100644
--- a/src/dynamics/joint/fixed_joint.rs
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -30,4 +30,9 @@ impl FixedJoint {
             impulse: SpacialVector::zeros(),
         }
     }
+
+    /// Can a SIMD constraint be used for resolving this joint?
+    pub fn supports_simd_constraints(&self) -> bool {
+        true
+    }
 }
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index fe77b4b..b4089ae 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -128,3 +128,14 @@ pub struct Joint {
     /// The joint geometric parameters and impulse.
     pub params: JointParams,
 }
+
+impl Joint {
+    pub fn supports_simd_constraints(&self) -> bool {
+        match &self.params {
+            JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(),
+            JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(),
+            JointParams::FixedJoint(joint) => joint.supports_simd_constraints(),
+            JointParams::BallJoint(joint) => joint.supports_simd_constraints(),
+        }
+    }
+}
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 0edc939..6e32a01 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -135,6 +135,11 @@ impl PrismaticJoint {
         self.local_axis2
     }
 
+    /// Can a SIMD constraint be used for resolving this joint?
+    pub fn supports_simd_constraints(&self) -> bool {
+        true
+    }
+
     // FIXME: precompute this?
     #[cfg(feature = "dim2")]
     pub(crate) fn local_frame1(&self) -> Isometry<Real> {
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 9af0ae6..022779d 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -77,6 +77,12 @@ impl RevoluteJoint {
         }
     }
 
+    /// Can a SIMD constraint be used for resolving this joint?
+    pub fn supports_simd_constraints(&self) -> bool {
+        // SIMD revolute constraints don't support motors right now.
+        self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)
+    }
+
     pub fn configure_motor_model(&mut self, model: SpringModel) {
         self.motor_model = model;
     }