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.

---
 src/dynamics/joint/generic_joint.rs | 46 +++++++++++++++++++++++++++++++++++++
 src/dynamics/joint/joint.rs         | 23 +++++++++++++++++--
 src/dynamics/joint/mod.rs           |  2 ++
 3 files changed, 69 insertions(+), 2 deletions(-)
 create mode 100644 src/dynamics/joint/generic_joint.rs

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
new file mode 100644
index 0000000..cfea537
--- /dev/null
+++ b/src/dynamics/joint/generic_joint.rs
@@ -0,0 +1,46 @@
+use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that prevents all relative movement between two bodies.
+///
+/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
+pub struct GenericJoint {
+    /// The frame of reference for the first body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor1: Isometry<Real>,
+    /// The frame of reference for the second body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor2: Isometry<Real>,
+    /// The impulse applied to the first body affected by this joint.
+    ///
+    /// The impulse applied to the second body affected by this joint is given by `-impulse`.
+    /// This combines both linear and angular impulses:
+    /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
+    /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
+    pub impulse: SpacialVector<Real>,
+
+    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>,
+    /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
+    pub max_positive_impulse: SpacialVector<Real>,
+}
+
+impl GenericJoint {
+    /// Creates a new fixed joint from the frames of reference of both bodies.
+    pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            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),
+        }
+    }
+}
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 9fe6488..31c5e0a 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -1,6 +1,8 @@
 #[cfg(feature = "dim3")]
 use crate::dynamics::RevoluteJoint;
-use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
+use crate::dynamics::{
+    BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle,
+};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -17,6 +19,7 @@ pub enum JointParams {
     /// A revolute joint that removes all degrees of degrees of freedom between the affected
     /// bodies except for the translation along one axis.
     RevoluteJoint(RevoluteJoint),
+    GenericJoint(GenericJoint),
 }
 
 impl JointParams {
@@ -26,8 +29,9 @@ impl JointParams {
             JointParams::BallJoint(_) => 0,
             JointParams::FixedJoint(_) => 1,
             JointParams::PrismaticJoint(_) => 2,
+            JointParams::GenericJoint(_) => 3,
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => 3,
+            JointParams::RevoluteJoint(_) => 4,
         }
     }
 
@@ -49,6 +53,15 @@ impl JointParams {
         }
     }
 
+    /// Gets a reference to the underlying generic joint, if `self` is one.
+    pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
+        if let JointParams::GenericJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
     /// Gets a reference to the underlying prismatic joint, if `self` is one.
     pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
         if let JointParams::PrismaticJoint(j) = self {
@@ -81,6 +94,12 @@ impl From<FixedJoint> for JointParams {
     }
 }
 
+impl From<GenericJoint> for JointParams {
+    fn from(j: GenericJoint) -> Self {
+        JointParams::GenericJoint(j)
+    }
+}
+
 #[cfg(feature = "dim3")]
 impl From<RevoluteJoint> for JointParams {
     fn from(j: RevoluteJoint) -> Self {
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index b4dd60e..92dd715 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -1,5 +1,6 @@
 pub use self::ball_joint::BallJoint;
 pub use self::fixed_joint::FixedJoint;
+pub use self::generic_joint::GenericJoint;
 pub use self::joint::{Joint, JointParams};
 pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
 pub use self::joint_set::{JointHandle, JointSet};
@@ -9,6 +10,7 @@ pub use self::revolute_joint::RevoluteJoint;
 
 mod ball_joint;
 mod fixed_joint;
+mod generic_joint;
 mod joint;
 mod joint_set;
 mod prismatic_joint;
-- 
cgit 


From cc80e40067d100d0f519c9a20abb020726dd8514 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Thu, 11 Feb 2021 18:52:07 +0100
Subject: More experiments with the way the generic joint is stabilized.

---
 src/dynamics/joint/generic_joint.rs | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index cfea537..2aa9a51 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -1,4 +1,6 @@
+use crate::dynamics::RevoluteJoint;
 use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
+use crate::na::{Rotation3, UnitQuaternion};
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -44,3 +46,19 @@ impl GenericJoint {
         }
     }
 }
