Merge pull request #119 from dimforge/joint_drive

Add joint motors

24 files changed, 3267 insertions, 355 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 5d0d221..caad9b5 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -1,7 +1,7 @@
 use crate::math::Real;
 
 /// Parameters for a time-step of the physics engine.
-#[derive(Clone)]
+#[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct IntegrationParameters {
     /// The timestep length (default: `1.0 / 60.0`)
diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs
index 82e2a10..01b0f7f 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,31 @@ 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,
 }
 
 impl BallJoint {
@@ -29,6 +55,76 @@ 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(),
         }
     }
+
+    /// 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)
+    }
+
+    /// 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>,
+        stiffness: Real,
+        damping: Real,
+    ) {
+        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,
+        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;
+    }
+
+    /// Configure both the target orientation and target velocity of the motor.
+    #[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/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/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
new file mode 100644
index 0000000..c1549ff
--- /dev/null
+++ b/src/dynamics/joint/generic_joint.rs
@@ -0,0 +1,144 @@
+use crate::dynamics::{BallJoint, FixedJoint, PrismaticJoint, RevoluteJoint};
+use crate::math::{Isometry, Real, SpacialVector};
+use crate::na::{Rotation3, UnitQuaternion};
+
+#[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 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_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 {
+    /// 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(),
+            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 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;
+        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 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(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
+    }
+}
+
+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.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);
+        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)
+    }
+}
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 9fe6488..e0a9d38 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -17,6 +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),
 }
 
 impl JointParams {
@@ -26,8 +27,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 +51,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 +92,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 {
@@ -111,3 +128,16 @@ pub struct Joint {
     /// The joint geometric parameters and impulse.
     pub params: JointParams,
 }
+
+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(),
+            JointParams::FixedJoint(joint) => joint.supports_simd_constraints(),
+            JointParams::BallJoint(joint) => joint.supports_simd_constraints(),
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(joint) => joint.supports_simd_constraints(),
+        }
+    }
+}
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index b4dd60e..72a7483 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -1,16 +1,20 @@
 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};
 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 joint;
 mod joint_set;
 mod prismatic_joint;
 #[cfg(feature = "dim3")]
 mod revolute_joint;
+mod spring_model;
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 174ce79..3736b7f 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,
         }
     }
 
@@ -89,8 +106,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 +117,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()
@@ -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,
         }
     }
 
@@ -135,6 +155,12 @@ impl PrismaticJoint {
         self.local_axis2
     }
 
+    /// 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)
+    }
+
     // FIXME: precompute this?
     #[cfg(feature = "dim2")]
     pub(crate) fn local_frame1(&self) -> Isometry<Real> {
@@ -190,4 +216,33 @@ impl PrismaticJoint {
         let translation = self.local_anchor2.coords.into();
         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,
+        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 ad7db0d..d1181e9 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,30 @@ 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 +66,84 @@ 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::default(),
+            motor_last_angle: 0.0,
+        }
+    }
+
+    /// 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)
+    }
+
+    /// 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,
+        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..c2c9ebd
--- /dev/null
+++ b/src/dynamics/joint/spring_model.rs
@@ -0,0 +1,65 @@
+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 Default for SpringModel {
+    fn default() -> Self {
+        SpringModel::VelocityBased
+    }
+}
+
+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),
+        }
+    }
+}
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 8c38dc2..eab6916 100644
--- a/