From d9b6198fa0c7d933960030b7cff15cdaecb504e6 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Fri, 12 Feb 2021 16:00:57 +0100
Subject: Various generic joint fixes.

---
 src/dynamics/joint/generic_joint.rs | 86 +++++++++++++++++++++++++++++++------
 1 file changed, 72 insertions(+), 14 deletions(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index 2aa9a51..fa35520 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -1,4 +1,4 @@
-use crate::dynamics::RevoluteJoint;
+use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
 use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
 use crate::na::{Rotation3, UnitQuaternion};
 
@@ -24,11 +24,16 @@ pub struct GenericJoint {
 
     pub min_position: SpacialVector<Real>,
     pub max_position: SpacialVector<Real>,
-    pub target_velocity: SpacialVector<Real>,
-    /// The maximum negative impulse the joint can apply on each DoF. Must be <= 0.0
-    pub max_negative_impulse: SpacialVector<Real>,
+    pub min_velocity: SpacialVector<Real>,
+    pub max_velocity: SpacialVector<Real>,
+    /// The minimum negative impulse the joint can apply on each DoF. Must be <= 0.0
+    pub min_impulse: SpacialVector<Real>,
     /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
-    pub max_positive_impulse: SpacialVector<Real>,
+    pub max_impulse: SpacialVector<Real>,
+    /// The minimum negative position impulse the joint can apply on each DoF. Must be <= 0.0
+    pub min_pos_impulse: SpacialVector<Real>,
+    /// The maximum positive position impulse the joint can apply on each DoF. Must be >= 0.0
+    pub max_pos_impulse: SpacialVector<Real>,
 }
 
 impl GenericJoint {
@@ -40,25 +45,78 @@ impl GenericJoint {
             impulse: SpacialVector::zeros(),
             min_position: SpacialVector::zeros(),
             max_position: SpacialVector::zeros(),
-            target_velocity: SpacialVector::zeros(),
-            max_negative_impulse: SpacialVector::repeat(-Real::MAX),
-            max_positive_impulse: SpacialVector::repeat(Real::MAX),
+            min_velocity: SpacialVector::zeros(),
+            max_velocity: SpacialVector::zeros(),
+            min_impulse: SpacialVector::repeat(-Real::MAX),
+            max_impulse: SpacialVector::repeat(Real::MAX),
+            min_pos_impulse: SpacialVector::repeat(-Real::MAX),
+            max_pos_impulse: SpacialVector::repeat(Real::MAX),
         }
     }
+
+    pub fn free_dof(&mut self, dof: u8) {
+        self.min_position[dof as usize] = -Real::MAX;
+        self.max_position[dof as usize] = Real::MAX;
+        self.min_velocity[dof as usize] = -Real::MAX;
+        self.max_velocity[dof as usize] = Real::MAX;
+        self.min_impulse[dof as usize] = 0.0;
+        self.max_impulse[dof as usize] = 0.0;
+        self.min_pos_impulse[dof as usize] = 0.0;
+        self.max_pos_impulse[dof as usize] = 0.0;
+    }
+
+    pub fn set_dof_limits(&mut self, dof: u8, min: Real, max: Real) {
+        self.min_position[dof as usize] = min;
+        self.max_position[dof as usize] = max;
+    }
 }
 
 impl From<RevoluteJoint> for GenericJoint {
     fn from(joint: RevoluteJoint) -> Self {
-        let basis1 = [joint.local_axis1, joint.basis1[0], joint.basis1[1]];
-        let basis2 = [joint.local_axis2, joint.basis2[0], joint.basis2[1]];
-        let quat1 = UnitQuaternion::from_basis_unchecked(&basis1[..]);
-        let quat2 = UnitQuaternion::from_basis_unchecked(&basis2[..]);
+        let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
+        let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
+        let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
+        let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
         let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
         let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
 
         let mut result = Self::new(local_anchor1, local_anchor2);
-        result.min_position[3] = -Real::MAX;
-        result.max_position[3] = Real::MAX;
+        result.free_dof(3);
         result
     }
 }
+
+impl From<BallJoint> for GenericJoint {
+    fn from(joint: BallJoint) -> Self {
+        let local_anchor1 = Isometry::new(joint.local_anchor1.coords, na::zero());
+        let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
+
+        let mut result = Self::new(local_anchor1, local_anchor2);
+        result.free_dof(3);
+        result.free_dof(4);
+        result.free_dof(5);
+        result
+    }
+}
+
+impl From<PrismaticJoint> for GenericJoint {
+    fn from(joint: PrismaticJoint) -> Self {
+        let basis1 = [*joint.local_axis1, joint.basis1[0], joint.basis1[1]];
+        let basis2 = [*joint.local_axis2, joint.basis2[0], joint.basis2[1]];
+        let quat1 = UnitQuaternion::from_basis_unchecked(&basis1);
+        let quat2 = UnitQuaternion::from_basis_unchecked(&basis2);
+        let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
+        let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
+
+        let mut result = Self::new(local_anchor1, local_anchor2);
+        result.free_dof(0);
+        result.set_dof_limits(0, joint.limits[0], joint.limits[1]);
+        result
+    }
+}
+
+impl From<FixedJoint> for GenericJoint {
+    fn from(joint: FixedJoint) -> Self {
+        Self::new(joint.local_anchor1, joint.local_anchor2)
+    }
+}
-- 
cgit