+
+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 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
+    }
+}
-- 
cgit 


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 


From de39a41faa2ea722042231f91b5d579efdf1a02d Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Mon, 15 Feb 2021 11:20:09 +0100
Subject: Implement non-linear position stabilization for the generic
 constraint.

---
 src/dynamics/joint/prismatic_joint.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 174ce79..0edc939 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -89,8 +89,8 @@ impl PrismaticJoint {
             Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
         {
             [
-                local_bitangent1.into_inner(),
                 local_bitangent1.cross(&local_axis1),
+                local_bitangent1.into_inner(),
             ]
         } else {
             local_axis1.orthonormal_basis()
@@ -100,8 +100,8 @@ impl PrismaticJoint {
             Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3)
         {
             [
-                local_bitangent2.into_inner(),
                 local_bitangent2.cross(&local_axis2),
+                local_bitangent2.into_inner(),
             ]
         } else {
             local_axis2.orthonormal_basis()
-- 
cgit 


From e9f17f32e8dda4b97d2eb7b2118b7373d0c554d0 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Fri, 19 Feb 2021 15:21:25 +0100
Subject: Complete the implementation of non-simd joint motor for the revolute
 joint.

---
 src/dynamics/joint/generic_joint.rs  | 22 +++++++++
 src/dynamics/joint/joint.rs          | 34 ++++++-------
 src/dynamics/joint/mod.rs            |  6 ++-
 src/dynamics/joint/revolute_joint.rs | 95 +++++++++++++++++++++++++++++++++++-
 src/dynamics/joint/spring_model.rs   | 59 ++++++++++++++++++++++
 5 files changed, 194 insertions(+), 22 deletions(-)
 create mode 100644 src/dynamics/joint/spring_model.rs

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index fa35520..5d776b0 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -54,6 +54,13 @@ impl GenericJoint {
         }
     }
 
+    pub fn set_dof_vel(&mut self, dof: u8, target_vel: Real, max_force: Real) {
+        self.min_velocity[dof as usize] = target_vel;
+        self.max_velocity[dof as usize] = target_vel;
+        self.min_impulse[dof as usize] = -max_force;
+        self.max_impulse[dof as usize] = max_force;
+    }
+
     pub fn free_dof(&mut self, dof: u8) {
         self.min_position[dof as usize] = -Real::MAX;
         self.max_position[dof as usize] = Real::MAX;
@@ -82,6 +89,18 @@ impl From<RevoluteJoint> for GenericJoint {
 
         let mut result = Self::new(local_anchor1, local_anchor2);
         result.free_dof(3);
+
+        if joint.motor_damping != 0.0 {
+            result.set_dof_vel(3, joint.motor_target_vel, joint.motor_max_impulse);
+        }
+
+        result.impulse[0] = joint.impulse[0];
+        result.impulse[1] = joint.impulse[1];
+        result.impulse[2] = joint.impulse[2];
+        result.impulse[3] = joint.motor_impulse;
+        result.impulse[4] = joint.impulse[3];
+        result.impulse[5] = joint.impulse[4];
+
         result
     }
 }
@@ -92,6 +111,9 @@ impl From<BallJoint> for GenericJoint {
         let local_anchor2 = Isometry::new(joint.local_anchor2.coords, na::zero());
 
         let mut result = Self::new(local_anchor1, local_anchor2);
+        result.impulse[0] = joint.impulse[0];
+        result.impulse[1] = joint.impulse[1];
+        result.impulse[2] = joint.impulse[2];
         result.free_dof(3);
         result.free_dof(4);
         result.free_dof(5);
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 31c5e0a..fe77b4b 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -1,8 +1,6 @@
 #[cfg(feature = "dim3")]
 use crate::dynamics::RevoluteJoint;
-use crate::dynamics::{
-    BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle,
-};
+use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -19,7 +17,7 @@ pub enum JointParams {
     /// A revolute joint that removes all degrees of degrees of freedom between the affected
     /// bodies except for the translation along one axis.
     RevoluteJoint(RevoluteJoint),
-    GenericJoint(GenericJoint),
+    // GenericJoint(GenericJoint),
 }
 
 impl JointParams {
@@ -29,7 +27,7 @@ impl JointParams {
             JointParams::BallJoint(_) => 0,
             JointParams::FixedJoint(_) => 1,
             JointParams::PrismaticJoint(_) => 2,
-            JointParams::GenericJoint(_) => 3,
+            // JointParams::GenericJoint(_) => 3,
             #[cfg(feature = "dim3")]
             JointParams::RevoluteJoint(_) => 4,
         }
@@ -53,14 +51,14 @@ impl JointParams {
         }
     }
 
-    /// Gets a reference to the underlying generic joint, if `self` is one.
-    pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
-        if let JointParams::GenericJoint(j) = self {
-            Some(j)
-        } else {
-            None
-        }
-    }
+    // /// Gets a reference to the underlying generic joint, if `self` is one.
+    // pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
+    //     if let JointParams::GenericJoint(j) = self {
+    //         Some(j)
+    //     } else {
+    //         None
+    //     }
+    // }
 
     /// Gets a reference to the underlying prismatic joint, if `self` is one.
     pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
@@ -94,11 +92,11 @@ impl From<FixedJoint> for JointParams {
     }
 }
 
-impl From<GenericJoint> for JointParams {
-    fn from(j: GenericJoint) -> Self {
-        JointParams::GenericJoint(j)
-    }
-}
+// impl From<GenericJoint> for JointParams {
+//     fn from(j: GenericJoint) -> Self {
+//         JointParams::GenericJoint(j)
+//     }
+// }
 
 #[cfg(feature = "dim3")]
 impl From<RevoluteJoint> for JointParams {
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index 92dd715..72a7483 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -1,18 +1,20 @@
 pub use self::ball_joint::BallJoint;
 pub use self::fixed_joint::FixedJoint;
-pub use self::generic_joint::GenericJoint;
+// pub use self::generic_joint::GenericJoint;
 pub use self::joint::{Joint, JointParams};
 pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
 pub use self::joint_set::{JointHandle, JointSet};
 pub use self::prismatic_joint::PrismaticJoint;
 #[cfg(feature = "dim3")]
 pub use self::revolute_joint::RevoluteJoint;
+pub use self::spring_model::SpringModel;
 
 mod ball_joint;
 mod fixed_joint;
-mod generic_joint;
+// mod generic_joint;
 mod joint;
 mod joint_set;
 mod prismatic_joint;
 #[cfg(feature = "dim3")]
 mod revolute_joint;
+mod spring_model;
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index ad7db0d..9af0ae6 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -1,6 +1,7 @@
-use crate::math::{Point, Real, Vector};
+use crate::dynamics::SpringModel;
+use crate::math::{Isometry, Point, Real, Vector};
 use crate::utils::WBasis;
-use na::{Unit, Vector5};
+use na::{RealField, Unit, Vector5};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -22,6 +23,28 @@ pub struct RevoluteJoint {
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     pub impulse: Vector5<Real>,
+    /// The target relative angular velocity the motor will attempt to reach.
+    pub motor_target_vel: Real,
+    /// The target relative angle along the joint axis the motor will attempt to reach.
+    pub motor_target_pos: Real,
+    /// The motor's stiffness.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_stiffness: Real,
+    /// The motor's damping.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_damping: Real,
+    /// The maximal impulse the motor is able to deliver.
+    pub motor_max_impulse: Real,
+    /// The angular impulse applied by the motor.
+    pub motor_impulse: Real,
+    /// The spring-like model used by the motor to reach the target velocity and .
+    pub motor_model: SpringModel,
+    // Used to handle cases where the position target ends up being more than pi radians away.
+    pub(crate) motor_last_angle: Real,
+    // The angular impulse expressed in world-space.
+    pub(crate) world_ang_impulse: Vector<Real>,
+    // The world-space orientation of the free axis of the first attached body.
+    pub(crate) prev_axis1: Vector<Real>,
 }
 
 impl RevoluteJoint {
@@ -41,6 +64,74 @@ impl RevoluteJoint {
             basis1: local_axis1.orthonormal_basis(),
             basis2: local_axis2.orthonormal_basis(),
             impulse: na::zero(),
+            world_ang_impulse: na::zero(),
+            motor_target_vel: 0.0,
+            motor_target_pos: 0.0,
+            motor_stiffness: 0.0,
+            motor_damping: 0.0,
+            motor_max_impulse: Real::MAX,
+            motor_impulse: 0.0,
+            prev_axis1: *local_axis1,
+            motor_model: SpringModel::VelocityBased,
+            motor_last_angle: 0.0,
         }
     }
+
+    pub fn configure_motor_model(&mut self, model: SpringModel) {
+        self.motor_model = model;
+    }
+
+    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
+        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    }
+
+    pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
+        self.configure_motor(target_pos, 0.0, stiffness, damping)
+    }
+
+    pub fn configure_motor(
+        &mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        self.motor_target_vel = target_vel;
+        self.motor_target_pos = target_pos;
+        self.motor_stiffness = stiffness;
+        self.motor_damping = damping;
+    }
+
+    /// Estimates the current position of the motor angle.
+    pub fn estimate_motor_angle(
+        &self,
+        body_pos1: &Isometry<Real>,
+        body_pos2: &Isometry<Real>,
+    ) -> Real {
+        let motor_axis1 = body_pos1 * self.local_axis1;
+        let ref1 = body_pos1 * self.basis1[0];
+        let ref2 = body_pos2 * self.basis2[0];
+
+        let last_angle_cycles = (self.motor_last_angle / Real::two_pi()).trunc() * Real::two_pi();
+
+        // Measure the position between 0 and 2-pi
+        let new_angle = if ref1.cross(&ref2).dot(&motor_axis1) < 0.0 {
+            Real::two_pi() - ref1.angle(&ref2)
+        } else {
+            ref1.angle(&ref2)
+        };
+
+        // The last angle between 0 and 2-pi
+        let last_angle_zero_two_pi = self.motor_last_angle - last_angle_cycles;
+
+        // Figure out the smallest angle differance.
+        let mut angle_diff = new_angle - last_angle_zero_two_pi;
+        if angle_diff > Real::pi() {
+            angle_diff -= Real::two_pi()
+        } else if angle_diff < -Real::pi() {
+            angle_diff += Real::two_pi()
+        }
+
+        self.motor_last_angle + angle_diff
+    }
 }
diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs
new file mode 100644
index 0000000..dd6035d
--- /dev/null
+++ b/src/dynamics/joint/spring_model.rs
@@ -0,0 +1,59 @@
+use crate::math::Real;
+
+/// The spring-like model used for constraints resolution.
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum SpringModel {
+    /// No equation is solved.
+    Disabled,
+    /// The solved spring-like equation is:
+    /// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
+    ///
+    /// Here the `stiffness` is the ratio of position error to be solved at each timestep (like
+    /// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at
+    /// each timestep.
+    VelocityBased,
+    /// The solved spring-like equation is:
+    /// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
+    AccelerationBased,
+    /// The solved spring-like equation is:
+    /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
+    ForceBased,
+}
+
+impl SpringModel {
+    /// Combines the coefficients used for solving the spring equation.
+    ///
+    /// Returns the new coefficients (stiffness, damping, inv_lhs_scale, keep_inv_lhs)
+    /// coefficients for the equivalent impulse-based equation. These new
+    /// coefficients must be used in the following way:
+    /// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`.
+    /// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`.
+    /// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero.
+    pub fn combine_coefficients(
+        self,
+        dt: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> (Real, Real, Real, bool) {
+        match self {
+            SpringModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
+            SpringModel::AccelerationBased => {
+                let effective_stiffness = stiffness * dt;
+                let effective_damping = damping * dt;
+                // TODO: Using gamma behaves very badly for some reasons.
+                // Maybe I got the formulation wrong, so let's keep it to 1.0 for now,
+                // and get back to this later.
+                // let gamma = effective_stiffness * dt + effective_damping;
+                (effective_stiffness, effective_damping, 1.0, true)
+            }
+            SpringModel::ForceBased => {
+                let effective_stiffness = stiffness * dt;
+                let effective_damping = damping * dt;
+                let gamma = effective_stiffness * dt + effective_damping;
+                (effective_stiffness, effective_damping, gamma, false)
+            }
+            SpringModel::Disabled => return (0.0, 0.0, 0.0, false),
+        }
+    }
+}
-- 
cgit 


From dc8ccc0c30e75aea8f144dbfad16be088d4d4ea2 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Fri, 19 Feb 2021 17:32:09 +0100
Subject: Make revolute joint actuation work properly even when SIMD is
 enabled.

---
 src/dynamics/joint/ball_joint.rs      |  5 +++++
 src/dynamics/joint/fixed_joint.rs     |  5 +++++
 src/dynamics/joint/joint.rs           | 11 +++++++++++
 src/dynamics/joint/prismatic_joint.rs |  5 +++++
 src/dynamics/joint/revolute_joint.rs  |  6 ++++++
 5 files changed, 32 insertions(+)

(limited to 'src/dynamics/joint')

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;
     }
