Merge pull request #119 from dimforge/joint_drive

Add joint motors

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" }
+nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }
-#parry3d = { git = "https://github.com/sebcrozet/parry" }
+parry2d = { git = "https://github.com/dimforge/parry" }
-#parry2d-f64 = { git = "https://github.com/sebcrozet/parry" }
+parry3d = { git = "https://github.com/dimforge/parry" }
-#parry3d-f64 = { git = "https://github.com/sebcrozet/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" }

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),
+        Point3::new(20.0, 5.0, 0.0),
-        5,
+        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);
 

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`)

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

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
+    }
 }

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)
+    }
+}

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(),
+        }
+    }
+}

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;

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,
+    /// The target relative angular velocity the motor will attempt to reach.
-    // pub max_motor_impulse: Real,
+    pub motor_target_vel: Real,
-    // pub motor_impulse: 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,
+            motor_target_vel: 0.0,
-            // target_motor_vel: 0.0,
+            motor_target_pos: 0.0,
-            // max_motor_impulse: Real::MAX,
+            motor_stiffness: 0.0,
-            // motor_impulse: 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,
+            motor_target_vel: 0.0,
-            // target_motor_vel: 0.0,
+            motor_target_pos: 0.0,
-            // max_motor_impulse: Real::MAX,
+            motor_stiffness: 0.0,
-            // motor_impulse: 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;
+    }
 }

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
+    }
 }

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),
+        }
+    }
+}

src/dynamics/mod.rs

@@ -5,7 +5,14 @@ pub(crate) use self::joint::JointIndex;
 #[cfg(feature = "dim3")]
 pub use self::joint::RevoluteJoint;
 pub use self::joint::{
-    BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint,
+    BallJoint,
+    FixedJoint,
+    Joint,
+    JointHandle,
+    JointParams,
+    JointSet,
+    PrismaticJoint,
+    SpringModel, // GenericJoint
 };
 pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
 pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};

src/dynamics/solver/interaction_groups.rs

@@ -214,6 +214,12 @@ impl InteractionGroups {
                 continue;
             }
 
+            if !interaction.supports_simd_constraints() {
+                // This joint does not support simd constraints yet.
+                self.nongrouped_interactions.push(*interaction_i);
+                continue;
+            }
+
             let ijoint = interaction.params.type_id();
             let i1 = body1.active_set_offset;
             let i2 = body2.active_set_offset;

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{AngularInertia, Real, SdpMatrix, Vector};
+use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -13,13 +13,18 @@ pub(crate) struct BallVelocityConstraint {
     joint_id: JointIndex,
 
     rhs: Vector<Real>,
-    pub(crate) impulse: Vector<Real>,
+    impulse: Vector<Real>,
 
     r1: Vector<Real>,
     r2: Vector<Real>,
 
     inv_lhs: SdpMatrix<Real>,
 
+    motor_rhs: AngVector<Real>,
+    motor_impulse: AngVector<Real>,
+    motor_inv_lhs: Option<AngularInertia<Real>>,
+    motor_max_impulse: Real,
+
     im1: Real,
     im2: Real,
 
@@ -33,10 +38,10 @@ impl BallVelocityConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &BallJoint,
+        joint: &BallJoint,
     ) -> Self {
-        let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com;
+        let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com;
-        let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com;
+        let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com;
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
@@ -77,17 +82,85 @@ impl BallVelocityConstraint {
 
         let inv_lhs = lhs.inverse_unchecked();
 
+        /*
+         * Motor part.
+         */
+        let mut motor_rhs = na::zero();
+        let mut motor_inv_lhs = None;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        if motor_max_impulse > 0.0 {
+            let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+                params.dt,
+                joint.motor_stiffness,
+                joint.motor_damping,
+            );
+
+            if stiffness != 0.0 {
+                let dpos = rb2.position.rotation
+                    * (rb1.position.rotation * joint.motor_target_pos).inverse();
+                #[cfg(feature = "dim2")]
+                {
+                    motor_rhs += dpos.angle() * stiffness;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    motor_rhs += dpos.scaled_axis() * stiffness;
+                }
+            }
+
+            if damping != 0.0 {
+                let curr_vel = rb2.angvel - rb1.angvel;
+                motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+            }
+
+            #[cfg(feature = "dim2")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some(gamma / (ii1 + ii2))
+                } else {
+                    Some(gamma)
+                };
+                motor_rhs /= gamma;
+            }
+
+            #[cfg(feature = "dim3")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some((ii1 + ii2).inverse_unchecked() * gamma)
+                } else {
+                    Some(SdpMatrix::diagonal(gamma))
+                };
+                motor_rhs /= gamma;
+            }
+        }
+
+        #[cfg(feature = "dim2")]
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+        #[cfg(feature = "dim3")]
+        let motor_impulse =
+            joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
+
         BallVelocityConstraint {
             joint_id,
             mj_lambda1: rb1.active_set_offset,
             mj_lambda2: rb2.active_set_offset,
             im1,
             im2,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             r1: anchor1,
             r2: anchor2,
             rhs,
             inv_lhs,
+            motor_rhs,
+            motor_impulse,
+            motor_inv_lhs,
+            motor_max_impulse: joint.motor_max_impulse,
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
@@ -98,18 +171,19 @@ impl BallVelocityConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         mj_lambda1.linear += self.im1 * self.impulse;
-        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse);
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -124,6 +198,37 @@ impl BallVelocityConstraint {
 
         mj_lambda2.linear -= self.im2 * impulse;
         mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
+    }
+
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs;
+
+            let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
+
+            #[cfg(feature = "dim2")]
+            let clamped_impulse =
+                na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
+            #[cfg(feature = "dim3")]
+            let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
+
+            let effective_impulse = clamped_impulse - self.motor_impulse;
+            self.motor_impulse = clamped_impulse;
+
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse);
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -132,7 +237,8 @@ impl BallVelocityConstraint {
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse
+            ball.impulse = self.impulse;
+            ball.motor_impulse = self.motor_impulse;
         }
     }
 }
@@ -141,10 +247,17 @@ impl BallVelocityConstraint {
 pub(crate) struct BallVelocityGroundConstraint {
     mj_lambda2: usize,
     joint_id: JointIndex,
+    r2: Vector<Real>,
+
     rhs: Vector<Real>,
     impulse: Vector<Real>,
-    r2: Vector<Real>,
     inv_lhs: SdpMatrix<Real>,
+
+    motor_rhs: AngVector<Real>,
+    motor_impulse: AngVector<Real>,
+    motor_inv_lhs: Option<AngularInertia<Real>>,
+    motor_max_impulse: Real,
+
     im2: Real,
     ii2_sqrt: AngularInertia<Real>,
 }
@@ -155,18 +268,18 @@ impl BallVelocityGroundConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &BallJoint,
+        joint: &BallJoint,
         flipped: bool,
     ) -> Self {
         let (anchor1, anchor2) = if flipped {
             (
-                rb1.position * cparams.local_anchor2 - rb1.world_com,
+                rb1.position * joint.local_anchor2 - rb1.world_com,
-                rb2.position * cparams.local_anchor1 - rb2.world_com,
+                rb2.position * joint.local_anchor1 - rb2.world_com,
             )
         } else {
             (
-                rb1.position * cparams.local_anchor1 - rb1.world_com,
+                rb1.position * joint.local_anchor1 - rb1.world_com,
-                rb2.position * cparams.local_anchor2 - rb2.world_com,
+                rb2.position * joint.local_anchor2 - rb2.world_com,
             )
         };
 
@@ -199,14 +312,80 @@ impl BallVelocityGroundConstraint {
 
         let inv_lhs = lhs.inverse_unchecked();
 
+        /*
+         * Motor part.
+         */
+        let mut motor_rhs = na::zero();
+        let mut motor_inv_lhs = None;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        if motor_max_impulse > 0.0 {
+            let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+                params.dt,
+                joint.motor_stiffness,
+                joint.motor_damping,
+            );
+
+            if stiffness != 0.0 {
+                let dpos = rb2.position.rotation
+                    * (rb1.position.rotation * joint.motor_target_pos).inverse();
+                #[cfg(feature = "dim2")]
+                {
+                    motor_rhs += dpos.angle() * stiffness;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    motor_rhs += dpos.scaled_axis() * stiffness;
+                }
+            }
+
+            if damping != 0.0 {
+                let curr_vel = rb2.angvel - rb1.angvel;
+                motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+            }
+
+            #[cfg(feature = "dim2")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some(gamma / ii2)
+                } else {
+                    Some(gamma)
+                };
+                motor_rhs /= gamma;
+            }
+
+            #[cfg(feature = "dim3")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some(ii2.inverse_unchecked() * gamma)
+                } else {
+                    Some(SdpMatrix::diagonal(gamma))
+                };
+                motor_rhs /= gamma;
+            }
+        }
+
+        #[cfg(feature = "dim2")]
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+        #[cfg(feature = "dim3")]
+        let motor_impulse =
+            joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
+
         BallVelocityGroundConstraint {
             joint_id,
             mj_lambda2: rb2.active_set_offset,
             im2,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             r2: anchor2,
             rhs,
             inv_lhs,
+            motor_rhs,
+            motor_impulse,
+            motor_inv_lhs,
+            motor_max_impulse: joint.motor_max_impulse,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
@@ -214,13 +393,13 @@ impl BallVelocityGroundConstraint {
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(self.r2.gcross(self.impulse) + self.motor_impulse);
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
         let dvel = vel2 + self.rhs;
@@ -230,6 +409,33 @@ impl BallVelocityGroundConstraint {
 
         mj_lambda2.linear -= self.im2 * impulse;
         mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
+    }
+
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
+        if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dangvel = ang_vel2 + self.motor_rhs;
+            let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
+
+            #[cfg(feature = "dim2")]
+            let clamped_impulse =
+                na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
+            #[cfg(feature = "dim3")]
+            let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
+
+            let effective_impulse = clamped_impulse - self.motor_impulse;
+            self.motor_impulse = clamped_impulse;
+
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
@@ -238,7 +444,8 @@ impl BallVelocityGroundConstraint {
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse
+            ball.impulse = self.impulse;
+            ball.motor_impulse = self.motor_impulse;
         }
     }
 }

src/dynamics/solver/joint_constraint/generic_position_constraint.rs

@@ -0,0 +1,346 @@
+use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::math::{
+    AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
+    DIM,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use na::{Vector3, Vector6};
+
+// FIXME: review this code for the case where the center of masses are not the origin.
+#[derive(Debug)]
+pub(crate) struct GenericPositionConstraint {
+    position1: usize,
+    position2: usize,
+    local_anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
+    im1: Real,
+    im2: Real,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
+
+    joint: GenericJoint,
+}
+
+impl GenericPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, joint: &GenericJoint) -> Self {
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
+
+        Self {
+            local_anchor1: joint.local_anchor1,
+            local_anchor2: joint.local_anchor2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
+            joint: *joint,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        let mut position1 = positions[self.position1];
+        let mut position2 = positions[self.position2];
+        let mut params = *params;
+        params.joint_erp = 0.8;
+
+        /*
+         *
+         * Translation part.
+         *
+         */
+        {
+            let anchor1 = position1 * self.joint.local_anchor1;
+            let anchor2 = position2 * self.joint.local_anchor2;
+            let basis = anchor1.rotation;
+            let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
+            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
+            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
+
+            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.xyz(),
+                &self.joint.max_position.xyz(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let rmat1 = r1.gcross_matrix() * rotmat;
+            let rmat2 = r2.gcross_matrix() * rotmat;
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1)
+                + self.ii2.quadform(&rmat2).add_diagonal(self.im2))
+            .into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot1 = self.ii1.transform_vector(r1.gcross(impulse));
+            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
+
+            position1.translation.vector += self.im1 * impulse;
+            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
+            position2.translation.vector -= self.im2 * impulse;
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        /*
+         *
+         * Rotation part
+         *
+         */
+        {
+            let anchor1 = position1 * self.joint.local_anchor1;
+            let anchor2 = position2 * self.joint.local_anchor2;
+            let basis = anchor1.rotation;
+            let mut min_pos_impulse = self
+                .joint
+                .min_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+            let mut max_pos_impulse = self
+                .joint
+                .max_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+
+            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
+                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            // TODO: we should keep the SdpMatrix3 type.
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot1 = self.ii1.transform_vector(impulse);
+            let rot2 = self.ii2.transform_vector(impulse);
+
+            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        positions[self.position1] = position1;
+        positions[self.position2] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct GenericPositionGroundConstraint {
+    position2: usize,
+    anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com2: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    joint: GenericJoint,
+}
+
+impl GenericPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        joint: &GenericJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor1;
+        let local_anchor2;
+
+        if flipped {
+            anchor1 = rb1.predicted_position * joint.local_anchor2;
+            local_anchor2 = joint.local_anchor1;
+        } else {
+            anchor1 = rb1.predicted_position * joint.local_anchor1;
+            local_anchor2 = joint.local_anchor2;
+        };
+
+        Self {
+            anchor1,
+            local_anchor2,
+            position2: rb2.active_set_offset,
+            im2: rb2.effective_inv_mass,
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+            local_com2: rb2.mass_properties.local_com,
+            joint: *joint,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        let mut position2 = positions[self.position2];
+        let mut params = *params;
+        params.joint_erp = 0.8;
+
+        /*
+         *
+         * Translation part.
+         *
+         */
+        {
+            let anchor1 = self.anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+            let basis = anchor1.rotation;
+            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
+            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
+
+            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.xyz(),
+                &self.joint.max_position.xyz(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let rmat2 = r2.gcross_matrix() * rotmat;
+
+            // TODO: we should keep the SdpMatrix3 type.
+            let delassus = self
+                .ii2
+                .quadform(&rmat2)
+                .add_diagonal(self.im2)
+                .into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
+
+            position2.translation.vector -= self.im2 * impulse;
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        /*
+         *
+         * Rotation part
+         *
+         */
+        {
+            let anchor1 = self.anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+            let basis = anchor1.rotation;
+            let mut min_pos_impulse = self
+                .joint
+                .min_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+            let mut max_pos_impulse = self
+                .joint
+                .max_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+
+            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
+                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let delassus = self.ii2.quadform(&rotmat).into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+            let rot2 = self.ii2.transform_vector(impulse);
+
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        positions[self.position2] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs

@@ -0,0 +1,51 @@
+use super::{GenericPositionConstraint, GenericPositionGroundConstraint};
+use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WGenericPositionConstraint {
+    constraints: [GenericPositionConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WGenericPositionGroundConstraint {
+    constraints: [GenericPositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs

@@ -0,0 +1,706 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM};
+use crate::na::UnitQuaternion;
+use crate::parry::math::{AngDim, SpatialVector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Matrix3, Matrix6, Vector3, Vector6, U3};
+#[cfg(feature = "dim2")]
+use na::{Matrix3, Vector3};
+
+#[derive(Debug)]
+pub(crate) struct GenericVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    inv_lhs_lin: Matrix3<Real>,
+    inv_lhs_ang: Matrix3<Real>,
+
+    im1: Real,
+    im2: Real,
+
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
+
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+
+    r1: Vector<Real>,
+    r2: Vector<Real>,
+    basis1: Rotation<Real>,
+    basis2: Rotation<Real>,
+    dependant_set_mask: SpacialVector<Real>,
+
+    vel: GenericConstraintPart,
+}
+
+impl GenericVelocityConstraint {
+    pub fn compute_velocity_error(
+        min_velocity: &SpatialVector<Real>,
+        max_velocity: &SpatialVector<Real>,
+        r1: &Vector<Real>,
+        r2: &Vector<Real>,
+        basis1: &Rotation<Real>,
+        basis2: &Rotation<Real>,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+    ) -> SpatialVector<Real> {
+        let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+            + basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
+        let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+            + basis2.inverse_transform_vector(&rb2.angvel);
+
+        let min_linvel = min_velocity.xyz();
+        let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned();
+        let max_linvel = max_velocity.xyz();
+        let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned();
+        let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel);
+        let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel);
+
+        #[cfg(feature = "dim2")]
+        return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
+
+        #[cfg(feature = "dim3")]
+        return Vector6::new(
+            lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z,
+        );
+    }
+
+    pub fn compute_lin_position_error(
+        anchor1: &Isometry<Real>,
+        anchor2: &Isometry<Real>,
+        basis: &Rotation<Real>,
+        min_position: &Vector<Real>,
+        max_position: &Vector<Real>,
+    ) -> Vector<Real> {
+        let dpos = anchor2.translation.vector - anchor1.translation.vector;
+        let local_dpos = basis.inverse_transform_vector(&dpos);
+        local_dpos - local_dpos.sup(min_position).inf(max_position)
+    }
+
+    pub fn compute_ang_position_error(
+        anchor1: &Isometry<Real>,
+        anchor2: &Isometry<Real>,
+        basis: &Rotation<Real>,
+        min_position: &Vector<Real>,
+        max_position: &Vector<Real>,
+    ) -> Vector<Real> {
+        let drot = anchor2.rotation * anchor1.rotation.inverse();
+        let local_drot_diff = basis.inverse_transform_vector(&drot.scaled_axis());
+
+        let error = local_drot_diff - local_drot_diff.sup(min_position).inf(max_position);
+        let error_code =
+            (error[0] == 0.0) as usize + (error[1] == 0.0) as usize + (error[2] == 0.0) as usize;
+
+        if error_code == 1 {
+            // Align two axes.
+            let constrained_axis = error.iamin();
+            let axis1 = anchor1
+                .rotation
+                .to_rotation_matrix()
+                .into_inner()
+                .column(constrained_axis)
+                .into_owned();
+            let axis2 = anchor2
+                .rotation
+                .to_rotation_matrix()
+                .into_inner()
+                .column(constrained_axis)
+                .into_owned();
+            let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
+                .unwrap_or(UnitQuaternion::identity());
+            let local_drot_diff = basis.inverse_transform_vector(&rot_cross.scaled_axis());
+            local_drot_diff - local_drot_diff.sup(min_position).inf(max_position)
+        } else {
+            error
+        }
+    }
+
+    pub fn invert_partial_delassus_matrix(
+        min_impulse: &Vector<Real>,
+        max_impulse: &Vector<Real>,
+        dependant_set_mask: &mut Vector<Real>,
+        mut delassus: na::Matrix3<Real>,
+    ) -> na::Matrix3<Real> {
+        // Adjust the Delassus matrix to take force limits into account.
+        // If a DoF has a force limit, then we need to make its
+        // constraint independent from the others because otherwise
+        // the force clamping will cause errors to propagate in the
+        // other constraints.
+        for i in 0..3 {
+            if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX {
+                let diag = delassus[(i, i)];
+                delassus.column_mut(i).fill(0.0);
+                delassus.row_mut(i).fill(0.0);
+                delassus[(i, i)] = diag;
+                dependant_set_mask[i] = 0.0;
+            } else {
+                dependant_set_mask[i] = 1.0;
+            }
+        }
+
+        delassus.try_inverse().unwrap()
+    }
+
+    pub fn compute_position_error(
+        joint: &GenericJoint,
+        anchor1: &Isometry<Real>,
+        anchor2: &Isometry<Real>,
+        basis: &Rotation<Real>,
+    ) -> SpatialVector<Real> {
+        let delta_pos = Isometry::from_parts(
+            anchor2.translation * anchor1.translation.inverse(),
+            anchor2.rotation * anchor1.rotation.inverse(),
+        );
+        let lin_dpos = basis.inverse_transform_vector(&delta_pos.translation.vector);
+        let ang_dpos = basis.inverse_transform_vector(&delta_pos.rotation.scaled_axis());
+
+        let dpos = Vector6::new(
+            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
+        );
+
+        let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
+        let error_code =
+            (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize;
+
+        match error_code {
+            1 => {
+                let constrained_axis = error.rows(3, 3).iamin();
+                let axis1 = anchor1
+                    .rotation
+                    .to_rotation_matrix()
+                    .into_inner()
+                    .column(constrained_axis)
+                    .into_owned();
+                let axis2 = anchor2
+                    .rotation
+                    .to_rotation_matrix()
+                    .into_inner()
+                    .column(constrained_axis)
+                    .into_owned();
+                let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
+                    .unwrap_or(UnitQuaternion::identity());
+                let ang_dpos = basis.inverse_transform_vector(&rot_cross.scaled_axis());
+                let dpos = Vector6::new(
+                    lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
+                );
+
+                dpos - dpos.sup(&joint.min_position).inf(&joint.max_position)
+            }
+            _ => error,
+        }
+    }
+
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        joint: &GenericJoint,
+    ) -> Self {
+        let anchor1 = rb1.position * joint.local_anchor1;
+        let anchor2 = rb2.position * joint.local_anchor2;
+        let basis1 = anchor1.rotation;
+        let basis2 = anchor2.rotation;
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+        let mut min_impulse = joint.min_impulse;
+        let mut max_impulse = joint.max_impulse;
+        let mut min_pos_impulse = joint.min_pos_impulse;
+        let mut max_pos_impulse = joint.max_pos_impulse;
+        let mut min_velocity = joint.min_velocity;
+        let mut max_velocity = joint.max_velocity;
+        let mut dependant_set_mask = SpacialVector::repeat(1.0);
+
+        let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1)
+            * params.inv_dt()
+            * params.joint_erp;
+
+        for i in 0..6 {
+            if pos_rhs[i] < 0.0 {
+                min_impulse[i] = -Real::MAX;
+                min_pos_impulse[i] = -Real::MAX;
+                min_velocity[i] = 0.0;
+            }
+            if pos_rhs[i] > 0.0 {
+                max_impulse[i] = Real::MAX;
+                max_pos_impulse[i] = Real::MAX;
+                max_velocity[i] = 0.0;
+            }
+        }
+
+        let rhs = Self::compute_velocity_error(
+            &min_velocity,
+            &max_velocity,
+            &r1,
+            &r2,
+            &basis1,
+            &basis2,
+            rb1,
+            rb2,
+        );
+        let rhs_lin = rhs.xyz();
+        let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into();
+
+        // TODO: we should keep the SdpMatrix3 type.
+        let rotmat1 = basis1.to_rotation_matrix().into_inner();
+        let rotmat2 = basis2.to_rotation_matrix().into_inner();
+        let rmat1 = r1.gcross_matrix() * rotmat1;
+        let rmat2 = r2.gcross_matrix() * rotmat2;
+        let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1)
+            + ii2.quadform(&rmat2).add_diagonal(im2))
+        .into_matrix();
+        let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix();
+
+        let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.xyz(),
+            &max_pos_impulse.xyz(),
+            &mut Vector3::zeros(),
+            delassus00,
+        );
+        let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &mut Vector3::zeros(),
+            delassus11,
+        );
+
+        let impulse = (joint.impulse * params.warmstart_coeff)
+            .inf(&max_impulse)
+            .sup(&min_impulse);
+
+        let lin_impulse = impulse.xyz();
+        let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let min_lin_impulse = min_impulse.xyz();
+        let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let max_lin_impulse = max_impulse.xyz();
+        let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
+
+        GenericVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            inv_lhs_lin,
+            inv_lhs_ang,
+            r1,
+            r2,
+            basis1,
+            basis2,
+            dependant_set_mask,
+            vel: GenericConstraintPart {
+                lin_impulse,
+                ang_impulse,
+                min_lin_impulse,
+                min_ang_impulse,
+                max_lin_impulse,
+                max_ang_impulse,
+                rhs_lin,
+                rhs_ang,
+            },
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse1 = self.basis1 * self.vel.lin_impulse;
+        let ang_impulse1 = self.basis1 * self.vel.ang_impulse;
+        let lin_impulse2 = self.basis2 * self.vel.lin_impulse;
+        let ang_impulse2 = self.basis2 * self.vel.ang_impulse;
+
+        mj_lambda1.linear += self.im1 * lin_impulse1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2);
+        self.vel.lin_impulse = lin_imp;
+        self.vel.ang_impulse = ang_imp;
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        match &mut joint.params {
+            JointParams::GenericJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.impulse[3] = self.vel.ang_impulse.x;
+                out.impulse[4] = self.vel.ang_impulse.y;
+                out.impulse[5] = self.vel.ang_impulse.z;
+            }
+            JointParams::RevoluteJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.motor_impulse = self.vel.ang_impulse.x;
+                out.impulse[3] = self.vel.ang_impulse.y;
+                out.impulse[4] = self.vel.ang_impulse.z;
+            }
+            _ => unimplemented!(),
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct GenericVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    inv_lhs_lin: Matrix3<Real>,
+    inv_lhs_ang: Matrix3<Real>,
+
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+    r2: Vector<Real>,
+    basis: Rotation<Real>,
+
+    dependant_set_mask: SpacialVector<Real>,
+
+    vel: GenericConstraintPart,
+}
+
+impl GenericVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        joint: &GenericJoint,
+        flipped: bool,
+    ) -> Self {
+        let (anchor1, anchor2) = if flipped {
+            (
+                rb1.position * joint.local_anchor2,
+                rb2.position * joint.local_anchor1,
+            )
+        } else {
+            (
+                rb1.position * joint.local_anchor1,
+                rb2.position * joint.local_anchor2,
+            )
+        };
+
+        let basis = anchor2.rotation;
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+        let mut min_impulse = joint.min_impulse;
+        let mut max_impulse = joint.max_impulse;
+        let mut min_pos_impulse = joint.min_pos_impulse;
+        let mut max_pos_impulse = joint.max_pos_impulse;
+        let mut min_velocity = joint.min_velocity;
+        let mut max_velocity = joint.max_velocity;
+        let mut dependant_set_mask = SpacialVector::repeat(1.0);
+
+        let pos_rhs =
+            GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2, &basis)
+                * params.inv_dt()
+                * params.joint_erp;
+
+        for i in 0..6 {
+            if pos_rhs[i] < 0.0 {
+                min_impulse[i] = -Real::MAX;
+                min_pos_impulse[i] = -Real::MAX;
+                min_velocity[i] = 0.0;
+            }
+            if pos_rhs[i] > 0.0 {
+                max_impulse[i] = Real::MAX;
+                max_pos_impulse[i] = Real::MAX;
+                max_velocity[i] = 0.0;
+            }
+        }
+
+        let rhs = GenericVelocityConstraint::compute_velocity_error(
+            &min_velocity,
+            &max_velocity,
+            &r1,
+            &r2,
+            &basis,
+            &basis,
+            rb1,
+            rb2,
+        );
+        let rhs_lin = rhs.xyz();
+        let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into_owned();
+
+        // TODO: we should keep the SdpMatrix3 type.
+        let rotmat = basis.to_rotation_matrix().into_inner();
+        let rmat2 = r2.gcross_matrix() * rotmat;
+        let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix();
+        let delassus11 = ii2.quadform(&rotmat).into_matrix();
+
+        let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.xyz(),
+            &max_pos_impulse.xyz(),
+            &mut Vector3::zeros(),
+            delassus00,
+        );
+        let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &mut Vector3::zeros(),
+            delassus11,
+        );
+
+        let impulse = (joint.impulse * params.warmstart_coeff)
+            .inf(&max_impulse)
+            .sup(&min_impulse);
+
+        let lin_impulse = impulse.xyz();
+        let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let min_lin_impulse = min_impulse.xyz();
+        let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let max_lin_impulse = max_impulse.xyz();
+        let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
+
+        GenericVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            inv_lhs_lin,
+            inv_lhs_ang,
+            r2,
+            basis,
+            vel: GenericConstraintPart {
+                lin_impulse,
+                ang_impulse,
+                min_lin_impulse,
+                min_ang_impulse,
+                max_lin_impulse,
+                max_ang_impulse,
+                rhs_lin,
+                rhs_ang,
+            },
+            dependant_set_mask,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.basis * self.vel.lin_impulse;
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.basis * self.vel.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.basis * self.vel.ang_impulse;
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2);
+        self.vel.lin_impulse = lin_imp;
+        self.vel.ang_impulse = ang_imp;
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // TODO: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        match &mut joint.params {
+            JointParams::GenericJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.impulse[3] = self.vel.ang_impulse.x;
+                out.impulse[4] = self.vel.ang_impulse.y;
+                out.impulse[5] = self.vel.ang_impulse.z;
+            }
+            JointParams::RevoluteJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.motor_impulse = self.vel.ang_impulse.x;
+                out.impulse[3] = self.vel.ang_impulse.y;
+                out.impulse[4] = self.vel.ang_impulse.z;
+            }
+            _ => unimplemented!(),
+        }
+    }
+}
+
+#[derive(Debug)]
+struct GenericConstraintPart {
+    lin_impulse: Vector3<Real>,
+    max_lin_impulse: Vector3<Real>,
+    min_lin_impulse: Vector3<Real>,
+    rhs_lin: Vector3<Real>,
+
+    ang_impulse: Vector3<Real>,
+    max_ang_impulse: Vector3<Real>,
+    min_ang_impulse: Vector3<Real>,
+    rhs_ang: Vector3<Real>,
+}
+
+impl GenericConstraintPart {
+    fn solve(
+        &self,
+        parent: &GenericVelocityConstraint,
+        mj_lambda1: &mut DeltaVel<Real>,
+        mj_lambda2: &mut DeltaVel<Real>,
+    ) -> (Vector3<Real>, Vector3<Real>) {
+        let new_lin_impulse;
+        let new_ang_impulse;
+
+        /*
+         *
+         * Solve translations.
+         *
+         */
+        {
+            let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent
+                .basis1
+                .inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)))
+                + parent
+                    .basis2
+                    .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
+
+            let err = dvel + self.rhs_lin;
+
+            new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
+                .sup(&self.min_lin_impulse)
+                .inf(&self.max_lin_impulse);
+            let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse);
+            let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse);
+
+            mj_lambda1.linear += parent.im1 * effective_impulse1;
+            mj_lambda1.angular += parent
+                .ii1_sqrt
+                .transform_vector(parent.r1.gcross(effective_impulse1));
+
+            mj_lambda2.linear -= parent.im2 * effective_impulse2;
+            mj_lambda2.angular -= parent
+                .ii2_sqrt
+                .transform_vector(parent.r2.gcross(effective_impulse2));
+        }
+
+        /*
+         *
+         * Solve rotations.
+         *
+         */
+        {
+            let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent.basis2.inverse_transform_vector(&ang_vel2)
+                - parent.basis1.inverse_transform_vector(&ang_vel1);
+            let err = dvel + self.rhs_ang;
+
+            new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
+                .sup(&self.min_ang_impulse)
+                .inf(&self.max_ang_impulse);
+            let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse);
+            let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse);
+
+            mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1);
+            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2);
+        }
+
+        (new_lin_impulse, new_ang_impulse)
+    }
+
+    fn solve_ground(
+        &self,
+        parent: &GenericVelocityGroundConstraint,
+        mj_lambda2: &mut DeltaVel<Real>,
+    ) -> (Vector3<Real>, Vector3<Real>) {
+        let new_lin_impulse;
+        let new_ang_impulse;
+
+        /*
+         *
+         * Solve translations.
+         *
+         */
+        {
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent
+                .basis
+                .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
+
+            let err = dvel + self.rhs_lin;
+
+            new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
+                .sup(&self.min_lin_impulse)
+                .inf(&self.max_lin_impulse);
+            let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse);
+
+            mj_lambda2.linear -= parent.im2 * effective_impulse;
+            mj_lambda2.angular -= parent
+                .ii2_sqrt
+                .transform_vector(parent.r2.gcross(effective_impulse));
+        }
+
+        /*
+         *
+         * Solve rotations.
+         *
+         */
+        {
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent.basis.inverse_transform_vector(&ang_vel2);
+            let err = dvel + self.rhs_ang;
+
+            new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
+                .sup(&self.min_ang_impulse)
+                .inf(&self.max_ang_impulse);
+            let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse);
+
+            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse);
+        }
+
+        (new_lin_impulse, new_ang_impulse)
+    }
+}

src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs

@@ -0,0 +1,472 @@
+use simba::simd::SimdValue;
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
+    Vector, SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix6, Vector6, U3};
+#[cfg(feature = "dim2")]
+use {
+    na::{Matrix3, Vector3},
+    parry::utils::SdpMatrix3,
+};
+
+#[derive(Debug)]
+pub(crate) struct WGenericVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdReal>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdReal>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdReal>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdReal>,
+
+    im1: SimdReal,
+    im2: SimdReal,
+
+    ii1: AngularInertia<SimdReal>,
+    ii2: AngularInertia<SimdReal>,
+
+    ii1_sqrt: AngularInertia<SimdReal>,
+    ii2_sqrt: AngularInertia<SimdReal>,
+
+    r1: Vector<SimdReal>,
+    r2: Vector<SimdReal>,
+}
+
+impl WGenericVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let ii1 = ii1_sqrt.squared();
+        let ii2 = ii2_sqrt.squared();
+        let r1 = anchor1.translation.vector - world_com1.coords;
+        let r2 = anchor2.translation.vector - world_com2.coords;
+        let rmat1: CrossMatrix<_> = r1.gcross_matrix();
+        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let lhs00 =
+                ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
+            let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
+            let lhs11 = (ii1 + ii2).into_matrix();
+
+            // Note that Cholesky only reads the lower-triangular part of the matrix
+            // so we don't need to fill lhs01.
+            lhs = Matrix6::zeros();
+            lhs.fixed_slice_mut::<U3, U3>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
+            let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
+            let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
+            let m13 = rmat1.x * ii1 + rmat2.x * ii2;
+            let m23 = rmat1.y * ii1 + rmat2.y * ii2;
+            let m33 = ii1 + ii2;
+            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
+        }
+
+        // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix?
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
+        let ang_dvel = -angvel1 + angvel2;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
+        );
+
+        WGenericVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt,
+            ii2_sqrt,
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
+            + mj_lambda2.linear
+            + ang_vel2.gcross(self.r2);
+        let dangvel = -ang_vel1 + ang_vel2;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::GenericJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WGenericVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdReal>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdReal>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdReal>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdReal>,
+
+    im2: SimdReal,
+    ii2: AngularInertia<SimdReal>,
+    ii2_sqrt: AngularInertia<SimdReal>,
+    r2: Vector<SimdReal>,
+}
+
+impl WGenericVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Isometry::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Isometry::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let ii2 = ii2_sqrt.squared();
+        let r1 = anchor1.translation.vector - world_com1.coords;
+        let r2 = anchor2.translation.vector - world_com2.coords;
+        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
+            let lhs10 = ii2.transform_matrix(&rmat2);
+            let lhs11 = ii2.into_matrix();
+
+            lhs = Matrix6::zeros();
+            lhs.fixed_slice_mut::<U3, U3>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let m11 = im2 + rmat2.x * rmat2.x * ii2;
+            let m12 = rmat2.x * rmat2.y * ii2;
+            let m22 = im2 + rmat2.y * rmat2.y * ii2;
+            let m13 = rmat2.x * ii2;
+            let m23 = rmat2.y * ii2;
+            let m33 = ii2;
+            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
+        }
+
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let ang_dvel = angvel2 - angvel1;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
+        );
+
+        WGenericVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2,
+            ii2_sqrt,
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let dangvel = ang_vel2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let impulse = self.inv_lhs * rhs;
+
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::GenericJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -13,6 +13,9 @@ use super::{
 #[cfg(feature = "dim3")]
 #[cfg(feature = "simd-is-enabled")]
 use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
+// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
+//     GenericVelocityConstraint, GenericVelocityGroundConstraint,
+// };
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
@@ -34,6 +37,12 @@ pub(crate) enum AnyJointVelocityConstraint {
     WFixedConstraint(WFixedVelocityConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedVelocityGroundConstraint),
+    // GenericConstraint(GenericVelocityConstraint),
+    // GenericGroundConstraint(GenericVelocityGroundConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericConstraint(WGenericVelocityConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericGroundConstraint(WGenericVelocityGroundConstraint),
     PrismaticConstraint(PrismaticVelocityConstraint),
     PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -79,6 +88,9 @@ impl AnyJointVelocityConstraint {
             JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
                 PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
             ),
+            // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
+            //     GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            // ),
             #[cfg(feature = "dim3")]
             JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
                 RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
@@ -109,6 +121,12 @@ impl AnyJointVelocityConstraint {
                     params, joint_id, rbs1, rbs2, joints,
                 ))
             }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointVelocityConstraint::WGenericConstraint(
+            //         WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+            //     )
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -148,6 +166,11 @@ impl AnyJointVelocityConstraint {
             JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
                 FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
             ),
+            // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
+            //     GenericVelocityGroundConstraint::from_params(
+            //         params, joint_id, rb1, rb2, p, flipped,
+            //     ),
+            // ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointVelocityConstraint::PrismaticGroundConstraint(
                     PrismaticVelocityGroundConstraint::from_params(
@@ -156,11 +179,9 @@ impl AnyJointVelocityConstraint {
                 )
             }
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
+            JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
-                RevoluteVelocityGroundConstraint::from_params(
                 params, joint_id, rb1, rb2, p, flipped,
             ),
-            ),
         }
     }
 
@@ -199,6 +220,14 @@ impl AnyJointVelocityConstraint {
                     ),
                 )
             }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointVelocityConstraint::WGenericGroundConstraint(
+            //         WGenericVelocityGroundConstraint::from_params(
+            //             params, joint_id, rbs1, rbs2, joints, flipped,
+            //         ),
+            //     )
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -235,6 +264,12 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
@@ -269,6 +304,12 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
@@ -311,6 +352,16 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)
             }
+            // AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
+            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
+            //     c.writeback_impulses(joints_all)
+            // }
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
+            //     c.writeback_impulses(joints_all)
+            // }
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -31,6 +31,12 @@ pub(crate) enum AnyJointPositionConstraint {
     WFixedJoint(WFixedPositionConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedPositionGroundConstraint),
+    // GenericJoint(GenericPositionConstraint),
+    // GenericGroundConstraint(GenericPositionGroundConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericJoint(WGenericPositionConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericGroundConstraint(WGenericPositionGroundConstraint),
     PrismaticJoint(PrismaticPositionConstraint),
     PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -61,6 +67,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
                 FixedPositionConstraint::from_params(rb1, rb2, p),
             ),
+            // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
+            //     GenericPositionConstraint::from_params(rb1, rb2, p),
+            // ),
             JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
                 PrismaticPositionConstraint::from_params(rb1, rb2, p),
             ),
@@ -89,6 +98,12 @@ impl AnyJointPositionConstraint {
                     rbs1, rbs2, joints,
                 ))
             }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
+            //         rbs1, rbs2, joints,
+            //     ))
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -123,6 +138,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
                 FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
             ),
+            // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
+            //     GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            // ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointPositionConstraint::PrismaticGroundConstraint(
                     PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -161,6 +179,12 @@ impl AnyJointPositionConstraint {
                     WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
+            // JointParams::GenericJoint(_) => {
+            //     let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+            //     AnyJointPositionConstraint::WGenericGroundConstraint(
+            //         WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+            //     )
+            // }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -193,6 +217,12 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
+            // AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
+            // AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]

src/dynamics/solver/joint_constraint/mod.rs

@@ -18,6 +18,21 @@ pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocity
 pub(self) use fixed_velocity_constraint_wide::{
     WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
 };
+// pub(self) use generic_position_constraint::{
+//     GenericPositionConstraint, GenericPositionGroundConstraint,
+// };
+// #[cfg(feature = "simd-is-enabled")]
+// pub(self) use generic_position_constraint_wide::{
+//     WGenericPositionConstraint, WGenericPositionGroundConstraint,
+// };
+// pub(self) use generic_velocity_constraint::{
+//     GenericVelocityConstraint, GenericVelocityGroundConstraint,
+// };
+// #[cfg(feature = "simd-is-enabled")]
+// pub(self) use generic_velocity_constraint_wide::{
+//     WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
+// };
+
 pub(crate) use joint_constraint::AnyJointVelocityConstraint;
 pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
 pub(self) use prismatic_position_constraint::{
@@ -63,6 +78,12 @@ mod fixed_position_constraint_wide;
 mod fixed_velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod fixed_velocity_constraint_wide;
+// mod generic_position_constraint;
+// #[cfg(feature = "simd-is-enabled")]
+// mod generic_position_constraint_wide;
+// mod generic_velocity_constraint;
+// #[cfg(feature = "simd-is-enabled")]
+// mod generic_velocity_constraint_wide;
 mod joint_constraint;
 mod joint_position_constraint;
 mod prismatic_position_constraint;

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -3,7 +3,7 @@ use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
 };
 use crate::math::{AngularInertia, Real, Vector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
@@ -41,9 +41,17 @@ pub(crate) struct PrismaticVelocityConstraint {
     #[cfg(feature = "dim2")]
     impulse: Vector2<Real>,
 
+    motor_axis1: Vector<Real>,
+    motor_axis2: Vector<Real>,
+    motor_impulse: Real,
+    motor_rhs: Real,
+    motor_inv_lhs: Real,
+    motor_max_impulse: Real,
+
     limits_impulse: Real,
     limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
     limits_rhs: Real,
+    limits_inv_lhs: Real,
 
     #[cfg(feature = "dim2")]
     basis1: Vector2<Real>,
@@ -63,35 +71,21 @@ impl PrismaticVelocityConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &PrismaticJoint,
+        joint: &PrismaticJoint,
     ) -> Self {
         // Linear part.
-        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor1 = rb1.position * joint.local_anchor1;
-        let anchor2 = rb2.position * cparams.local_anchor2;
+        let anchor2 = rb2.position * joint.local_anchor2;
-        let axis1 = rb1.position * cparams.local_axis1;
+        let axis1 = rb1.position * joint.local_axis1;
-        let axis2 = rb2.position * cparams.local_axis2;
+        let axis2 = rb2.position * joint.local_axis2;
         #[cfg(feature = "dim2")]
-        let basis1 = rb1.position * cparams.basis1[0];
+        let basis1 = rb1.position * joint.basis1[0];
         #[cfg(feature = "dim3")]
         let basis1 = Matrix3x2::from_columns(&[
-            rb1.position * cparams.basis1[0],
+            rb1.position * joint.basis1[0],
-            rb1.position * cparams.basis1[1],
+            rb1.position * joint.basis1[1],
         ]);
 
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
-
         let im1 = rb1.effective_inv_mass;
         let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
@@ -149,25 +143,71 @@ impl PrismaticVelocityConstraint {
         #[cfg(feature = "dim3")]
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
 
-        // Setup limit constraint.
+        /*
+         * Setup motor.
+         */
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
+            motor_rhs += (dist - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs { gamma / (im1 + im2) } else { gamma };
+            motor_rhs /= gamma;
+        }
+
+        let motor_impulse = na::clamp(
+            joint.motor_impulse,
+            -joint.motor_max_impulse,
+            joint.motor_max_impulse,
+        );
+
+        /*
+         * Setup limit constraint.
+         */
         let mut limits_forcedirs = None;
         let mut limits_rhs = 0.0;
         let mut limits_impulse = 0.0;
+        let mut limits_inv_lhs = 0.0;
 
-        if cparams.limits_enabled {
+        if joint.limits_enabled {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // FIXME: we should allow both limits to be active at
+            // TODO: we should allow both limits to be active at
             // the same time, and allow predictive constraint activation.
-            if dist < cparams.limits[0] {
+            if dist < joint.limits[0] {
                 limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
                 limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
+                limits_impulse = joint.limits_impulse;
-            } else if dist > cparams.limits[1] {
+            } else if dist > joint.limits[1] {
                 limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
                 limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
+                limits_impulse = joint.limits_impulse;
+            }
+
+            if dist < joint.limits[0] || dist > joint.limits[1] {
+                let gcross1 = r1.gcross(*axis1);
+                let gcross2 = r2.gcross(*axis2);
+                limits_inv_lhs = crate::utils::inv(
+                    im1 + im2
+                        + gcross1.gdot(ii1.transform_vector(gcross1))
+                        + gcross2.gdot(ii2.transform_vector(gcross2)),
+                );
             }
         }
 
@@ -179,10 +219,17 @@ impl PrismaticVelocityConstraint {
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             limits_impulse: limits_impulse * params.warmstart_coeff,
             limits_forcedirs,
             limits_rhs,
+            limits_inv_lhs,
+            motor_rhs,
+            motor_inv_lhs,
+            motor_impulse,
+            motor_axis1: *axis1,
+            motor_axis2: *axis2,
+            motor_max_impulse: joint.motor_max_impulse,
             basis1,
             inv_lhs,
             rhs,
@@ -211,22 +258,30 @@ impl PrismaticVelocityConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        // Warmstart motors.
+        mj_lambda1.linear += self.motor_axis1 * (self.im1 * self.motor_impulse);
+        mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse);
+
+        // Warmstart limits.
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
+            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
+
+            mj_lambda1.linear += self.im1 * limit_impulse1;
+            mj_lambda1.angular += self
+                .ii1_sqrt
+                .transform_vector(self.r1.gcross(limit_impulse1));
+            mj_lambda2.linear += self.im2 * limit_impulse2;
+            mj_lambda2.angular += self
+                .ii2_sqrt
+                .transform_vector(self.r2.gcross(limit_impulse2));
         }
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        /*
-         * Joint consraint.
-         */
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -255,26 +310,55 @@ impl PrismaticVelocityConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
+    fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
-         * Joint limits.
-         */
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
             let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
                 + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0);
+            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs).max(0.0);
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
+            let lin_impulse1 = limits_forcedir1 * dimpulse;
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+            let lin_impulse2 = limits_forcedir2 * dimpulse;
+
+            mj_lambda1.linear += self.im1 * lin_impulse1;
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
+            mj_lambda2.linear += self.im2 * lin_impulse2;
+            mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
         }
+    }
+
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear)
+                - self.motor_axis1.dot(&mj_lambda1.linear)
+                + self.motor_rhs;
+            let new_impulse = na::clamp(
+                self.motor_impulse + lin_dvel * self.motor_inv_lhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let dimpulse = new_impulse - self.motor_impulse;
+            self.motor_impulse = new_impulse;
+
+            mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse);
+            mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -284,6 +368,7 @@ impl PrismaticVelocityConstraint {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            revolute.motor_impulse = self.motor_impulse;
             revolute.limits_impulse = self.limits_impulse;
         }
     }
@@ -315,6 +400,11 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
     limits_rhs: Real,
 
     axis2: Vector<Real>,
+    motor_impulse: Real,
+    motor_rhs: Real,
+    motor_inv_lhs: Real,
+    motor_max_impulse: Real,
+
     #[cfg(feature = "dim2")]
     basis1: Vector2<Real>,
     #[cfg(feature = "dim3")]
@@ -331,7 +421,7 @@ impl PrismaticVelocityGroundConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &PrismaticJoint,
+        joint: &PrismaticJoint,
         flipped: bool,
     ) -> Self {
         let anchor2;
@@ -341,35 +431,35 @@ impl PrismaticVelocityGroundConstraint {
         let basis1;
 
         if flipped {
-            anchor2 = rb2.position * cparams.local_anchor1;
+            anchor2 = rb2.position * joint.local_anchor1;
-            anchor1 = rb1.position * cparams.local_anchor2;
+            anchor1 = rb1.position * joint.local_anchor2;
-            axis2 = rb2.position * cparams.local_axis1;
+            axis2 = rb2.position * joint.local_axis1;
-            axis1 = rb1.position * cparams.local_axis2;
+            axis1 = rb1.position * joint.local_axis2;
             #[cfg(feature = "dim2")]
             {
-                basis1 = rb1.position * cparams.basis2[0];
+                basis1 = rb1.position * joint.basis2[0];
             }
             #[cfg(feature = "dim3")]
             {
                 basis1 = Matrix3x2::from_columns(&[
-                    rb1.position * cparams.basis2[0],
+                    rb1.position * joint.basis2[0],
-                    rb1.position * cparams.basis2[1],
+                    rb1.position * joint.basis2[1],
                 ]);
             }
         } else {
-            anchor2 = rb2.position * cparams.local_anchor2;
+            anchor2 = rb2.position * joint.local_anchor2;
-            anchor1 = rb1.position * cparams.local_anchor1;
+            anchor1 = rb1.position * joint.local_anchor1;
-            axis2 = rb2.position * cparams.local_axis2;
+            axis2 = rb2.position * joint.local_axis2;
-            axis1 = rb1.position * cparams.local_axis1;
+            axis1 = rb1.position * joint.local_axis1;
             #[cfg(feature = "dim2")]
             {
-                basis1 = rb1.position * cparams.basis1[0];
+                basis1 = rb1.position * joint.basis1[0];
             }
             #[cfg(feature = "dim3")]
             {
                 basis1 = Matrix3x2::from_columns(&[
-                    rb1.position * cparams.basis1[0],
+                    rb1.position * joint.basis1[0],
-                    rb1.position * cparams.basis1[1],
+                    rb1.position * joint.basis1[1],
                 ]);
             }
         };
@@ -438,26 +528,61 @@ impl PrismaticVelocityGroundConstraint {
         #[cfg(feature = "dim3")]
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
 
-        // Setup limit constraint.
+        /*
+         * Setup motor.
+         */
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
+            motor_rhs += (dist - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs { gamma / im2 } else { gamma };
+            motor_rhs /= gamma;
+        }
+
+        let motor_impulse = na::clamp(
+            joint.motor_impulse,
+            -joint.motor_max_impulse,
+            joint.motor_max_impulse,
+        );
+
+        /*
+         * Setup limit constraint.
+         */
         let mut limits_forcedir2 = None;
         let mut limits_rhs = 0.0;
         let mut limits_impulse = 0.0;
 
-        if cparams.limits_enabled {
+        if joint.limits_enabled {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // FIXME: we should allow both limits to be active at
+            // TODO: we should allow both limits to be active at
             // the same time.
-            // FIXME: allow predictive constraint activation.
+            // TODO: allow predictive constraint activation.
-            if dist < cparams.limits[0] {
+            if dist < joint.limits[0] {
                 limits_forcedir2 = Some(axis2.into_inner());
                 limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
+                limits_impulse = joint.limits_impulse;
-            } else if dist > cparams.limits[1] {
+            } else if dist > joint.limits[1] {
                 limits_forcedir2 = Some(-axis2.into_inner());
                 limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
+                limits_impulse = joint.limits_impulse;
             }
         }
 
@@ -466,8 +591,12 @@ impl PrismaticVelocityGroundConstraint {
             mj_lambda2: rb2.active_set_offset,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             limits_impulse: limits_impulse * params.warmstart_coeff,
+            motor_rhs,
+            motor_inv_lhs,
+            motor_impulse,
+            motor_max_impulse: joint.motor_max_impulse,
             basis1,
             inv_lhs,
             rhs,
@@ -492,6 +621,10 @@ impl PrismaticVelocityGroundConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        // Warmstart motors.
+        mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse);
+
+        // Warmstart limits.
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
         }
@@ -499,12 +632,7 @@ impl PrismaticVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
-        /*
-         * Joint consraint.
-         */
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let lin_dvel = self.basis1.tr_mul(&lin_vel2);
@@ -526,13 +654,10 @@ impl PrismaticVelocityGroundConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
+    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-         * Joint limits.
-         */
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
             let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
@@ -543,15 +668,39 @@ impl PrismaticVelocityGroundConstraint {
 
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
         }
+    }
+
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs;
+            let new_impulse = na::clamp(
+                self.motor_impulse + lin_dvel * self.motor_inv_lhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let dimpulse = new_impulse - self.motor_impulse;
+            self.motor_impulse = new_impulse;
+
+            mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_limits(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
+        self.solve_dofs(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    // FIXME: duplicated code with the non-ground constraint.
+    // TODO: duplicated code with the non-ground constraint.
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            revolute.motor_impulse = self.motor_impulse;
             revolute.limits_impulse = self.limits_impulse;
         }
     }

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -7,7 +7,7 @@ use crate::dynamics::{
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
 };
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
@@ -48,6 +48,7 @@ pub(crate) struct WPrismaticVelocityConstraint {
     limits_impulse: SimdReal,
     limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
     limits_rhs: SimdReal,
+    limits_inv_lhs: SimdReal,
 
     #[cfg(feature = "dim2")]
     basis1: Vector2<SimdReal>,
@@ -187,10 +188,13 @@ impl WPrismaticVelocityConstraint {
         #[cfg(feature = "dim3")]
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
 
-        // Setup limit constraint.
+        /*
+         * Setup limit constraint.
+         */
         let mut limits_forcedirs = None;
         let mut limits_rhs = na::zero();
         let mut limits_impulse = na::zero();
+        let mut limits_inv_lhs = na::zero();
         let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
 
         if limits_enabled.any() {
@@ -210,9 +214,17 @@ impl WPrismaticVelocityConstraint {
             let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
 
             if sign != _0 {
+                let gcross1 = r1.gcross(axis1);
+                let gcross2 = r2.gcross(axis2);
+
                 limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
                 limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
                 limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
+                limits_inv_lhs = SimdReal::splat(1.0)
+                    / (im1
+                        + im2
+                        + gcross1.gdot(ii1.transform_vector(gcross1))
+                        + gcross2.gdot(ii2.transform_vector(gcross2)));
             }
         }
 
@@ -228,6 +240,7 @@ impl WPrismaticVelocityConstraint {
             limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
             limits_forcedirs,
             limits_rhs,
+            limits_inv_lhs,
             basis1,
             inv_lhs,
             rhs,
@@ -270,9 +283,19 @@ impl WPrismaticVelocityConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        // Warmstart limits.
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
+            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
+
+            mj_lambda1.linear += limit_impulse1 * self.im1;
+            mj_lambda1.angular += self
+                .ii1_sqrt
+                .transform_vector(self.r1.gcross(limit_impulse1));
+            mj_lambda2.linear += limit_impulse2 * self.im2;
+            mj_lambda2.angular += self
+                .ii2_sqrt
+                .transform_vector(self.r2.gcross(limit_impulse2));
         }
 
         for ii in 0..SIMD_WIDTH {
@@ -285,27 +308,11 @@ impl WPrismaticVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(
-        let mut mj_lambda1 = DeltaVel {
+        &mut self,
-            linear: Vector::from(
+        mj_lambda1: &mut DeltaVel<SimdReal>,
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+        mj_lambda2: &mut DeltaVel<SimdReal>,
-            ),
+    ) {
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
-            ),
-        };
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
-            ),
-        };
-
-        /*
-         * Joint consraint.
-         */
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -334,13 +341,14 @@ impl WPrismaticVelocityConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
+    fn solve_limits(
-         * Joint limits.
+        &mut self,
-         */
+        mj_lambda1: &mut DeltaVel<SimdReal>,
+        mj_lambda2: &mut DeltaVel<SimdReal>,
+    ) {
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
@@ -348,13 +356,40 @@ impl WPrismaticVelocityConstraint {
                 + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.limits_rhs;
             let new_impulse =
-                (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero());
+                (self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero());
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
+            let lin_impulse1 = limits_forcedir1 * dimpulse;
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+            let lin_impulse2 = limits_forcedir2 * dimpulse;
+
+            mj_lambda1.linear += lin_impulse1 * self.im1;
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
+            mj_lambda2.linear += lin_impulse2 * self.im2;
+            mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -477,19 +512,6 @@ impl WPrismaticVelocityGroundConstraint {
         let axis1 = position1 * local_axis1;
         let axis2 = position2 * local_axis2;
 
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
         let ii2 = ii2_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r2 = anchor2 - world_com2;
@@ -616,19 +638,7 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-        let mut mj_lambda2 = DeltaVel {
-            linear: Vector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
-            ),
-            angular: AngVector::from(
-                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
-            ),
-        };
-
-        /*
-         * Joint consraint.
-         */
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let lin_dvel = self.basis1.tr_mul(&lin_vel2);
@@ -650,10 +660,9 @@ impl WPrismaticVelocityGroundConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
+    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
-         * Joint limits.
-         */
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
             // FIXME: the transformation by ii2_sqrt could be avoided by
             // reusing some computations above.
@@ -667,6 +676,20 @@ impl WPrismaticVelocityGroundConstraint {
 
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
         }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_limits(&mut mj_lambda2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -674,7 +697,6 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    // FIXME: duplicated code with the non-ground constraint.
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         for ii in 0..SIMD_WIDTH {
             let joint = &mut joints_all[self.joint_id[ii]].weight;

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -1,6 +1,6 @@
 use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
-use crate::utils::WAngularInertia;
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::Unit;
 
 #[derive(Debug)]
@@ -8,13 +8,15 @@ pub(crate) struct RevolutePositionConstraint {
     position1: usize,
     position2: usize,
 
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
+
     im1: Real,
     im2: Real,
 
     ii1: AngularInertia<Real>,
     ii2: AngularInertia<Real>,
 
-    lin_inv_lhs: Real,
     ang_inv_lhs: AngularInertia<Real>,
 
     local_anchor1: Point<Real>,
@@ -22,6 +24,8 @@ pub(crate) struct RevolutePositionConstraint {
 
     local_axis1: Unit<Vector<Real>>,
     local_axis2: Unit<Vector<Real>>,
+    local_basis1: [Vector<Real>; 2],
+    local_basis2: [Vector<Real>; 2],
 }
 
 impl RevolutePositionConstraint {
@@ -30,7 +34,6 @@ impl RevolutePositionConstraint {
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
-        let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
         Self {
@@ -38,14 +41,17 @@ impl RevolutePositionConstraint {
             im2,
             ii1,
             ii2,
-            lin_inv_lhs,
             ang_inv_lhs,
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
             local_axis1: cparams.local_axis1,
             local_axis2: cparams.local_axis2,
             position1: rb1.active_set_offset,
             position2: rb2.active_set_offset,
+            local_basis1: cparams.basis1,
+            local_basis2: cparams.basis2,
         }
     }
 
@@ -53,6 +59,44 @@ impl RevolutePositionConstraint {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
+        /*
+         * Linear part.
+         */
+        {
+            let anchor1 = position1 * self.local_anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+
+            let r1 = anchor1 - position1 * self.local_com1;
+            let r2 = anchor2 - position2 * self.local_com2;
+
+            // TODO: don't do the "to_matrix".
+            let lhs = (self
+                .ii2
+                .quadform(&r2.gcross_matrix())
+                .add_diagonal(self.im2)
+                + self
+                    .ii1
+                    .quadform(&r1.gcross_matrix())
+                    .add_diagonal(self.im1))
+            .into_matrix();
+            let inv_lhs = lhs.try_inverse().unwrap();
+
+            let delta_tra = anchor2 - anchor1;
+            let lin_error = delta_tra * params.joint_erp;
+            let lin_impulse = inv_lhs * lin_error;
+
+            let rot1 = self.ii1 * r1.gcross(lin_impulse);
+            let rot2 = self.ii2 * r2.gcross(lin_impulse);
+            position1.rotation = Rotation::new(rot1) * position1.rotation;
+            position2.rotation = Rotation::new(-rot2) * position2.rotation;
+            position1.translation.vector += self.im1 * lin_impulse;
+            position2.translation.vector -= self.im2 * lin_impulse;
+        }
+
+        /*
+         * Angular part.
+         */
+        {
             let axis1 = position1 * self.local_axis1;
             let axis2 = position2 * self.local_axis2;
             let delta_rot =
@@ -64,16 +108,7 @@ impl RevolutePositionConstraint {
                 Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
             position2.rotation =
                 Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
-
+        }
-        let anchor1 = position1 * self.local_anchor1;
-        let anchor2 = position2 * self.local_anchor2;
-
-        let delta_tra = anchor2 - anchor1;
-        let lin_error = delta_tra * params.joint_erp;
-        let lin_impulse = self.lin_inv_lhs * lin_error;
-
-        position1.translation.vector += self.im1 * lin_impulse;
-        position2.translation.vector -= self.im2 * lin_impulse;
 
         positions[self.position1 as usize] = position1;
         positions[self.position2 as usize] = position2;
@@ -83,10 +118,16 @@ impl RevolutePositionConstraint {
 #[derive(Debug)]
 pub(crate) struct RevolutePositionGroundConstraint {
     position2: usize,
+    local_com2: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
     anchor1: Point<Real>,
     local_anchor2: Point<Real>,
     axis1: Unit<Vector<Real>>,
     local_axis2: Unit<Vector<Real>>,
+
+    basis1: [Vector<Real>; 2],
+    local_basis2: [Vector<Real>; 2],
 }
 
 impl RevolutePositionGroundConstraint {
@@ -100,42 +141,82 @@ impl RevolutePositionGroundConstraint {
         let local_anchor2;
         let axis1;
         let local_axis2;
+        let basis1;
+        let local_basis2;
 
         if flipped {
             anchor1 = rb1.predicted_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
             axis1 = rb1.predicted_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
+            basis1 = [
+                rb1.predicted_position * cparams.basis2[0],
+                rb1.predicted_position * cparams.basis2[1],
+            ];
+            local_basis2 = cparams.basis1;
         } else {
             anchor1 = rb1.predicted_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
             axis1 = rb1.predicted_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
+            basis1 = [
+                rb1.predicted_position * cparams.basis1[0],
+                rb1.predicted_position * cparams.basis1[1],
+            ];
+            local_basis2 = cparams.basis2;
         };
 
         Self {
             anchor1,
             local_anchor2,
+            im2: rb2.effective_inv_mass,
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+            local_com2: rb2.mass_properties.local_com,
             axis1,
             local_axis2,
             position2: rb2.active_set_offset,
+            basis1,
+            local_basis2,
         }
     }
 
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
-        let axis2 = position2 * self.local_axis2;
+        /*
-
+         * Linear part.
-        let delta_rot =
+         */
-            Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp)
+        {
-                .unwrap_or_else(Rotation::identity);
-        position2.rotation = delta_rot * position2.rotation;
-
             let anchor2 = position2 * self.local_anchor2;
+
+            let r2 = anchor2 - position2 * self.local_com2;
+            // TODO: don't the the "to_matrix".
+            let lhs = self
+                .ii2
+                .quadform(&r2.gcross_matrix())
+                .add_diagonal(self.im2)
+                .into_matrix();
+            let inv_lhs = lhs.try_inverse().unwrap();
+
             let delta_tra = anchor2 - self.anchor1;
             let lin_error = delta_tra * params.joint_erp;
-        position2.translation.vector -= lin_error;
+            let lin_impulse = inv_lhs * lin_error;
+
+            let rot2 = self.ii2 * r2.gcross(lin_impulse);
+            position2.rotation = Rotation::new(-rot2) * position2.rotation;
+            position2.translation.vector -= self.im2 * lin_impulse;
+        }
+
+        /*
+         * Angular part.
+         */
+        {
+            let axis2 = position2 * self.local_axis2;
+            let delta_rot = Rotation::rotation_between_axis(&self.axis1, &axis2)
+                .unwrap_or_else(Rotation::identity);
+            let ang_error = delta_rot.scaled_axis() * params.joint_erp;
+            position2.rotation = Rotation::new(-ang_error) * position2.rotation;
+        }
 
         positions[self.position2 as usize] = position2;
     }

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -1,8 +1,9 @@
-use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
-use crate::math::{AngularInertia, Real, Vector};
+use crate::math::{AngularInertia, Real, Rotation, Vector};
+use crate::na::UnitQuaternion;
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 
@@ -20,7 +21,17 @@ pub(crate) struct RevoluteVelocityConstraint {
     rhs: Vector5<Real>,
     impulse: Vector5<Real>,
 
+    motor_inv_lhs: Real,
+    motor_rhs: Real,
+    motor_impulse: Real,
+    motor_max_impulse: Real,
+    motor_angle: Real, // Exists only to write it back into the joint.
+
+    motor_axis1: Vector<Real>,
+    motor_axis2: Vector<Real>,
+
     basis1: Matrix3x2<Real>,
+    basis2: Matrix3x2<Real>,
 
     im1: Real,
     im2: Real,
@@ -35,23 +46,23 @@ impl RevoluteVelocityConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &RevoluteJoint,
+        joint: &RevoluteJoint,
     ) -> Self {
         // Linear part.
-        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor1 = rb1.position * joint.local_anchor1;
-        let anchor2 = rb2.position * cparams.local_anchor2;
+        let anchor2 = rb2.position * joint.local_anchor2;
         let basis1 = Matrix3x2::from_columns(&[
-            rb1.position * cparams.basis1[0],
+            rb1.position * joint.basis1[0],
-            rb1.position * cparams.basis1[1],
+            rb1.position * joint.basis1[1],
         ]);
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        let basis2 = Matrix3x2::from_columns(&[
-        //            .unwrap_or_else(Rotation::identity)
+            rb2.position * joint.basis2[0],
-        //            .to_rotation_matrix()
+            rb2.position * joint.basis2[1],
-        //            .into_inner();
+        ]);
-        //        let basis2 = r21 * basis1;
+        let basis_projection2 = basis2 * basis2.transpose();
-        // NOTE: to simplify, we use basis2 = basis1.
+        let basis2 = basis_projection2 * basis1;
-        // Though we may want to test if that does not introduce any instability.
+
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
 
@@ -64,12 +75,13 @@ impl RevoluteVelocityConstraint {
         let r2_mat = r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
+
         let lhs00 =
             ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat));
-        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
 
-        // Note that cholesky won't read the upper-right part
+        // Note that Cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
         lhs.fixed_slice_mut::<U3, U3>(0, 0)
             .copy_from(&lhs00.into_matrix());
@@ -78,10 +90,64 @@ impl RevoluteVelocityConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
+        /*
+         * Motor.
+         */
+        let motor_axis1 = rb1.position * *joint.local_axis1;
+        let motor_axis2 = rb2.position * *joint.local_axis2;
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+        let mut motor_angle = 0.0;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
+            motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs {
+                crate::utils::inv(
+                    motor_axis2.dot(&ii2.transform_vector(motor_axis2))
+                        + motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
+                ) * gamma
+            } else {
+                gamma
+            };
+            motor_rhs /= gamma;
+        }
+
+        /*
+         * Adjust the warmstart impulse.
+         * If the velocity along the free axis is somewhat high,
+         * we need to adjust the angular warmstart impulse because it
+         * may have a direction that is too different than last frame,
+         * making it counter-productive.
+         */
+        let mut impulse = joint.impulse * params.warmstart_coeff;
+        let axis_rot = Rotation::rotation_between(&joint.prev_axis1, &motor_axis1)
+            .unwrap_or_else(UnitQuaternion::identity);
+        let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse));
+        impulse[3] = rotated_impulse.x * params.warmstart_coeff;
+        impulse[4] = rotated_impulse.y * params.warmstart_coeff;
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+
         RevoluteVelocityConstraint {
             joint_id,
             mj_lambda1: rb1.active_set_offset,
@@ -89,13 +155,21 @@ impl RevoluteVelocityConstraint {
             im1,
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             basis1,
+            basis2,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse,
             inv_lhs,
             rhs,
             r1,
             r2,
+            motor_rhs,
+            motor_inv_lhs,
+            motor_max_impulse,
+            motor_axis1,
+            motor_axis2,
+            motor_impulse,
+            motor_angle,
         }
     }
 
@@ -103,49 +177,90 @@ impl RevoluteVelocityConstraint {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
-        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.linear += self.im1 * lin_impulse1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+
+        /*
+         * Motor
+         */
+        if self.motor_inv_lhs != 0.0 {
+            mj_lambda1.angular += self
+                .ii1_sqrt
+                .transform_vector(self.motor_axis1 * self.motor_impulse);
+            mj_lambda2.angular -= self
+                .ii2_sqrt
+                .transform_vector(self.motor_axis2 * self.motor_impulse);
+        }
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
+            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+    }
+
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+            let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1);
+            let rhs = ang_dvel + self.motor_rhs;
+
+            let new_motor_impulse = na::clamp(
+                self.motor_impulse + self.motor_inv_lhs * rhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let impulse = new_motor_impulse - self.motor_impulse;
+            self.motor_impulse = new_motor_impulse;
+
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse);
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
+        }
+    }
+
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
-        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
-        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
-        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
-            - mj_lambda1.linear
-            - ang_vel1.gcross(self.r1);
-        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -155,6 +270,11 @@ impl RevoluteVelocityConstraint {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
+            revolute.world_ang_impulse = self.basis1 * rot_part;
+            revolute.prev_axis1 = self.motor_axis1;
+            revolute.motor_last_angle = self.motor_angle;
+            revolute.motor_impulse = self.motor_impulse;
         }
     }
 }
@@ -171,7 +291,14 @@ pub(crate) struct RevoluteVelocityGroundConstraint {
     rhs: Vector5<Real>,
     impulse: Vector5<Real>,
 
-    basis1: Matrix3x2<Real>,
+    motor_axis2: Vector<Real>,
+    motor_inv_lhs: Real,
+    motor_rhs: Real,
+    motor_impulse: Real,
+    motor_max_impulse: Real,
+    motor_angle: Real, // Exists just for writing it into the joint.
+
+    basis2: Matrix3x2<Real>,
 
     im2: Real,
 
@@ -184,34 +311,46 @@ impl RevoluteVelocityGroundConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &RevoluteJoint,
+        joint: &RevoluteJoint,
         flipped: bool,
-    ) -> Self {
+    ) -> AnyJointVelocityConstraint {
         let anchor2;
         let anchor1;
+        let axis1;
+        let axis2;
         let basis1;
+        let basis2;
 
         if flipped {
-            anchor1 = rb1.position * cparams.local_anchor2;
+            axis1 = rb1.position * *joint.local_axis2;
-            anchor2 = rb2.position * cparams.local_anchor1;
+            axis2 = rb2.position * *joint.local_axis1;
+            anchor1 = rb1.position * joint.local_anchor2;
+            anchor2 = rb2.position * joint.local_anchor1;
             basis1 = Matrix3x2::from_columns(&[
-                rb1.position * cparams.basis2[0],
+                rb1.position * joint.basis2[0],
-                rb1.position * cparams.basis2[1],
+                rb1.position * joint.basis2[1],
+            ]);
+            basis2 = Matrix3x2::from_columns(&[
+                rb2.position * joint.basis1[0],
+                rb2.position * joint.basis1[1],
             ]);
         } else {
-            anchor1 = rb1.position * cparams.local_anchor1;
+            axis1 = rb1.position * *joint.local_axis1;
-            anchor2 = rb2.position * cparams.local_anchor2;
+            axis2 = rb2.position * *joint.local_axis2;
+            anchor1 = rb1.position * joint.local_anchor1;
+            anchor2 = rb2.position * joint.local_anchor2;
             basis1 = Matrix3x2::from_columns(&[
-                rb1.position * cparams.basis1[0],
+                rb1.position * joint.basis1[0],
-                rb1.position * cparams.basis1[1],
+                rb1.position * joint.basis1[1],
+            ]);
+            basis2 = Matrix3x2::from_columns(&[
+                rb2.position * joint.basis2[0],
+                rb2.position * joint.basis2[1],
             ]);
         };
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        let basis_projection2 = basis2 * basis2.transpose();
-        //            .unwrap_or_else(Rotation::identity)
+        let basis2 = basis_projection2 * basis1;
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = /*r21 * */ basis1;
         let im2 = rb2.effective_inv_mass;
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
@@ -220,8 +359,8 @@ impl RevoluteVelocityGroundConstraint {
 
         let mut lhs = Matrix5::zeros();
         let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
 
         // Note that cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
@@ -232,54 +371,130 @@ impl RevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
-        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
-        RevoluteVelocityGroundConstraint {
+        /*
+         * Motor part.
+         */
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+        let mut motor_angle = 0.0;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
+            motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs {
+                crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2))) * gamma
+            } else {
+                gamma
+            };
+            motor_rhs /= gamma;
+        }
+
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+
+        let result = RevoluteVelocityGroundConstraint {
             joint_id,
             mj_lambda2: rb2.active_set_offset,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
-            basis1,
+            basis2,
             inv_lhs,
             rhs,
             r2,
-        }
+            motor_inv_lhs,
+            motor_impulse,
+            motor_axis2: axis2,
+            motor_max_impulse,
+            motor_rhs,
+            motor_angle,
+        };
+
+        AnyJointVelocityConstraint::RevoluteGroundConstraint(result)
     }
 
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= self.im2 * lin_impulse;
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        /*
+         * Motor
+         */
+        if self.motor_inv_lhs != 0.0 {
+            mj_lambda2.angular -= self
+                .ii2_sqrt
+                .transform_vector(self.motor_axis2 * self.motor_impulse);
+        }
+
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
-        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
-
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
         let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= self.im2 * lin_impulse;
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+            let ang_dvel = ang_vel2.dot(&self.motor_axis2);
+            let rhs = ang_dvel + self.motor_rhs;
+
+            let new_motor_impulse = na::clamp(
+                self.motor_impulse + self.motor_inv_lhs * rhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let impulse = new_motor_impulse - self.motor_impulse;
+            self.motor_impulse = new_motor_impulse;
+
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
@@ -289,6 +504,8 @@ impl RevoluteVelocityGroundConstraint {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            revolute.motor_impulse = self.motor_impulse;
+            revolute.motor_last_angle = self.motor_angle;
         }
     }
 }

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -4,7 +4,9 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
-use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
+};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 
@@ -22,7 +24,9 @@ pub(crate) struct WRevoluteVelocityConstraint {
     rhs: Vector5<SimdReal>,
     impulse: Vector5<SimdReal>,
 
+    axis1: [Vector<Real>; SIMD_WIDTH],
     basis1: Matrix3x2<SimdReal>,
+    basis2: Matrix3x2<SimdReal>,
 
     im1: SimdReal,
     im2: SimdReal,
@@ -37,7 +41,7 @@ impl WRevoluteVelocityConstraint {
         joint_id: [JointIndex; SIMD_WIDTH],
         rbs1: [&RigidBody; SIMD_WIDTH],
         rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        joints: [&RevoluteJoint; SIMD_WIDTH],
     ) -> Self {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
@@ -59,26 +63,27 @@ impl WRevoluteVelocityConstraint {
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
         let local_basis1 = [
-            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
-            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
         ];
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let local_basis2 = [
+            Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
+        ];
+        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
         let basis1 =
             Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
+        let basis2 =
+            Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]);
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = r21 * basis1;
-        // NOTE: to simplify, we use basis2 = basis1.
-        // Though we may want to test if that does not introduce any instability.
         let ii1 = ii1_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r1_mat = r1.gcross_matrix();
@@ -90,10 +95,10 @@ impl WRevoluteVelocityConstraint {
         let mut lhs = Matrix5::zeros();
         let lhs00 =
             ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat));
-        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
 
-        // Note that cholesky won't read the upper-right part
+        // Note that Cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
         lhs.fixed_slice_mut::<U3, U3>(0, 0)
             .copy_from(&lhs00.into_matrix());
@@ -103,19 +108,42 @@ impl WRevoluteVelocityConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
+        /*
+         * Adjust the warmstart impulse.
+         * If the velocity along the free axis is somewhat high,
+         * we need to adjust the angular warmstart impulse because it
+         * may have a direction that is too different than last frame,
+         * making it counter-productive.
+         */
+        let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
+        let mut impulse = impulse * warmstart_coeff;
+
+        let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
+        let rotated_impulse = Vector::from(array![|ii| {
+            let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
+                .unwrap_or_else(Rotation::identity);
+            axis_rot * joints[ii].world_ang_impulse
+        }; SIMD_WIDTH]);
+
+        let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
+        impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
+        impulse[4] = rotated_basis_impulse.y * warmstart_coeff;
+
         WRevoluteVelocityConstraint {
             joint_id,
             mj_lambda1,
             mj_lambda2,
             im1,
             ii1_sqrt,
+            axis1,
             basis1,
+            basis2,
             im2,
             ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            impulse,
             inv_lhs,
             rhs,
             r1,
@@ -141,18 +169,20 @@ impl WRevoluteVelocityConstraint {
             ),
         };
 
-        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -184,26 +214,28 @@ impl WRevoluteVelocityConstraint {
 
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
+
-            - mj_lambda1.linear
+        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
-            - ang_vel1.gcross(self.r1);
+            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
-        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -216,10 +248,15 @@ impl WRevoluteVelocityConstraint {
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
+        let world_ang_impulse = self.basis1 * rot_part;
+
         for ii in 0..SIMD_WIDTH {
             let joint = &mut joints_all[self.joint_id[ii]].weight;
             if let JointParams::RevoluteJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii)
+                rev.impulse = self.impulse.extract(ii);
+                rev.world_ang_impulse = world_ang_impulse.extract(ii);
+                rev.prev_axis1 = self.axis1[ii];
             }
         }
     }
@@ -237,7 +274,7 @@ pub(crate) struct WRevoluteVelocityGroundConstraint {
     rhs: Vector5<SimdReal>,
     impulse: Vector5<SimdReal>,
 
-    basis1: Matrix3x2<SimdReal>,
+    basis2: Matrix3x2<SimdReal>,
 
     im2: SimdReal,
 
@@ -250,7 +287,7 @@ impl WRevoluteVelocityGroundConstraint {
         joint_id: [JointIndex; SIMD_WIDTH],
         rbs1: [&RigidBody; SIMD_WIDTH],
         rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        joints: [&RevoluteJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
@@ -267,33 +304,40 @@ impl WRevoluteVelocityGroundConstraint {
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
 
         let local_anchor1 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
         );
         let local_anchor2 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
         );
         let basis1 = Matrix3x2::from_columns(&[
             position1
                 * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                    array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
                 ),
             position1
                 * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                    array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
                 ),
         ]);
+        let basis2 = Matrix3x2::from_columns(&[
+            position2
+                * Vector::from(
+                    array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
+                ),
+            position2
+                * Vector::from(
+                    array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
+                ),
+        ]);
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = /*r21 * */ basis1;
         let ii2 = ii2_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r2 = anchor2 - world_com2;
@@ -301,8 +345,8 @@ impl WRevoluteVelocityGroundConstraint {
 
         let mut lhs = Matrix5::zeros();
         let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
 
         // Note that cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
@@ -313,8 +357,8 @@ impl WRevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
-        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
         WRevoluteVelocityGroundConstraint {
@@ -323,7 +367,7 @@ impl WRevoluteVelocityGroundConstraint {
             im2,
             ii2_sqrt,
             impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            basis1,
+            basis2,
             inv_lhs,
             rhs,
             r2,
@@ -341,7 +385,7 @@ impl WRevoluteVelocityGroundConstraint {
         };
 
         let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= lin_impulse * self.im2;
         mj_lambda2.angular -= self
@@ -366,13 +410,13 @@ impl WRevoluteVelocityGroundConstraint {
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
         let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= lin_impulse * self.im2;
         mj_lambda2.angular -= self

src_testbed/nphysics_backend.rs

@@ -121,7 +121,11 @@ impl NPhysicsWorld {
                     }
 
                     nphysics_joints.insert(c);
-                }
+                } // JointParams::GenericJoint(_) => {
+                  //     eprintln!(
+                  //         "Joint type currently unsupported by the nphysics backend: GenericJoint."
+                  //     )
+                  // }
             }
         }
 

src_testbed/physx_backend.rs

@@ -344,13 +344,27 @@ impl PhysxWorld {
                             .into_physx()
                             .into();
 
-                        physx_sys::phys_PxRevoluteJointCreate(
+                        let revolute_joint = physx_sys::phys_PxRevoluteJointCreate(
                             physics.as_mut_ptr(),
                             actor1,
                             &frame1 as *const _,
                             actor2,
                             &frame2 as *const _,
                         );
+
+                        physx_sys::PxRevoluteJoint_setDriveVelocity_mut(
+                            revolute_joint,
+                            params.motor_target_vel,
+                            true,
+                        );
+
+                        if params.motor_damping != 0.0 {
+                            physx_sys::PxRevoluteJoint_setRevoluteJointFlag_mut(
+                                revolute_joint,
+                                physx_sys::PxRevoluteJointFlag::eDRIVE_ENABLED,
+                                true,
+                            );
+                        }
                     }
 
                     JointParams::PrismaticJoint(params) => {
@@ -420,7 +434,11 @@ impl PhysxWorld {
                             actor2,
                             &frame2 as *const _,
                         );
-                    }
+                    } // JointParams::GenericJoint(_) => {
+                      //     eprintln!(
+                      //         "Joint type currently unsupported by the PhysX backend: GenericJoint."
+                      //     )
+                      // }
                 }
             }
         }