From f7643272f40fa5776ce21a5ccdb43101d987030e Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Sat, 7 Aug 2021 18:20:19 +0200
Subject: Implement limits for ball joints.

---
 src/dynamics/joint/ball_joint.rs     | 24 +++++++++++++++++++++---
 src/dynamics/joint/revolute_joint.rs |  3 ++-
 2 files changed, 23 insertions(+), 4 deletions(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index ee11489..0b626c0 100644
--- a/src/dynamics/joint/ball_joint.rs
+++ b/src/dynamics/joint/ball_joint.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::SpringModel;
-use crate::math::{Point, Real, Rotation, Vector};
+use crate::math::{Point, Real, Rotation, UnitVector, Vector};
 
 #[derive(Copy, Clone, PartialEq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -38,6 +38,17 @@ pub struct BallJoint {
     pub motor_impulse: Vector<Real>,
     /// The spring-like model used by the motor to reach the target velocity and .
     pub motor_model: SpringModel,
+
+    /// Are the limits enabled for this joint?
+    pub limits_enabled: bool,
+    /// The axis of the limit cone for this joint, if the local-space of the first body.
+    pub limits_local_axis1: UnitVector<Real>,
+    /// The axis of the limit cone for this joint, if the local-space of the first body.
+    pub limits_local_axis2: UnitVector<Real>,
+    /// The maximum angle allowed between the two limit axes in world-space.
+    pub limits_angle: Real,
+    /// The impulse applied to enforce joint limits.
+    pub limits_impulse: Real,
 }
 
 impl BallJoint {
@@ -62,13 +73,20 @@ impl BallJoint {
             motor_impulse: na::zero(),
             motor_max_impulse: Real::MAX,
             motor_model: SpringModel::default(),
+            limits_enabled: false,
+            limits_local_axis1: Vector::x_axis(),
+            limits_local_axis2: Vector::x_axis(),
+            limits_angle: Real::MAX,
+            limits_impulse: 0.0,
         }
     }
 
     /// Can a SIMD constraint be used for resolving this joint?
     pub fn supports_simd_constraints(&self) -> bool {
-        // SIMD ball constraints don't support motors right now.
-        self.motor_max_impulse == 0.0 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0)
+        // SIMD ball constraints don't support motors and limits right now.
+        !self.limits_enabled
+            && (self.motor_max_impulse == 0.0
+                || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 53a82f5..6531a89 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -93,7 +93,7 @@ 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.
+        // SIMD revolute constraints don't support motors and limits right now.
         !self.limits_enabled
             && (self.motor_max_impulse == 0.0
                 || (self.motor_stiffness == 0.0 && self.motor_damping == 0.0))
@@ -142,6 +142,7 @@ impl RevoluteJoint {
         )
     }
 
+    /// Estimates the current position of the motor angle given the joint parameters.
     pub fn estimate_motor_angle_from_params(
         axis1: &Unit<Vector<Real>>,
         tangent1: &Vector<Real>,
-- 
cgit