-- 
cgit 


From f5515c39736aced54f65879a42b2a74a68609ee7 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Sun, 21 Feb 2021 17:14:34 +0100
Subject: Fix lever-arm handling in the revolute joint.

---
 src/dynamics/joint/revolute_joint.rs | 2 +-
 src/dynamics/joint/spring_model.rs   | 6 ++++++
 2 files changed, 7 insertions(+), 1 deletion(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 022779d..e1e1441 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -72,7 +72,7 @@ impl RevoluteJoint {
             motor_max_impulse: Real::MAX,
             motor_impulse: 0.0,
             prev_axis1: *local_axis1,
-            motor_model: SpringModel::VelocityBased,
+            motor_model: SpringModel::default(),
             motor_last_angle: 0.0,
         }
     }
diff --git a/src/dynamics/joint/spring_model.rs b/src/dynamics/joint/spring_model.rs
index dd6035d..c2c9ebd 100644
--- a/src/dynamics/joint/spring_model.rs
+++ b/src/dynamics/joint/spring_model.rs
@@ -21,6 +21,12 @@ pub enum SpringModel {
     ForceBased,
 }
 
+impl Default for SpringModel {
+    fn default() -> Self {
+        SpringModel::VelocityBased
+    }
+}
+
 impl SpringModel {
     /// Combines the coefficients used for solving the spring equation.
     ///
-- 
cgit 


From 01496d43e5a39a7348578faa4e6c2fcf1793785b Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Sun, 21 Feb 2021 17:15:00 +0100
Subject: Add motors to ball joints.

---
 src/dynamics/joint/ball_joint.rs | 92 +++++++++++++++++++++++++++++++++++++++-
 src/dynamics/joint/joint.rs      |  3 +-
 2 files changed, 92 insertions(+), 3 deletions(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index f1e8fdf..47c90ab 100644
--- a/src/dynamics/joint/ball_joint.rs
+++ b/src/dynamics/joint/ball_joint.rs
@@ -1,4 +1,5 @@
-use crate::math::{Point, Real, Vector};
+use crate::dynamics::SpringModel;
+use crate::math::{Point, Real, Rotation, Vector};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -12,6 +13,33 @@ pub struct BallJoint {
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     pub impulse: Vector<Real>,
+
+    /// The target relative angular velocity the motor will attempt to reach.
+    #[cfg(feature = "dim2")]
+    pub motor_target_vel: Real,
+    /// The target relative angular velocity the motor will attempt to reach.
+    #[cfg(feature = "dim3")]
+    pub motor_target_vel: Vector<Real>,
+    /// The target angular position of this joint, expressed as an axis-angle.
+    pub motor_target_pos: Rotation<Real>,
+    /// The motor's stiffness.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_stiffness: Real,
+    /// The motor's damping.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_damping: Real,
+    /// The maximal impulse the motor is able to deliver.
+    pub motor_max_impulse: Real,
+    /// The angular impulse applied by the motor.
+    #[cfg(feature = "dim2")]
+    pub motor_impulse: Real,
+    /// The angular impulse applied by the motor.
+    #[cfg(feature = "dim3")]
+    pub motor_impulse: Vector<Real>,
+    /// The spring-like model used by the motor to reach the target velocity and .
+    pub motor_model: SpringModel,
+    // Used to handle cases where the position target ends up being more than pi radians away.
+    pub(crate) motor_last_angle: Real,
 }
 
 impl BallJoint {
@@ -29,11 +57,71 @@ impl BallJoint {
             local_anchor1,
             local_anchor2,
             impulse,
+            motor_target_vel: na::zero(),
+            motor_target_pos: Rotation::identity(),
+            motor_stiffness: 0.0,
+            motor_damping: 0.0,
+            motor_impulse: na::zero(),
+            motor_max_impulse: Real::MAX,
+            motor_model: SpringModel::default(),
+            motor_last_angle: 0.0,
         }
     }
 
     /// Can a SIMD constraint be used for resolving this joint?
     pub fn supports_simd_constraints(&self) -> bool {
-        true
+        // 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)
+    }
+
+    pub fn configure_motor_model(&mut self, model: SpringModel) {
+        self.motor_model = model;
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
+        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn configure_motor_velocity(&mut self, target_vel: Vector<Real>, factor: Real) {
+        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    }
+
+    pub fn configure_motor_position(
+        &mut self,
+        target_pos: Rotation<Real>,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        self.configure_motor(target_pos, na::zero(), stiffness, damping)
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn configure_motor(
+        &mut self,
+        target_pos: Rotation<Real>,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        self.motor_target_vel = target_vel;
+        self.motor_target_pos = target_pos;
+        self.motor_stiffness = stiffness;
+        self.motor_damping = damping;
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn configure_motor(
+        &mut self,
+        target_pos: Rotation<Real>,
+        target_vel: Vector<Real>,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        self.motor_target_vel = target_vel;
+        self.motor_target_pos = target_pos;
+        self.motor_stiffness = stiffness;
+        self.motor_damping = damping;
     }
 }
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index b4089ae..290c34c 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -132,10 +132,11 @@ pub struct Joint {
 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(),
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(),
         }
     }
 }
-- 
cgit 


From aaba6c8927b0cdd0de7739b37f2bdfe267074dbb Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Mon, 22 Feb 2021 12:12:24 +0100
Subject: Add motors to prismatic joints.

---
 src/dynamics/joint/prismatic_joint.rs | 72 ++++++++++++++++++++++++++++-------
 src/dynamics/joint/revolute_joint.rs  |  2 +
 2 files changed, 61 insertions(+), 13 deletions(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 6e32a01..4f5c984 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -1,3 +1,4 @@
+use crate::dynamics::SpringModel;
 use crate::math::{Isometry, Point, Real, Vector, DIM};
 use crate::utils::WBasis;
 use na::Unit;
@@ -36,10 +37,23 @@ pub struct PrismaticJoint {
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     pub limits_impulse: Real,
-    // pub motor_enabled: bool,
-    // pub target_motor_vel: Real,
-    // pub max_motor_impulse: Real,
-    // pub motor_impulse: Real,
+
+    /// The target relative angular velocity the motor will attempt to reach.
+    pub motor_target_vel: Real,
+    /// The target relative angle along the joint axis the motor will attempt to reach.
+    pub motor_target_pos: Real,
+    /// The motor's stiffness.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_stiffness: Real,
+    /// The motor's damping.
+    /// See the documentation of `SpringModel` for more information on this parameter.
+    pub motor_damping: Real,
+    /// The maximal impulse the motor is able to deliver.
+    pub motor_max_impulse: Real,
+    /// The angular impulse applied by the motor.
+    pub motor_impulse: Real,
+    /// The spring-like model used by the motor to reach the target velocity and .
+    pub motor_model: SpringModel,
 }
 
 impl PrismaticJoint {
@@ -63,10 +77,13 @@ impl PrismaticJoint {
             limits_enabled: false,
             limits: [-Real::MAX, Real::MAX],
             limits_impulse: 0.0,
-            // motor_enabled: false,
-            // target_motor_vel: 0.0,
-            // max_motor_impulse: Real::MAX,
-            // motor_impulse: 0.0,
+            motor_target_vel: 0.0,
+            motor_target_pos: 0.0,
+            motor_stiffness: 0.0,
+            motor_damping: 0.0,
+            motor_max_impulse: Real::MAX,
+            motor_impulse: 0.0,
+            motor_model: SpringModel::VelocityBased,
         }
     }
 
@@ -118,10 +135,13 @@ impl PrismaticJoint {
             limits_enabled: false,
             limits: [-Real::MAX, Real::MAX],
             limits_impulse: 0.0,
-            // motor_enabled: false,
-            // target_motor_vel: 0.0,
-            // max_motor_impulse: Real::MAX,
-            // motor_impulse: 0.0,
+            motor_target_vel: 0.0,
+            motor_target_pos: 0.0,
+            motor_stiffness: 0.0,
+            motor_damping: 0.0,
+            motor_max_impulse: Real::MAX,
+            motor_impulse: 0.0,
+            motor_model: SpringModel::VelocityBased,
         }
     }
 
@@ -137,7 +157,8 @@ impl PrismaticJoint {
 
     /// Can a SIMD constraint be used for resolving this joint?
     pub fn supports_simd_constraints(&self) -> bool {
-        true
+        // 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)
     }
 
     // FIXME: precompute this?
@@ -195,4 +216,29 @@ impl PrismaticJoint {
         let translation = self.local_anchor2.coords.into();
         Isometry::from_parts(translation, rotation)
     }
+
+    pub fn configure_motor_model(&mut self, model: SpringModel) {
+        self.motor_model = model;
+    }
+
+    pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
+        self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
+    }
+
+    pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
+        self.configure_motor(target_pos, 0.0, stiffness, damping)
+    }
+
+    pub fn configure_motor(
+        &mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        self.motor_target_vel = target_vel;
+        self.motor_target_pos = target_pos;
+        self.motor_stiffness = stiffness;
+        self.motor_damping = damping;
+    }
 }
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index e1e1441..6895d3d 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -23,6 +23,7 @@ pub struct RevoluteJoint {
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     pub impulse: Vector5<Real>,
+
     /// The target relative angular velocity the motor will attempt to reach.
     pub motor_target_vel: Real,
     /// The target relative angle along the joint axis the motor will attempt to reach.
@@ -39,6 +40,7 @@ pub struct RevoluteJoint {
     pub motor_impulse: Real,
     /// The spring-like model used by the motor to reach the target velocity and .
     pub motor_model: SpringModel,
+
     // Used to handle cases where the position target ends up being more than pi radians away.
     pub(crate) motor_last_angle: Real,
     // The angular impulse expressed in world-space.
-- 
cgit 


From 0eec28325ecf192f386a6894526e97a462e68802 Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Mon, 22 Feb 2021 14:20:06 +0100
Subject: Fix warnings.

---
 src/dynamics/joint/ball_joint.rs      | 6 ++++++
 src/dynamics/joint/generic_joint.rs   | 2 +-
 src/dynamics/joint/joint.rs           | 1 +
 src/dynamics/joint/prismatic_joint.rs | 4 ++++
 src/dynamics/joint/revolute_joint.rs  | 4 ++++
 5 files changed, 16 insertions(+), 1 deletion(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index 47c90ab..8481961 100644
--- a/src/dynamics/joint/ball_joint.rs
+++ b/src/dynamics/joint/ball_joint.rs
@@ -74,20 +74,24 @@ impl BallJoint {
         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.
     pub fn configure_motor_model(&mut self, model: SpringModel) {
         self.motor_model = model;
     }
 
+    /// Sets the target velocity and velocity correction factor this motor.
     #[cfg(feature = "dim2")]
     pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
         self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
     }
 
+    /// Sets the target velocity and velocity correction factor this motor.
     #[cfg(feature = "dim3")]
     pub fn configure_motor_velocity(&mut self, target_vel: Vector<Real>, factor: Real) {
         self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
     }
 
+    /// Sets the target orientation this motor needs to reach.
     pub fn configure_motor_position(
         &mut self,
         target_pos: Rotation<Real>,
@@ -97,6 +101,7 @@ impl BallJoint {
         self.configure_motor(target_pos, na::zero(), stiffness, damping)
     }
 
+    /// Sets the target orientation this motor needs to reach.
     #[cfg(feature = "dim2")]
     pub fn configure_motor(
         &mut self,
@@ -111,6 +116,7 @@ impl BallJoint {
         self.motor_damping = damping;
     }
 
+    /// Configure both the target orientation and target velocity of the motor.
     #[cfg(feature = "dim3")]
     pub fn configure_motor(
         &mut self,
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index 5d776b0..c1549ff 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
-use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
+use crate::math::{Isometry, Real, SpacialVector};
 use crate::na::{Rotation3, UnitQuaternion};
 
 #[derive(Copy, Clone, Debug)]
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 290c34c..e0a9d38 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -130,6 +130,7 @@ pub struct Joint {
 }
 
 impl Joint {
+    /// Can this joint use SIMD-accelerated constraint formulations?
     pub fn supports_simd_constraints(&self) -> bool {
         match &self.params {
             JointParams::PrismaticJoint(joint) => joint.supports_simd_constraints(),
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 4f5c984..3736b7f 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -217,18 +217,22 @@ impl PrismaticJoint {
         Isometry::from_parts(translation, rotation)
     }
 
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
     pub fn configure_motor_model(&mut self, model: SpringModel) {
         self.motor_model = model;
     }
 
+    /// Sets the target velocity this motor needs to reach.
     pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
         self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
     }
 
+    /// Sets the target position this motor needs to reach.
     pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
         self.configure_motor(target_pos, 0.0, stiffness, damping)
     }
 
+    /// Configure both the target position and target velocity of the motor.
     pub fn configure_motor(
         &mut self,
         target_pos: Real,
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 6895d3d..d1181e9 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -85,18 +85,22 @@ impl RevoluteJoint {
         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.
     pub fn configure_motor_model(&mut self, model: SpringModel) {
         self.motor_model = model;
     }
 
+    /// Sets the target velocity this motor needs to reach.
     pub fn configure_motor_velocity(&mut self, target_vel: Real, factor: Real) {
         self.configure_motor(self.motor_target_pos, target_vel, 0.0, factor)
     }
 
+    /// Sets the target angle this motor needs to reach.
     pub fn configure_motor_position(&mut self, target_pos: Real, stiffness: Real, damping: Real) {
         self.configure_motor(target_pos, 0.0, stiffness, damping)
     }
 
+    /// Configure both the target angle and target velocity of the motor.
     pub fn configure_motor(
         &mut self,
         target_pos: Real,
-- 
cgit 


From e5c4c2e8ffccf81aa5436c166b426a01b8b8831e Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Mon, 22 Feb 2021 15:06:33 +0100
Subject: Ball joint: remove unused field.

---
 src/dynamics/joint/ball_joint.rs | 3 ---
 1 file changed, 3 deletions(-)

(limited to 'src/dynamics/joint')

diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index 8481961..01b0f7f 100644
--- a/src/dynamics/joint/ball_joint.rs
+++ b/src/dynamics/joint/ball_joint.rs
@@ -38,8 +38,6 @@ 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,
-    // Used to handle cases where the position target ends up being more than pi radians away.
-    pub(crate) motor_last_angle: Real,
 }
 
 impl BallJoint {
@@ -64,7 +62,6 @@ impl BallJoint {
             motor_impulse: na::zero(),
             motor_max_impulse: Real::MAX,
             motor_model: SpringModel::default(),
-            motor_last_angle: 0.0,
         }
     }
 
-- 
cgit