Merge pull request #119 from dimforge/joint_drive

Add joint motors

28 files changed, 3517 insertions, 369 deletions

diff --git a/Cargo.toml b/Cargo.toml
index db1ac16..0b0cdd2 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -18,10 +18,11 @@ members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "e
 #nalgebra = { path = "../nalgebra" }
 
 #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
-#parry2d = { git = "https://github.com/sebcrozet/parry" }
-#parry3d = { git = "https://github.com/sebcrozet/parry" }
-#parry2d-f64 = { git = "https://github.com/sebcrozet/parry" }
-#parry3d-f64 = { git = "https://github.com/sebcrozet/parry" }
+nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }
+parry2d = { git = "https://github.com/dimforge/parry" }
+parry3d = { git = "https://github.com/dimforge/parry" }
+parry2d-f64 = { git = "https://github.com/dimforge/parry" }
+parry3d-f64 = { git = "https://github.com/dimforge/parry" }
 #ncollide2d = { git = "https://github.com/dimforge/ncollide" }
 #ncollide3d = { git = "https://github.com/dimforge/ncollide" }
 #nphysics2d = { git = "https://github.com/dimforge/nphysics" }
diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index d777655..1246bc4 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -1,7 +1,7 @@
-use na::{Isometry3, Point3, Unit, Vector3};
+use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3};
 use rapier3d::dynamics::{
     BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
-    RigidBodySet,
+    RigidBodyHandle, RigidBodySet,
 };
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -14,7 +14,7 @@ fn create_prismatic_joints(
     num: usize,
 ) {
     let rad = 0.4;
-    let shift = 1.0;
+    let shift = 2.0;
 
     let ground = RigidBodyBuilder::new_static()
         .translation(origin.x, origin.y, origin.z)
@@ -40,7 +40,7 @@ fn create_prismatic_joints(
 
         let z = Vector3::z();
         let mut prism = PrismaticJoint::new(
-            Point3::origin(),
+            Point3::new(0.0, 0.0, 0.0),
             axis,
             z,
             Point3::new(0.0, 0.0, -shift),
@@ -50,6 +50,74 @@ fn create_prismatic_joints(
         prism.limits_enabled = true;
         prism.limits[0] = -2.0;
         prism.limits[1] = 2.0;
+
+        joints.insert(bodies, curr_parent, curr_child, prism);
+
+        curr_parent = curr_child;
+    }
+}
+
+fn create_actuated_prismatic_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(origin.x, origin.y, origin.z)
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        let z = origin.z + (i + 1) as f32 * shift;
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(origin.x, origin.y, z)
+            .build();
+        let curr_child = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+        colliders.insert(collider, curr_child, bodies);
+
+        let axis = if i % 2 == 0 {
+            Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
+        } else {
+            Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
+        };
+
+        let z = Vector3::z();
+        let mut prism = PrismaticJoint::new(
+            Point3::new(0.0, 0.0, 0.0),
+            axis,
+            z,
+            Point3::new(0.0, 0.0, -shift),
+            axis,
+            z,
+        );
+
+        if i == 1 {
+            prism.configure_motor_velocity(1.0, 1.0);
+            prism.limits_enabled = true;
+            prism.limits[1] = 5.0;
+            // We set a max impulse so that the motor doesn't fight
+            // the limits with large forces.
+            prism.motor_max_impulse = 1.0;
+        } else if i > 1 {
+            prism.configure_motor_position(2.0, 0.2, 1.0);
+        } else {
+            prism.configure_motor_velocity(1.0, 1.0);
+            // We set a max impulse so that the motor doesn't fight
+            // the limits with large forces.
+            prism.motor_max_impulse = 1.0;
+            prism.limits_enabled = true;
+            prism.limits[0] = -2.0;
+            prism.limits[1] = 5.0;
+        }
+
         joints.insert(bodies, curr_parent, curr_child, prism);
 
         curr_parent = curr_child;
@@ -222,6 +290,130 @@ fn create_ball_joints(
     }
 }
 
+fn create_actuated_revolute_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    // We will reuse this base configuration for all the joints here.
+    let joint_template = RevoluteJoint::new(
+        Point3::origin(),
+        Vector3::z_axis(),
+        Point3::new(0.0, 0.0, -shift),
+        Vector3::z_axis(),
+    );
+
+    let mut parent_handle = RigidBodyHandle::invalid();
+
+    for i in 0..num {
+        let fi = i as f32;
+
+        // NOTE: the num - 2 test is to avoid two consecutive
+        // fixed bodies. Because physx will crash if we add
+        // a joint between these.
+        let status = if i == 0 {
+            BodyStatus::Static
+        } else {
+            BodyStatus::Dynamic
+        };
+
+        let shifty = (i >= 1) as u32 as f32 * -2.0;
+
+        let rigid_body = RigidBodyBuilder::new(status)
+            .translation(origin.x, origin.y + shifty, origin.z + fi * shift)
+            // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
+            .build();
+
+        let child_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build();
+        colliders.insert(collider, child_handle, bodies);
+
+        if i > 0 {
+            let mut joint = joint_template.clone();
+
+            if i % 3 == 1 {
+                joint.configure_motor_velocity(-20.0, 0.1);
+            } else if i == num - 1 {
+                let stiffness = 0.2;
+                let damping = 1.0;
+                joint.configure_motor_position(3.14 / 2.0, stiffness, damping);
+            }
+
+            if i == 1 {
+                joint.local_anchor2.y = 2.0;
+                joint.configure_motor_velocity(-2.0, 0.1);
+            }
+
+            joints.insert(bodies, parent_handle, child_handle, joint);
+        }
+
+        parent_handle = child_handle;
+    }
+}
+
+fn create_actuated_ball_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    // We will reuse this base configuration for all the joints here.
+    let joint_template = BallJoint::new(Point3::new(0.0, 0.0, shift), Point3::origin());
+
+    let mut parent_handle = RigidBodyHandle::invalid();
+
+    for i in 0..num {
+        let fi = i as f32;
+
+        // NOTE: the num - 2 test is to avoid two consecutive
+        // fixed bodies. Because physx will crash if we add
+        // a joint between these.
+        let status = if i == 0 {
+            BodyStatus::Static
+        } else {
+            BodyStatus::Dynamic
+        };
+
+        let rigid_body = RigidBodyBuilder::new(status)
+            .translation(origin.x, origin.y, origin.z + fi * shift)
+            // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
+            .build();
+
+        let child_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build();
+        colliders.insert(collider, child_handle, bodies);
+
+        if i > 0 {
+            let mut joint = joint_template.clone();
+
+            if i == 1 {
+                joint.configure_motor_velocity(Vector3::new(0.0, 0.5, -2.0), 0.1);
+            } else if i == num - 1 {
+                let stiffness = 0.2;
+                let damping = 1.0;
+                joint.configure_motor_position(
+                    UnitQuaternion::new(Vector3::new(0.0, 1.0, 3.14 / 2.0)),
+                    stiffness,
+                    damping,
+                );
+            }
+
+            joints.insert(bodies, parent_handle, child_handle, joint);
+        }
+
+        parent_handle = child_handle;
+    }
+}
+
 pub fn init_world(testbed: &mut Testbed) {
     /*
      * World
@@ -234,8 +426,15 @@ pub fn init_world(testbed: &mut Testbed) {
         &mut bodies,
         &mut colliders,
         &mut joints,
-        Point3::new(20.0, 10.0, 0.0),
-        5,
+        Point3::new(20.0, 5.0, 0.0),
+        4,
+    );
+    create_actuated_prismatic_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(25.0, 5.0, 0.0),
+        4,
     );
     create_revolute_joints(
         &mut bodies,
@@ -249,7 +448,21 @@ pub fn init_world(testbed: &mut Testbed) {
         &mut colliders,
         &mut joints,
         Point3::new(0.0, 10.0, 0.0),
-        5,
+        10,
+    );
+    create_actuated_revolute_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(20.0, 10.0, 0.0),
+        6,
+    );
+    create_actuated_ball_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(13.0, 10.0, 0.0),
+        3,
     );
     create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
 
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(),
             ]