Joint API and joint motors improvements

src/dynamics/integration_parameters.rs

@@ -18,10 +18,6 @@ pub struct IntegrationParameters {
     /// to numerical instabilities.
     pub min_ccd_dt: Real,
 
-    /// 0-1: how much of the velocity to dampen out in the constraint solver?
-    /// (default `1.0`).
-    pub velocity_solve_fraction: Real,
-
     /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
     /// will be compensated for during the velocity solve.
     /// If zero, you need to enable the positional solver.
@@ -35,6 +31,9 @@ pub struct IntegrationParameters {
     /// (default `0.25`).
     pub damping_ratio: Real,
 
+    pub joint_erp: Real,
+    pub joint_damping_ratio: Real,
+
     /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
@@ -89,12 +88,17 @@ impl IntegrationParameters {
 
     /// The ERP coefficient, multiplied by the inverse timestep length.
     pub fn erp_inv_dt(&self) -> Real {
-        0.8 / self.dt
+        self.erp / self.dt
+    }
+
+    /// The joint ERP coefficient, multiplied by the inverse timestep length.
+    pub fn joint_erp_inv_dt(&self) -> Real {
+        self.joint_erp / self.dt
     }
 
     /// The CFM factor to be used in the constraints resolution.
     pub fn cfm_factor(&self) -> Real {
-        // Compute CFM assuming a critically damped spring multiplied by the dampingratio.
+        // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
         let inv_erp_minus_one = 1.0 / self.erp - 1.0;
 
         // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
@@ -124,6 +128,16 @@ impl IntegrationParameters {
         // in the constraints solver.
         1.0 / (1.0 + cfm_coeff)
     }
+
+    pub fn joint_cfm_coeff(&self) -> Real {
+        // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
+        let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
+        inv_erp_minus_one * inv_erp_minus_one
+            / ((1.0 + inv_erp_minus_one)
+                * 4.0
+                * self.joint_damping_ratio
+                * self.joint_damping_ratio)
+    }
 }
 
 impl Default for IntegrationParameters {
@@ -131,9 +145,10 @@ impl Default for IntegrationParameters {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            velocity_solve_fraction: 1.0,
             erp: 0.8,
             damping_ratio: 0.25,
+            joint_erp: 1.0,
+            joint_damping_ratio: 1.0,
             allowed_linear_error: 0.001, // 0.005
             prediction_distance: 0.002,
             max_velocity_iterations: 4,

src/dynamics/island_manager.rs

@@ -136,36 +136,6 @@ impl IslandManager {
             .chain(self.active_kinematic_set.iter().copied())
     }
 
-    /*
-    #[cfg(feature = "parallel")]
-    #[inline(always)]
-    #[allow(dead_code)]
-    pub(crate) fn foreach_active_island_body_mut_internal_parallel<Set>(
-        &self,
-        island_id: usize,
-        bodies: &mut Set,
-        f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
-    ) where
-        Set: ComponentSet<T>,
-    {
-        use std::sync::atomic::Ordering;
-
-        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
-        let bodies = std::sync::atomic::AtomicPtr::new(&mut bodies as *mut _);
-        self.active_dynamic_set[island_range]
-            .par_iter()
-            .for_each_init(
-                || bodies.load(Ordering::Relaxed),
-                |bodies, handle| {
-                    let bodies: &mut Set = unsafe { std::mem::transmute(*bodies) };
-                    if let Some(rb) = bodies.get_mut_internal(handle.0) {
-                        f(*handle, rb)
-                    }
-                },
-            );
-    }
-     */
-
     #[cfg(feature = "parallel")]
     pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
         self.active_islands[island_id]..self.active_islands[island_id + 1]
@@ -203,7 +173,7 @@ impl IslandManager {
         // NOTE: the `.rev()` is here so that two successive timesteps preserve
         // the order of the bodies in the `active_dynamic_set` vec. This reversal
         // does not seem to affect performances nor stability. However it makes
-        // debugging slightly nicer so we keep this rev.
+        // debugging slightly nicer.
         for h in self.active_dynamic_set.drain(..).rev() {
             let can_sleep = &mut self.can_sleep;
             let stack = &mut self.stack;

src/dynamics/joint/fixed_joint.rs

@@ -1,10 +1,11 @@
-use crate::dynamics::{JointAxesMask, JointData};
+use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask};
 use crate::math::{Isometry, Point, Real};
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
+#[repr(transparent)]
 pub struct FixedJoint {
-    data: JointData,
+    data: GenericJoint,
 }
 
 impl Default for FixedJoint {
@@ -14,48 +15,100 @@ impl Default for FixedJoint {
 }
 
 impl FixedJoint {
+    #[must_use]
     pub fn new() -> Self {
-        #[cfg(feature = "dim2")]
-        let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X;
-        #[cfg(feature = "dim3")]
-        let mask = JointAxesMask::X
-            | JointAxesMask::Y
-            | JointAxesMask::Z
-            | JointAxesMask::ANG_X
-            | JointAxesMask::ANG_Y
-            | JointAxesMask::ANG_Z;
-
-        let data = JointData::default().lock_axes(mask);
+        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
         Self { data }
     }
 
+    #[must_use]
+    pub fn local_frame1(&self) -> &Isometry<Real> {
+        &self.data.local_frame1
+    }
+
+    pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+        self.data.set_local_frame1(local_frame);
+        self
+    }
+
+    #[must_use]
+    pub fn local_frame2(&self) -> &Isometry<Real> {
+        &self.data.local_frame2
+    }
+
+    pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+        self.data.set_local_frame2(local_frame);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor1(&self) -> Point<Real> {
+        self.data.local_anchor1()
+    }
+
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(&self) -> Point<Real> {
+        self.data.local_anchor2()
+    }
+
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor2(anchor2);
+        self
+    }
+}
+
+impl Into<GenericJoint> for FixedJoint {
+    fn into(self) -> GenericJoint {
+        self.data
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq, Default)]
+pub struct FixedJointBuilder(FixedJoint);
+
+impl FixedJointBuilder {
+    pub fn new() -> Self {
+        Self(FixedJoint::new())
+    }
+
     #[must_use]
     pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
-        self.data = self.data.local_frame1(local_frame);
+        self.0.set_local_frame1(local_frame);
         self
     }
 
     #[must_use]
     pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
-        self.data = self.data.local_frame2(local_frame);
+        self.0.set_local_frame2(local_frame);
         self
     }
 
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
-        self.data = self.data.local_anchor1(anchor1);
+        self.0.set_local_anchor1(anchor1);
         self
     }
 
     #[must_use]
     pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
-        self.data = self.data.local_anchor2(anchor2);
+        self.0.set_local_anchor2(anchor2);
         self
     }
+
+    #[must_use]
+    pub fn build(self) -> FixedJoint {
+        self.0
+    }
 }
 
-impl Into<JointData> for FixedJoint {
-    fn into(self) -> JointData {
-        self.data
+impl Into<GenericJoint> for FixedJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0.into()
     }
 }

src/dynamics/joint/generic_joint.rs

@@ -0,0 +1,501 @@
+use na::SimdRealField;
+
+use crate::dynamics::solver::MotorParameters;
+use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint};
+use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM};
+use crate::utils::WBasis;
+
+#[cfg(feature = "dim3")]
+use crate::dynamics::SphericalJoint;
+
+#[cfg(feature = "dim3")]
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    pub struct JointAxesMask: u8 {
+        const X = 1 << 0;
+        const Y = 1 << 1;
+        const Z = 1 << 2;
+        const ANG_X = 1 << 3;
+        const ANG_Y = 1 << 4;
+        const ANG_Z = 1 << 5;
+        const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
+        const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
+        const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
+        const LOCKED_SPHERICAL_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits;
+        const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
+        const FREE_PRISMATIC_AXES = Self::X.bits;
+        const FREE_FIXED_AXES = 0;
+        const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
+    }
+}
+
+#[cfg(feature = "dim2")]
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    pub struct JointAxesMask: u8 {
+        const X = 1 << 0;
+        const Y = 1 << 1;
+        const ANG_X = 1 << 2;
+        const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits;
+        const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::ANG_X.bits;
+        const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::ANG_X.bits;
+        const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
+        const FREE_PRISMATIC_AXES = Self::X.bits;
+        const FREE_FIXED_AXES = 0;
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum JointAxis {
+    X = 0,
+    Y,
+    #[cfg(feature = "dim3")]
+    Z,
+    AngX,
+    #[cfg(feature = "dim3")]
+    AngY,
+    #[cfg(feature = "dim3")]
+    AngZ,
+}
+
+impl From<JointAxis> for JointAxesMask {
+    fn from(axis: JointAxis) -> Self {
+        JointAxesMask::from_bits(1 << axis as usize).unwrap()
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct JointLimits<N> {
+    pub min: N,
+    pub max: N,
+    pub impulse: N,
+}
+
+impl<N: SimdRealField<Element = Real>> Default for JointLimits<N> {
+    fn default() -> Self {
+        Self {
+            min: -N::splat(Real::MAX),
+            max: N::splat(Real::MAX),
+            impulse: N::splat(0.0),
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct JointMotor {
+    pub target_vel: Real,
+    pub target_pos: Real,
+    pub stiffness: Real,
+    pub damping: Real,
+    pub max_force: Real,
+    pub impulse: Real,
+    pub model: MotorModel,
+}
+
+impl Default for JointMotor {
+    fn default() -> Self {
+        Self {
+            target_pos: 0.0,
+            target_vel: 0.0,
+            stiffness: 0.0,
+            damping: 0.0,
+            max_force: Real::MAX,
+            impulse: 0.0,
+            model: MotorModel::AccelerationBased, // VelocityBased,
+        }
+    }
+}
+
+impl JointMotor {
+    pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
+        let (erp_inv_dt, cfm_coeff, cfm_gain) =
+            self.model
+                .combine_coefficients(dt, self.stiffness, self.damping);
+        MotorParameters {
+            erp_inv_dt,
+            cfm_coeff,
+            cfm_gain,
+            // keep_lhs,
+            target_pos: self.target_pos,
+            target_vel: self.target_vel,
+            max_impulse: self.max_force * dt,
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct GenericJoint {
+    pub local_frame1: Isometry<Real>,
+    pub local_frame2: Isometry<Real>,
+    pub locked_axes: JointAxesMask,
+    pub limit_axes: JointAxesMask,
+    pub motor_axes: JointAxesMask,
+    pub limits: [JointLimits<Real>; SPATIAL_DIM],
+    pub motors: [JointMotor; SPATIAL_DIM],
+}
+
+impl Default for GenericJoint {
+    fn default() -> Self {
+        Self {
+            local_frame1: Isometry::identity(),
+            local_frame2: Isometry::identity(),
+            locked_axes: JointAxesMask::empty(),
+            limit_axes: JointAxesMask::empty(),
+            motor_axes: JointAxesMask::empty(),
+            limits: [JointLimits::default(); SPATIAL_DIM],
+            motors: [JointMotor::default(); SPATIAL_DIM],
+        }
+    }
+}
+
+impl GenericJoint {
+    #[must_use]
+    pub fn new(locked_axes: JointAxesMask) -> Self {
+        *Self::default().lock_axes(locked_axes)
+    }
+
+    /// Can this joint use SIMD-accelerated constraint formulations?
+    pub(crate) fn supports_simd_constraints(&self) -> bool {
+        self.limit_axes.is_empty() && self.motor_axes.is_empty()
+    }
+
+    fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
+        let basis = axis.orthonormal_basis();
+
+        #[cfg(feature = "dim2")]
+        {
+            use na::{Matrix2, Rotation2, UnitComplex};
+            let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]);
+            let rotmat = Rotation2::from_matrix_unchecked(mat);
+            UnitComplex::from_rotation_matrix(&rotmat)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            use na::{Matrix3, Rotation3, UnitQuaternion};
+            let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
+            let rotmat = Rotation3::from_matrix_unchecked(mat);
+            UnitQuaternion::from_rotation_matrix(&rotmat)
+        }
+    }
+
+    pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
+        self.locked_axes |= axes;
+        self
+    }
+
+    pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+        self.local_frame1 = local_frame;
+        self
+    }
+
+    pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
+        self.local_frame2 = local_frame;
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis1(&self) -> UnitVector<Real> {
+        self.local_frame1 * Vector::x_axis()
+    }
+
+    pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
+        self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis2(&self) -> UnitVector<Real> {
+        self.local_frame2 * Vector::x_axis()
+    }
+
+    pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
+        self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor1(&self) -> Point<Real> {
+        self.local_frame1.translation.vector.into()
+    }
+
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.local_frame1.translation.vector = anchor1.coords;
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(&self) -> Point<Real> {
+        self.local_frame2.translation.vector.into()
+    }
+
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.local_frame2.translation.vector = anchor2.coords;
+        self
+    }
+
+    #[must_use]
+    pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
+        let i = axis as usize;
+        if self.limit_axes.contains(axis.into()) {
+            Some(&self.limits[i])
+        } else {
+            None
+        }
+    }
+
+    pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
+        let i = axis as usize;
+        self.limit_axes |= axis.into();
+        self.limits[i].min = limits[0];
+        self.limits[i].max = limits[1];
+        self
+    }
+
+    #[must_use]
+    pub fn motor_model(&self, axis: JointAxis) -> Option<MotorModel> {
+        let i = axis as usize;
+        if self.motor_axes.contains(axis.into()) {
+            Some(self.motors[i].model)
+        } else {
+            None
+        }
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
+        self.motors[axis as usize].model = model;
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn set_motor_velocity(
+        &mut self,
+        axis: JointAxis,
+        target_vel: Real,
+        factor: Real,
+    ) -> &mut Self {
+        self.set_motor(
+            axis,
+            self.motors[axis as usize].target_pos,
+            target_vel,
+            0.0,
+            factor,
+        )
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn set_motor_position(
+        &mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.set_motor(axis, target_pos, 0.0, stiffness, damping)
+    }
+
+    pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
+        self.motors[axis as usize].max_force = max_force;
+        self
+    }
+
+    #[must_use]
+    pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> {
+        let i = axis as usize;
+        if self.motor_axes.contains(axis.into()) {
+            Some(&self.motors[i])
+        } else {
+            None
+        }
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn set_motor(
+        &mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.motor_axes |= axis.into();
+        let i = axis as usize;
+        self.motors[i].target_vel = target_vel;
+        self.motors[i].target_pos = target_pos;
+        self.motors[i].stiffness = stiffness;
+        self.motors[i].damping = damping;
+        self
+    }
+}
+
+macro_rules! joint_conversion_methods(
+    ($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => {
+        #[must_use]
+        pub fn $as_joint(&self) -> Option<&$Joint> {
+            if self.locked_axes == $axes {
+                // SAFETY: this is OK because the target joint type is
+                //         a `repr(transparent)` newtype of `Joint`.
+                Some(unsafe { std::mem::transmute(self) })
+            } else {
+                None
+            }
+        }
+
+        #[must_use]
+        pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> {
+            if self.locked_axes == $axes {
+                // SAFETY: this is OK because the target joint type is
+                //         a `repr(transparent)` newtype of `Joint`.
+                Some(unsafe { std::mem::transmute(self) })
+            } else {
+                None
+            }
+        }
+    }
+);
+
+impl GenericJoint {
+    joint_conversion_methods!(
+        as_revolute,
+        as_revolute_mut,
+        RevoluteJoint,
+        JointAxesMask::LOCKED_REVOLUTE_AXES
+    );
+    joint_conversion_methods!(
+        as_fixed,
+        as_fixed_mut,
+        FixedJoint,
+        JointAxesMask::LOCKED_FIXED_AXES
+    );
+    joint_conversion_methods!(
+        as_prismatic,
+        as_prismatic_mut,
+        PrismaticJoint,
+        JointAxesMask::LOCKED_PRISMATIC_AXES
+    );
+
+    #[cfg(feature = "dim3")]
+    joint_conversion_methods!(
+        as_spherical,
+        as_spherical_mut,
+        SphericalJoint,
+        JointAxesMask::LOCKED_SPHERICAL_AXES
+    );
+}
+
+#[derive(Copy, Clone, Debug)]
+pub struct GenericJointBuilder(GenericJoint);
+
+impl GenericJointBuilder {
+    #[must_use]
+    pub fn new(locked_axes: JointAxesMask) -> Self {
+        Self(GenericJoint::new(locked_axes))
+    }
+
+    #[must_use]
+    pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
+        self.0.lock_axes(axes);
+        self
+    }
+
+    #[must_use]
+    pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
+        self.0.set_local_frame1(local_frame);
+        self
+    }
+
+    #[must_use]
+    pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
+        self.0.set_local_frame2(local_frame);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
+        self.0.set_local_axis1(local_axis);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
+        self.0.set_local_axis2(local_axis);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
+        self.0.set_local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
+        self.0.set_local_anchor2(anchor2);
+        self
+    }
+
+    #[must_use]
+    pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
+        self.0.set_limits(axis, limits);
+        self
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    #[must_use]
+    pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
+        self.0.set_motor_model(axis, model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    #[must_use]
+    pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
+        self.0.set_motor_velocity(axis, target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    #[must_use]
+    pub fn motor_position(
+        mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.0
+            .set_motor_position(axis, target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    #[must_use]
+    pub fn set_motor(
+        mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> Self {
+        self.0
+            .set_motor(axis, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    #[must_use]
+    pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
+        self.0.set_motor_max_force(axis, max_force);
+        self
+    }
+
+    #[must_use]
+    pub fn build(self) -> GenericJoint {
+        self.0
+    }
+}

src/dynamics/joint/impulse_joint/impulse_joint.rs

@@ -1,4 +1,4 @@
-use crate::dynamics::{ImpulseJointHandle, JointData, RigidBodyHandle};
+use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle};
 use crate::math::{Real, SpacialVector};
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -10,7 +10,7 @@ pub struct ImpulseJoint {
     /// Handle to the second body attached to this joint.
     pub body2: RigidBodyHandle,
 
-    pub data: JointData,
+    pub data: GenericJoint,
     pub impulses: SpacialVector<Real>,
 
     // A joint needs to know its handle to simplify its removal.

src/dynamics/joint/impulse_joint/impulse_joint_set.rs

@@ -3,8 +3,8 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio
 
 use crate::data::arena::Arena;
 use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
+use crate::dynamics::{GenericJoint, RigidBodyHandle};
 use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
-use crate::dynamics::{JointData, RigidBodyHandle};
 
 /// The unique identifier of a joint added to the joint set.
 /// The unique identifier of a collider added to a collider set.
@@ -177,7 +177,7 @@ impl ImpulseJointSet {
         &mut self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
-        data: impl Into<JointData>,
+        data: impl Into<GenericJoint>,
     ) -> ImpulseJointHandle {
         let data = data.into();
         let handle = self.joint_ids.insert(0.into());

src/dynamics/joint/joint_data.rs

@@ -1,275 +0,0 @@
-use crate::dynamics::solver::MotorParameters;
-use crate::dynamics::MotorModel;
-use crate::math::{Isometry, Point, Real, Rotation, UnitVector, SPATIAL_DIM};
-use crate::utils::WBasis;
-
-#[cfg(feature = "dim3")]
-bitflags::bitflags! {
-    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-    pub struct JointAxesMask: u8 {
-        const FREE = 0;
-        const X = 1 << 0;
-        const Y = 1 << 1;
-        const Z = 1 << 2;
-        const ANG_X = 1 << 3;
-        const ANG_Y = 1 << 4;
-        const ANG_Z = 1 << 5;
-    }
-}
-
-#[cfg(feature = "dim2")]
-bitflags::bitflags! {
-    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-    pub struct JointAxesMask: u8 {
-        const FREE = 0;
-        const X = 1 << 0;
-        const Y = 1 << 1;
-        const ANG_X = 1 << 2;
-    }
-}
-
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Copy, Clone, Debug, PartialEq)]
-pub enum JointAxis {
-    X = 0,
-    Y,
-    #[cfg(feature = "dim3")]
-    Z,
-    AngX,
-    #[cfg(feature = "dim3")]
-    AngY,
-    #[cfg(feature = "dim3")]
-    AngZ,
-}
-
-impl From<JointAxis> for JointAxesMask {
-    fn from(axis: JointAxis) -> Self {
-        JointAxesMask::from_bits(1 << axis as usize).unwrap()
-    }
-}
-
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Copy, Clone, Debug, PartialEq)]
-pub struct JointLimits {
-    pub min: Real,
-    pub max: Real,
-    pub impulse: Real,
-}
-
-impl Default for JointLimits {
-    fn default() -> Self {
-        Self {
-            min: -Real::MAX,
-            max: Real::MAX,
-            impulse: 0.0,
-        }
-    }
-}
-
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Copy, Clone, Debug, PartialEq)]
-pub struct JointMotor {
-    pub target_vel: Real,
-    pub target_pos: Real,
-    pub stiffness: Real,
-    pub damping: Real,
-    pub max_impulse: Real,
-    pub impulse: Real,
-    pub model: MotorModel,
-}
-
-impl Default for JointMotor {
-    fn default() -> Self {
-        Self {
-            target_pos: 0.0,
-            target_vel: 0.0,
-            stiffness: 0.0,
-            damping: 0.0,
-            max_impulse: Real::MAX,
-            impulse: 0.0,
-            model: MotorModel::VelocityBased,
-        }
-    }
-}
-
-impl JointMotor {
-    pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters<Real> {
-        let (stiffness, damping, gamma, _keep_lhs) =
-            self.model
-                .combine_coefficients(dt, self.stiffness, self.damping);
-        MotorParameters {
-            stiffness,
-            damping,
-            gamma,
-            // keep_lhs,
-            target_pos: self.target_pos,
-            target_vel: self.target_vel,
-            max_impulse: self.max_impulse,
-        }
-    }
-}
-
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Copy, Clone, Debug, PartialEq)]
-pub struct JointData {
-    pub local_frame1: Isometry<Real>,
-    pub local_frame2: Isometry<Real>,
-    pub locked_axes: JointAxesMask,
-    pub limit_axes: JointAxesMask,
-    pub motor_axes: JointAxesMask,
-    pub limits: [JointLimits; SPATIAL_DIM],
-    pub motors: [JointMotor; SPATIAL_DIM],
-}
-
-impl Default for JointData {
-    fn default() -> Self {
-        Self {
-            local_frame1: Isometry::identity(),
-            local_frame2: Isometry::identity(),
-            locked_axes: JointAxesMask::FREE,
-            limit_axes: JointAxesMask::FREE,
-            motor_axes: JointAxesMask::FREE,
-            limits: [JointLimits::default(); SPATIAL_DIM],
-            motors: [JointMotor::default(); SPATIAL_DIM],
-        }
-    }
-}
-
-impl JointData {
-    #[must_use]
-    pub fn new(locked_axes: JointAxesMask) -> Self {
-        Self::default().lock_axes(locked_axes)
-    }
-
-    /// Can this joint use SIMD-accelerated constraint formulations?
-    pub fn supports_simd_constraints(&self) -> bool {
-        self.limit_axes.is_empty() && self.motor_axes.is_empty()
-    }
-
-    #[must_use]
-    pub fn lock_axes(mut self, axes: JointAxesMask) -> Self {
-        self.locked_axes |= axes;
-        self
-    }
-
-    fn complete_ang_frame(axis: UnitVector<Real>) -> Rotation<Real> {
-        let basis = axis.orthonormal_basis();
-
-        #[cfg(feature = "dim2")]
-        {
-            use na::{Matrix2, Rotation2, UnitComplex};
-            let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]);
-            let rotmat = Rotation2::from_matrix_unchecked(mat);
-            UnitComplex::from_rotation_matrix(&rotmat)
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            use na::{Matrix3, Rotation3, UnitQuaternion};
-            let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]);
-            let rotmat = Rotation3::from_matrix_unchecked(mat);
-            UnitQuaternion::from_rotation_matrix(&rotmat)
-        }
-    }
-
-    #[must_use]
-    pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
-        self.local_frame1 = local_frame;
-        self
-    }
-
-    #[must_use]
-    pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
-        self.local_frame2 = local_frame;
-        self
-    }
-
-    #[must_use]
-    pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
-        self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
-        self
-    }
-
-    #[must_use]
-    pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
-        self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
-        self
-    }
-
-    #[must_use]
-    pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
-        self.local_frame1.translation.vector = anchor1.coords;
-        self
-    }
-
-    #[must_use]
-    pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
-        self.local_frame2.translation.vector = anchor2.coords;
-        self
-    }
-
-    #[must_use]
-    pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
-        let i = axis as usize;
-        self.limit_axes |= axis.into();
-        self.limits[i].min = limits[0];
-        self.limits[i].max = limits[1];
-        self
-    }
-
-    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
-    #[must_use]
-    pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
-        self.motors[axis as usize].model = model;
-        self
-    }
-
-    /// Sets the target velocity this motor needs to reach.
-    #[must_use]
-    pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
-        self.motor_axis(
-            axis,
-            self.motors[axis as usize].target_pos,
-            target_vel,
-            0.0,
-            factor,
-        )
-    }
-
-    /// Sets the target angle this motor needs to reach.
-    #[must_use]
-    pub fn motor_position(
-        self,
-        axis: JointAxis,
-        target_pos: Real,
-        stiffness: Real,
-        damping: Real,
-    ) -> Self {
-        self.motor_axis(axis, target_pos, 0.0, stiffness, damping)
-    }
-
-    /// Configure both the target angle and target velocity of the motor.
-    #[must_use]
-    pub fn motor_axis(
-        mut self,
-        axis: JointAxis,
-        target_pos: Real,
-        target_vel: Real,
-        stiffness: Real,
-        damping: Real,
-    ) -> Self {
-        self.motor_axes |= axis.into();
-        let i = axis as usize;
-        self.motors[i].target_vel = target_vel;
-        self.motors[i].target_pos = target_pos;
-        self.motors[i].stiffness = stiffness;
-        self.motors[i].damping = damping;
-        self
-    }
-
-    #[must_use]
-    pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
-        self.motors[axis as usize].max_impulse = max_impulse;
-        self
-    }
-}

src/dynamics/joint/mod.rs

@@ -1,17 +1,17 @@
-pub use self::fixed_joint::FixedJoint;
+pub use self::fixed_joint::*;
 pub use self::impulse_joint::*;
-pub use self::joint_data::*;
+pub use self::generic_joint::*;
 pub use self::motor_model::MotorModel;
 pub use self::multibody_joint::*;
-pub use self::prismatic_joint::PrismaticJoint;
-pub use self::revolute_joint::RevoluteJoint;
+pub use self::prismatic_joint::*;
+pub use self::revolute_joint::*;
 
 #[cfg(feature = "dim3")]
-pub use self::spherical_joint::SphericalJoint;
+pub use self::spherical_joint::*;
 
 mod fixed_joint;
 mod impulse_joint;
-mod joint_data;
+mod generic_joint;
 mod motor_model;
 mod multibody_joint;
 mod prismatic_joint;

src/dynamics/joint/motor_model.rs

@@ -5,57 +5,40 @@ use crate::math::Real;
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub enum MotorModel {
     /// 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))`
+    /// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
     AccelerationBased,
-    // /// The solved spring-like equation is:
-    // /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
-    // ForceBased,
+    /// The solved spring-like equation is:
+    /// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
+    ForceBased,
 }
 
 impl Default for MotorModel {
     fn default() -> Self {
-        MotorModel::VelocityBased
+        MotorModel::AccelerationBased
     }
 }
 
 impl MotorModel {
     /// Combines the coefficients used for solving the spring equation.
     ///
-    /// Returns the new coefficients (stiffness, damping, gamma, 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.
+    /// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain).
     pub fn combine_coefficients(
         self,
         dt: Real,
         stiffness: Real,
         damping: Real,
-    ) -> (Real, Real, Real, bool) {
+    ) -> (Real, Real, Real) {
         match self {
-            MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
             MotorModel::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)
-            } // MotorModel::ForceBased => {
-              //     let effective_stiffness = stiffness * dt;
-              //     let effective_damping = damping * dt;
-              //     let gamma = effective_stiffness * dt + effective_damping;
-              //     (effective_stiffness, effective_damping, gamma, false)
-              // }
+                let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
+                let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping);
+                (erp_inv_dt, cfm_coeff, 0.0)
+            }
+            MotorModel::ForceBased => {
+                let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
+                let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping);
+                (erp_inv_dt, 0.0, cfm_gain)
+            }
         }
     }
 }

src/dynamics/joint/multibody_joint/multibody_joint.rs

@@ -1,6 +1,6 @@
 use crate::dynamics::solver::AnyJointVelocityConstraint;
 use crate::dynamics::{
-    joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink,
+    joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
     RigidBodyVelocity,
 };
 use crate::math::{
@@ -14,13 +14,13 @@ use na::{UnitQuaternion, Vector3};
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug)]
 pub struct MultibodyJoint {
-    pub data: JointData,
+    pub data: GenericJoint,
     pub(crate) coords: SpacialVector<Real>,
     pub(crate) joint_rot: Rotation<Real>,
 }
 
 impl MultibodyJoint {
-    pub fn new(data: JointData) -> Self {
+    pub fn new(data: GenericJoint) -> Self {
         Self {
             data,
             coords: na::zero(),
@@ -29,13 +29,13 @@ impl MultibodyJoint {
     }
 
     pub(crate) fn free(pos: Isometry<Real>) -> Self {
-        let mut result = Self::new(JointData::default());
+        let mut result = Self::new(GenericJoint::default());
         result.set_free_pos(pos);
         result
     }
 
     pub(crate) fn fixed(pos: Isometry<Real>) -> Self {
-        Self::new(FixedJoint::new().local_frame1(pos).into())
+        Self::new(FixedJointBuilder::new().local_frame1(pos).build().into())
     }
 
     pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
@@ -263,19 +263,11 @@ impl MultibodyJoint {
 
         for i in 0..DIM {
             if (locked_bits & (1 << i)) == 0 {
-                if (limit_bits & (1 << i)) != 0 {
-                    joint::unit_joint_limit_constraint(
-                        params,
-                        multibody,
-                        link,
-                        [self.data.limits[i].min, self.data.limits[i].max],
-                        self.coords[i],
-                        dof_id + curr_free_dof,
-                        j_id,
-                        jacobians,
-                        constraints,
-                    );
-                }
+                let limits = if (limit_bits & (1 << i)) != 0 {
+                    Some([self.data.limits[i].min, self.data.limits[i].max])
+                } else {
+                    None
+                };
 
                 if (motor_bits & (1 << i)) != 0 {
                     joint::unit_joint_motor_constraint(
@@ -284,6 +276,21 @@ impl MultibodyJoint {
                         link,
                         &self.data.motors[i],
                         self.coords[i],
+                        limits,
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+
+                if (limit_bits & (1 << i)) != 0 {
+                    joint::unit_joint_limit_constraint(
+                        params,
+                        multibody,
+                        link,
+                        [self.data.limits[i].min, self.data.limits[i].max],
+                        self.coords[i],
                         dof_id + curr_free_dof,
                         j_id,
                         jacobians,
@@ -310,19 +317,23 @@ impl MultibodyJoint {
         // TODO: we should make special cases for multi-angular-dofs limits/motors
         for i in DIM..SPATIAL_DIM {
             if (locked_bits & (1 << i)) == 0 {
-                if (limit_bits & (1 << i)) != 0 {
+                let limits = if (limit_bits & (1 << i)) != 0 {
+                    let limits = [self.data.limits[i].min, self.data.limits[i].max];
                     joint::unit_joint_limit_constraint(
                         params,
                         multibody,
                         link,
-                        [self.data.limits[i].min, self.data.limits[i].max],
+                        limits,
                         self.coords[i],
                         dof_id + curr_free_dof,
                         j_id,
                         jacobians,
                         constraints,
                     );
-                }
+                    Some(limits)
+                } else {
+                    None
+                };
 
                 if (motor_bits & (1 << i)) != 0 {
                     joint::unit_joint_motor_constraint(
@@ -331,6 +342,7 @@ impl MultibodyJoint {
                         link,
                         &self.data.motors[i],
                         self.coords[i],
+                        limits,
                         dof_id + curr_free_dof,
                         j_id,
                         jacobians,

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -1,7 +1,7 @@
 use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
 use crate::dynamics::joint::MultibodyLink;
 use crate::dynamics::{
-    IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
+    GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
     RigidBodyIds, RigidBodyType,
 };
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
@@ -112,7 +112,7 @@ impl MultibodyJointSet {
         &mut self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
-        data: impl Into<JointData>,
+        data: impl Into<GenericJoint>,
     ) -> Option<MultibodyJointHandle> {
         let data = data.into();
         let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {

src/dynamics/joint/multibody_joint/unit_multibody_joint.rs

@@ -26,7 +26,8 @@ pub fn unit_joint_limit_constraint(
 
     let min_enabled = curr_pos < limits[0];
     let max_enabled = limits[1] < curr_pos;
-    let erp_inv_dt = params.erp_inv_dt();
+    let erp_inv_dt = params.joint_erp_inv_dt();
+    let cfm_coeff = params.joint_cfm_coeff();
     let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
     let rhs_wo_bias = joint_velocity[dof_id];
 
@@ -54,6 +55,8 @@ pub fn unit_joint_limit_constraint(
         inv_lhs: crate::utils::inv(lhs),
         rhs: rhs_wo_bias + rhs_bias,
         rhs_wo_bias,
+        cfm_coeff,
+        cfm_gain: 0.0,
         writeback_id: WritebackId::Limit(dof_id),
     };
 
@@ -71,11 +74,13 @@ pub fn unit_joint_motor_constraint(
     link: &MultibodyLink,
     motor: &JointMotor,
     curr_pos: Real,
+    limits: Option<[Real; 2]>,
     dof_id: usize,
     j_id: &mut usize,
     jacobians: &mut DVector<Real>,
     constraints: &mut Vec<AnyJointVelocityConstraint>,
 ) {
+    let inv_dt = params.inv_dt();
     let ndofs = multibody.ndofs();
     let joint_velocity = multibody.joint_velocity(link);
 
@@ -93,14 +98,20 @@ pub fn unit_joint_motor_constraint(
     let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
 
     let mut rhs_wo_bias = 0.0;
-    if motor_params.stiffness != 0.0 {
-        rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness;
+    if motor_params.erp_inv_dt != 0.0 {
+        rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt;
     }
 
-    if motor_params.damping != 0.0 {
-        let dvel = joint_velocity[dof_id];
-        rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-    }
+    let mut target_vel = motor_params.target_vel;
+    if let Some(limits) = limits {
+        target_vel = target_vel.clamp(
+            (limits[0] - curr_pos) * inv_dt,
+            (limits[1] - curr_pos) * inv_dt,
+        );
+    };
+
+    let dvel = joint_velocity[dof_id];
+    rhs_wo_bias += dvel - target_vel;
 
     let constraint = JointGenericVelocityGroundConstraint {
         mj_lambda2: multibody.solver_id,
@@ -109,6 +120,8 @@ pub fn unit_joint_motor_constraint(
         joint_id: usize::MAX,
         impulse: 0.0,
         impulse_bounds,
+        cfm_coeff: motor_params.cfm_coeff,
+        cfm_gain: motor_params.cfm_gain,
         inv_lhs: crate::utils::inv(lhs),
         rhs: rhs_wo_bias,
         rhs_wo_bias,

src/dynamics/joint/prismatic_joint.rs

@@ -1,91 +1,215 @@
-use crate::dynamics::joint::{JointAxesMask, JointData};
+use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
 use crate::dynamics::{JointAxis, MotorModel};
 use crate::math::{Point, Real, UnitVector};
 
+use super::{JointLimits, JointMotor};
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
+#[repr(transparent)]
 pub struct PrismaticJoint {
-    data: JointData,
+    data: GenericJoint,
 }
 
 impl PrismaticJoint {
     pub fn new(axis: UnitVector<Real>) -> Self {
-        #[cfg(feature = "dim2")]
-        let mask = JointAxesMask::Y | JointAxesMask::ANG_X;
-        #[cfg(feature = "dim3")]
-        let mask = JointAxesMask::Y
-            | JointAxesMask::Z
-            | JointAxesMask::ANG_X
-            | JointAxesMask::ANG_Y
-            | JointAxesMask::ANG_Z;
-
-        let data = JointData::default()
-            .lock_axes(mask)
+        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES)
             .local_axis1(axis)
-            .local_axis2(axis);
+            .local_axis2(axis)
+            .build();
         Self { data }
     }
 
+    #[must_use]
+    pub fn local_anchor1(&self) -> Point<Real> {
+        self.data.local_anchor1()
+    }
+
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(&self) -> Point<Real> {
+        self.data.local_anchor2()
+    }
+
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor2(anchor2);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis1(&self) -> UnitVector<Real> {
+        self.data.local_axis1()
+    }
+
+    pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
+        self.data.set_local_axis1(axis1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis2(&self) -> UnitVector<Real> {
+        self.data.local_axis2()
+    }
+
+    pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &mut Self {
+        self.data.set_local_axis2(axis2);
+        self
+    }
+
+    #[must_use]
+    pub fn motor(&self) -> Option<&JointMotor> {
+        self.data.motor(JointAxis::X)
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
+        self.data.set_motor_model(JointAxis::X, model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
+        self.data
+            .set_motor_velocity(JointAxis::X, target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn set_motor_position(
+        &mut self,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor_position(JointAxis::X, target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn set_motor(
+        &mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
+        self.data.set_motor_max_force(JointAxis::X, max_force);
+        self
+    }
+
+    #[must_use]
+    pub fn limits(&self) -> Option<&JointLimits<Real>> {
+        self.data.limits(JointAxis::X)
+    }
+
+    pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
+        self.data.set_limits(JointAxis::X, limits);
+        self
+    }
+}
+
+impl Into<GenericJoint> for PrismaticJoint {
+    fn into(self) -> GenericJoint {
+        self.data
+    }
+}
+
+pub struct PrismaticJointBuilder(PrismaticJoint);
+
+impl PrismaticJointBuilder {
+    pub fn new(axis: UnitVector<Real>) -> Self {
+        Self(PrismaticJoint::new(axis))
+    }
+
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
-        self.data = self.data.local_anchor1(anchor1);
+        self.0.set_local_anchor1(anchor1);
         self
     }
 
     #[must_use]
     pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
-        self.data = self.data.local_anchor2(anchor2);
+        self.0.set_local_anchor2(anchor2);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis1(mut self, axis1: UnitVector<Real>) -> Self {
+        self.0.set_local_axis1(axis1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
+        self.0.set_local_axis2(axis2);
         self
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    #[must_use]
     pub fn motor_model(mut self, model: MotorModel) -> Self {
-        self.data = self.data.motor_model(JointAxis::X, model);
+        self.0.set_motor_model(model);
         self
     }
 
     /// Sets the target velocity this motor needs to reach.
+    #[must_use]
     pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
-        self.data = self.data.motor_velocity(JointAxis::X, target_vel, factor);
+        self.0.set_motor_velocity(target_vel, factor);
         self
     }
 
     /// Sets the target angle this motor needs to reach.
+    #[must_use]
     pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
-        self.data = self
-            .data
-            .motor_position(JointAxis::X, target_pos, stiffness, damping);
+        self.0.set_motor_position(target_pos, stiffness, damping);
         self
     }
 
     /// Configure both the target angle and target velocity of the motor.
-    pub fn motor_axis(
+    #[must_use]
+    pub fn set_motor(
         mut self,
         target_pos: Real,
         target_vel: Real,
         stiffness: Real,
         damping: Real,
     ) -> Self {
-        self.data = self
-            .data
-            .motor_axis(JointAxis::X, target_pos, target_vel, stiffness, damping);
-        self
-    }
-
-    pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self {
-        self.data = self.data.motor_max_impulse(JointAxis::X, max_impulse);
+        self.0.set_motor(target_pos, target_vel, stiffness, damping);
         self
     }
 
     #[must_use]
-    pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
-        self.data = self.data.limit_axis(JointAxis::X, limits);
+    pub fn motor_max_force(mut self, max_force: Real) -> Self {
+        self.0.set_motor_max_force(max_force);
         self
     }
+
+    #[must_use]
+    pub fn limits(mut self, limits: [Real; 2]) -> Self {
+        self.0.set_limits(limits);
+        self
+    }
+
+    #[must_use]
+    pub fn build(self) -> PrismaticJoint {
+        self.0
+    }
 }
 
-impl Into<JointData> for PrismaticJoint {
-    fn into(self) -> JointData {
-        self.data
+impl Into<GenericJoint> for PrismaticJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0.into()
     }
 }

src/dynamics/joint/revolute_joint.rs

@@ -1,5 +1,5 @@
-use crate::dynamics::joint::{JointAxesMask, JointData};
-use crate::dynamics::{JointAxis, MotorModel};
+use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
+use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel};
 use crate::math::{Point, Real};
 
 #[cfg(feature = "dim3")]
@@ -7,100 +7,197 @@ use crate::math::UnitVector;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
+#[repr(transparent)]
 pub struct RevoluteJoint {
-    data: JointData,
+    data: GenericJoint,
 }
 
 impl RevoluteJoint {
     #[cfg(feature = "dim2")]
     pub fn new() -> Self {
-        let mask = JointAxesMask::X | JointAxesMask::Y;
-
-        let data = JointData::default().lock_axes(mask);
-        Self { data }
+        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
+        Self { data: data.build() }
     }
 
     #[cfg(feature = "dim3")]
     pub fn new(axis: UnitVector<Real>) -> Self {
-        let mask = JointAxesMask::X
-            | JointAxesMask::Y
-            | JointAxesMask::Z
-            | JointAxesMask::ANG_Y
-            | JointAxesMask::ANG_Z;
-
-        let data = JointData::default()
-            .lock_axes(mask)
+        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
             .local_axis1(axis)
-            .local_axis2(axis);
+            .local_axis2(axis)
+            .build();
         Self { data }
     }
 
-    pub fn data(&self) -> &JointData {
+    pub fn data(&self) -> &GenericJoint {
         &self.data
     }
 
+    #[must_use]
+    pub fn local_anchor1(&self) -> Point<Real> {
+        self.data.local_anchor1()
+    }
+
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor1(anchor1);
+        self
+    }
+
+    #[must_use]
+    pub fn local_anchor2(&self) -> Point<Real> {
+        self.data.local_anchor2()
+    }
+
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor2(anchor2);
+        self
+    }
+
+    #[must_use]
+    pub fn motor(&self) -> Option<&JointMotor> {
+        self.data.motor(JointAxis::AngX)
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self {
+        self.data.set_motor_model(JointAxis::AngX, model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self {
+        self.data
+            .set_motor_velocity(JointAxis::AngX, target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn set_motor_position(
+        &mut self,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor_position(JointAxis::AngX, target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn set_motor(
+        &mut self,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor(JointAxis::AngX, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self {
+        self.data.set_motor_max_force(JointAxis::AngX, max_force);
+        self
+    }
+
+    #[must_use]
+    pub fn limits(&self) -> Option<&JointLimits<Real>> {
+        self.data.limits(JointAxis::AngX)
+    }
+
+    pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self {
+        self.data.set_limits(JointAxis::AngX, limits);
+        self
+    }
+}
+
+impl Into<GenericJoint> for RevoluteJoint {
+    fn into(self) -> GenericJoint {
+        self.data
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct RevoluteJointBuilder(RevoluteJoint);
+
+impl RevoluteJointBuilder {
+    #[cfg(feature = "dim2")]
+    pub fn new() -> Self {
+        Self(RevoluteJoint::new())
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn new(axis: UnitVector<Real>) -> Self {
+        Self(RevoluteJoint::new(axis))
+    }
+
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
-        self.data = self.data.local_anchor1(anchor1);
+        self.0.set_local_anchor1(anchor1);
         self
     }
 
     #[must_use]
     pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
-        self.data = self.data.local_anchor2(anchor2);
+        self.0.set_local_anchor2(anchor2);
         self
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    #[must_use]
     pub fn motor_model(mut self, model: MotorModel) -> Self {
-        self.data = self.data.motor_model(JointAxis::AngX, model);
+        self.0.set_motor_model(model);
         self
     }
 
     /// Sets the target velocity this motor needs to reach.
+    #[must_use]
     pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
-        self.data = self
-            .data
-            .motor_velocity(JointAxis::AngX, target_vel, factor);
+        self.0.set_motor_velocity(target_vel, factor);
         self
     }
 
     /// Sets the target angle this motor needs to reach.
+    #[must_use]
     pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
-        self.data = self
-            .data
-            .motor_position(JointAxis::AngX, target_pos, stiffness, damping);
+        self.0.set_motor_position(target_pos, stiffness, damping);
         self
     }
 
     /// Configure both the target angle and target velocity of the motor.
-    pub fn motor_axis(
+    #[must_use]
+    pub fn motor(
         mut self,
         target_pos: Real,
         target_vel: Real,
         stiffness: Real,
         damping: Real,
     ) -> Self {
-        self.data =
-            self.data
-                .motor_axis(JointAxis::AngX, target_pos, target_vel, stiffness, damping);
-        self
-    }
-
-    pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self {
-        self.data = self.data.motor_max_impulse(JointAxis::AngX, max_impulse);
+        self.0.set_motor(target_pos, target_vel, stiffness, damping);
         self
     }
 
     #[must_use]
-    pub fn limit_axis(mut self, limits: [Real; 2]) -> Self {
-        self.data = self.data.limit_axis(JointAxis::AngX, limits);
+    pub fn motor_max_force(mut self, max_force: Real) -> Self {
+        self.0.set_motor_max_force(max_force);
         self
     }
+
+    #[must_use]
+    pub fn limits(mut self, limits: [Real; 2]) -> Self {
+        self.0.set_limits(limits);
+        self
+    }
+
+    #[must_use]
+    pub fn build(self) -> RevoluteJoint {
+        self.0
+    }
 }
 
-impl Into<JointData> for RevoluteJoint {
-    fn into(self) -> JointData {
-        self.data
+impl Into<GenericJoint> for RevoluteJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0.into()
     }
 }

src/dynamics/joint/spherical_joint.rs

@@ -1,11 +1,12 @@
-use crate::dynamics::joint::{JointAxesMask, JointData};
+use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
 use crate::dynamics::{JointAxis, MotorModel};
 use crate::math::{Point, Real};
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
+#[repr(transparent)]
 pub struct SphericalJoint {
-    data: JointData,
+    data: GenericJoint,
 }
 
 impl Default for SphericalJoint {
@@ -16,40 +17,128 @@ impl Default for SphericalJoint {
 
 impl SphericalJoint {
     pub fn new() -> Self {
-        let data =
-            JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z);
+        let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build();
         Self { data }
     }
 
-    pub fn data(&self) -> &JointData {
+    pub fn data(&self) -> &GenericJoint {
         &self.data
     }
 
+    pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor1(anchor1);
+        self
+    }
+
+    pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
+        self.data.set_local_anchor2(anchor2);
+        self
+    }
+
+    /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self {
+        self.data.set_motor_model(axis, model);
+        self
+    }
+
+    /// Sets the target velocity this motor needs to reach.
+    pub fn set_motor_velocity(
+        &mut self,
+        axis: JointAxis,
+        target_vel: Real,
+        factor: Real,
+    ) -> &mut Self {
+        self.data.set_motor_velocity(axis, target_vel, factor);
+        self
+    }
+
+    /// Sets the target angle this motor needs to reach.
+    pub fn set_motor_position(
+        &mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor_position(axis, target_pos, stiffness, damping);
+        self
+    }
+
+    /// Configure both the target angle and target velocity of the motor.
+    pub fn set_motor(
+        &mut self,
+        axis: JointAxis,
+        target_pos: Real,
+        target_vel: Real,
+        stiffness: Real,
+        damping: Real,
+    ) -> &mut Self {
+        self.data
+            .set_motor(axis, target_pos, target_vel, stiffness, damping);
+        self
+    }
+
+    pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self {
+        self.data.set_motor_max_force(axis, max_force);
+        self
+    }
+
+    pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self {
+        self.data.set_limits(axis, limits);
+        self
+    }
+}
+
+impl Into<GenericJoint> for SphericalJoint {
+    fn into(self) -> GenericJoint {
+        self.data
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct SphericalJointBuilder(SphericalJoint);
+
+impl Default for SphericalJointBuilder {
+    fn default() -> Self {
+        Self(SphericalJoint::new())
+    }
+}
+
+impl SphericalJointBuilder {
+    pub fn new() -> Self {
+        Self(SphericalJoint::new())
+    }
+
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
-        self.data = self.data.local_anchor1(anchor1);
+        self.0.set_local_anchor1(anchor1);
         self
     }
 
     #[must_use]
     pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
-        self.data = self.data.local_anchor2(anchor2);
+        self.0.set_local_anchor2(anchor2);
         self
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    #[must_use]
     pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
-        self.data = self.data.motor_model(axis, model);
+        self.0.set_motor_model(axis, model);
         self
     }
 
     /// Sets the target velocity this motor needs to reach.
+    #[must_use]
     pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
-        self.data = self.data.motor_velocity(axis, target_vel, factor);
+        self.0.set_motor_velocity(axis, target_vel, factor);
         self
     }
 
     /// Sets the target angle this motor needs to reach.
+    #[must_use]
     pub fn motor_position(
         mut self,
         axis: JointAxis,
@@ -57,14 +146,14 @@ impl SphericalJoint {
         stiffness: Real,
         damping: Real,
     ) -> Self {
-        self.data = self
-            .data
-            .motor_position(axis, target_pos, stiffness, damping);
+        self.0
+            .set_motor_position(axis, target_pos, stiffness, damping);
         self
     }
 
     /// Configure both the target angle and target velocity of the motor.
-    pub fn motor_axis(
+    #[must_use]
+    pub fn motor(
         mut self,
         axis: JointAxis,
         target_pos: Real,
@@ -72,26 +161,31 @@ impl SphericalJoint {
         stiffness: Real,
         damping: Real,
     ) -> Self {
-        self.data = self
-            .data
-            .motor_axis(axis, target_pos, target_vel, stiffness, damping);
-        self
-    }
-
-    pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
-        self.data = self.data.motor_max_impulse(axis, max_impulse);
+        self.0
+            .set_motor(axis, target_pos, target_vel, stiffness, damping);
         self
     }
 
     #[must_use]
-    pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
-        self.data = self.data.limit_axis(axis, limits);
+    pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
+        self.0.set_motor_max_force(axis, max_force);
         self
     }
+
+    #[must_use]
+    pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self {
+        self.0.set_limits(axis, limits);
+        self
+    }
+
+    #[must_use]
+    pub fn build(self) -> SphericalJoint {
+        self.0
+    }
 }
 
-impl Into<JointData> for SphericalJoint {
-    fn into(self) -> JointData {
-        self.data
+impl Into<GenericJoint> for SphericalJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0.into()
     }
 }

src/dynamics/rigid_body.rs

@@ -1096,3 +1096,9 @@ impl RigidBodyBuilder {
         rb
     }
 }
+
+impl Into<RigidBody> for RigidBodyBuilder {
+    fn into(self) -> RigidBody {
+        self.build()
+    }
+}

src/dynamics/rigid_body_set.rs

@@ -121,7 +121,8 @@ impl RigidBodySet {
     }
 
     /// Insert a rigid body into this set and retrieve its handle.
-    pub fn insert(&mut self, mut rb: RigidBody) -> RigidBodyHandle {
+    pub fn insert(&mut self, rb: impl Into<RigidBody>) -> RigidBodyHandle {
+        let mut rb = rb.into();
         // Make sure the internal links are reset, they may not be
         // if this rigid-body was obtained by cloning another one.
         rb.reset_internal_references();

src/dynamics/solver/generic_velocity_constraint.rs

@@ -261,7 +261,7 @@ impl GenericVelocityConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
 

src/dynamics/solver/generic_velocity_ground_constraint.rs

@@ -145,7 +145,7 @@ impl GenericVelocityGroundConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting ;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
 

src/dynamics/solver/island_solver.rs

@@ -1,6 +1,6 @@
 use super::VelocitySolver;
 use crate::counters::Counters;
-use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::data::{ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
     AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
     SolverConstraints,

src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
 use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
 use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex, Multibody};
+use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
 use crate::math::{Isometry, Real, DIM};
 use crate::prelude::SPATIAL_DIM;
 use na::{DVector, DVectorSlice, DVectorSliceMut};
@@ -25,6 +25,8 @@ pub struct JointGenericVelocityConstraint {
     pub inv_lhs: Real,
     pub rhs: Real,
     pub rhs_wo_bias: Real,
+    pub cfm_coeff: Real,
+    pub cfm_gain: Real,
 
     pub writeback_id: WritebackId,
 }
@@ -52,6 +54,8 @@ impl JointGenericVelocityConstraint {
             inv_lhs: 0.0,
             rhs: 0.0,
             rhs_wo_bias: 0.0,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -65,7 +69,7 @@ impl JointGenericVelocityConstraint {
         mb2: Option<(&Multibody, usize)>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         out: &mut [Self],
@@ -83,26 +87,10 @@ impl JointGenericVelocityConstraint {
             locked_axes,
         );
 
-        for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_generic(
-                    params,
-                    jacobians,
-                    j_id,
-                    joint_id,
-                    body1,
-                    body2,
-                    mb1,
-                    mb2,
-                    i,
-                    WritebackId::Dof(i),
-                );
-                len += 1;
-            }
-        }
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_generic(
+            if (motor_axes >> DIM) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_generic(
                     params,
                     jacobians,
                     j_id,
@@ -112,12 +100,12 @@ impl JointGenericVelocityConstraint {
                     mb1,
                     mb2,
                     i - DIM,
-                    WritebackId::Dof(i),
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
             if motor_axes & (1 << i) != 0 {
                 out[len] = builder.motor_linear_generic(
@@ -137,10 +125,15 @@ impl JointGenericVelocityConstraint {
                 len += 1;
             }
         }
+        JointVelocityConstraintBuilder::finalize_generic_constraints(
+            jacobians,
+            &mut out[start..len],
+        );
 
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if (motor_axes >> DIM) & (1 << i) != 0 {
-                out[len] = builder.motor_angular_generic(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_generic(
                     params,
                     jacobians,
                     j_id,
@@ -150,16 +143,14 @@ impl JointGenericVelocityConstraint {
                     mb1,
                     mb2,
                     i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear_generic(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_generic(
                     params,
                     jacobians,
                     j_id,
@@ -169,8 +160,7 @@ impl JointGenericVelocityConstraint {
                     mb1,
                     mb2,
                     i,
-                    [joint.limits[i].min, joint.limits[i].max],
-                    WritebackId::Limit(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
@@ -194,8 +184,29 @@ impl JointGenericVelocityConstraint {
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
 
-        JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]);
+        JointVelocityConstraintBuilder::finalize_generic_constraints(
+            jacobians,
+            &mut out[start..len],
+        );
         len
     }
 
@@ -273,7 +284,7 @@ impl JointGenericVelocityConstraint {
 
         let dvel = self.rhs + (vel2 - vel1);
         let total_impulse = na::clamp(
-            self.impulse + self.inv_lhs * dvel,
+            self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
             self.impulse_bounds[0],
             self.impulse_bounds[1],
         );
@@ -316,6 +327,8 @@ pub struct JointGenericVelocityGroundConstraint {
     pub inv_lhs: Real,
     pub rhs: Real,
     pub rhs_wo_bias: Real,
+    pub cfm_coeff: Real,
+    pub cfm_gain: Real,
 
     pub writeback_id: WritebackId,
 }
@@ -338,6 +351,8 @@ impl JointGenericVelocityGroundConstraint {
             inv_lhs: 0.0,
             rhs: 0.0,
             rhs_wo_bias: 0.0,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -350,7 +365,7 @@ impl JointGenericVelocityGroundConstraint {
         mb2: (&Multibody, usize),
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         out: &mut [Self],
@@ -368,32 +383,20 @@ impl JointGenericVelocityGroundConstraint {
             locked_axes,
         );
 
-        for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_generic_ground(
-                    params,
-                    jacobians,
-                    j_id,
-                    joint_id,
-                    body1,
-                    mb2,
-                    i,
-                    WritebackId::Dof(i),
-                );
-                len += 1;
-            }
-        }
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_generic_ground(
+            if (motor_axes >> DIM) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_generic_ground(
                     params,
                     jacobians,
                     j_id,
                     joint_id,
                     body1,
+                    body2,
                     mb2,
                     i - DIM,
-                    WritebackId::Dof(i),
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
                 );
                 len += 1;
             }
@@ -418,27 +421,30 @@ impl JointGenericVelocityGroundConstraint {
             }
         }
 
+        JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
+            jacobians,
+            &mut out[start..len],
+        );
+
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if (motor_axes >> DIM) & (1 << i) != 0 {
-                out[len] = builder.motor_angular_generic_ground(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_generic_ground(
                     params,
                     jacobians,
                     j_id,
                     joint_id,
                     body1,
-                    body2,
                     mb2,
                     i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear_generic_ground(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_generic_ground(
                     params,
                     jacobians,
                     j_id,
@@ -446,8 +452,7 @@ impl JointGenericVelocityGroundConstraint {
                     body1,
                     mb2,
                     i,
-                    [joint.limits[i].min, joint.limits[i].max],
-                    WritebackId::Limit(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
@@ -469,10 +474,26 @@ impl JointGenericVelocityGroundConstraint {
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    mb2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
 
         JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
             jacobians,
-            &mut out[..len],
+            &mut out[start..len],
         );
         len
     }
@@ -511,7 +532,7 @@ impl JointGenericVelocityGroundConstraint {
 
         let dvel = self.rhs + vel2;
         let total_impulse = na::clamp(
-            self.impulse + self.inv_lhs * dvel,
+            self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
             self.impulse_bounds[0],
             self.impulse_bounds[1],
         );

src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs

@@ -113,7 +113,7 @@ impl JointVelocityConstraintBuilder<Real> {
             j.copy_from(&wj);
         }
 
-        let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
+        let rhs_wo_bias = vel2 - vel1;
 
         let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]);
         let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
@@ -133,6 +133,8 @@ impl JointVelocityConstraintBuilder<Real> {
             inv_lhs: 0.0,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id,
         }
     }
@@ -169,7 +171,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac2,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
         c.rhs += rhs_bias;
         c
@@ -212,7 +214,7 @@ impl JointVelocityConstraintBuilder<Real> {
         let min_enabled = dist < limits[0];
         let max_enabled = limits[1] < dist;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
         constraint.rhs += rhs_bias;
         constraint.impulse_bounds = [
@@ -265,20 +267,20 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs_wo_bias = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             let dist = self.lin_err.dot(&lin_jac);
-            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != 0.0 {
-            let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
-                + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-        }
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
         constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint
     }
 
@@ -312,7 +314,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
@@ -364,7 +366,7 @@ impl JointVelocityConstraintBuilder<Real> {
             max_enabled as u32 as Real * Real::MAX,
         ];
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias =
             ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
 
@@ -409,22 +411,22 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs_wo_bias = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             #[cfg(feature = "dim2")]
             let s_ang_dist = self.ang_err.im;
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
             let s_target_ang = motor_params.target_pos.sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != 0.0 {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs_wo_bias += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         constraint.rhs_wo_bias = rhs_wo_bias;
         constraint.rhs = rhs_wo_bias;
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint
     }
@@ -436,6 +438,11 @@ impl JointVelocityConstraintBuilder<Real> {
         // TODO: orthogonalization doesn’t seem to give good results for multibodies?
         const ORTHOGONALIZE: bool = false;
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let ndofs1 = constraints[0].ndofs1;
         let ndofs2 = constraints[0].ndofs2;
 
@@ -449,8 +456,10 @@ impl JointVelocityConstraintBuilder<Real> {
             let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
 
             let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
-            let inv_dot_jj = crate::utils::inv(dot_jj);
-            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
+            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
+            c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
+            c_j.cfm_gain = cfm_gain;
 
             if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
                 // Don't remove constraints with limited forces from the others
@@ -510,7 +519,7 @@ impl JointVelocityConstraintBuilder<Real> {
         let vel2 = mb2
             .fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
             .1;
-        let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
+        let rhs_wo_bias = vel2 - vel1;
 
         let mj_lambda2 = mb2.solver_id;
 
@@ -524,6 +533,8 @@ impl JointVelocityConstraintBuilder<Real> {
             inv_lhs: 0.0,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id,
         }
     }
@@ -556,7 +567,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac2,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
         c.rhs += rhs_bias;
         c
@@ -595,7 +606,7 @@ impl JointVelocityConstraintBuilder<Real> {
         let min_enabled = dist < limits[0];
         let max_enabled = limits[1] < dist;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
         constraint.rhs += rhs_bias;
         constraint.impulse_bounds = [
@@ -645,20 +656,20 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs_wo_bias = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             let dist = self.lin_err.dot(&lin_jac);
-            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != 0.0 {
-            let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
-                + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-        }
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
         constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint
     }
 
@@ -688,7 +699,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
@@ -736,7 +747,7 @@ impl JointVelocityConstraintBuilder<Real> {
             max_enabled as u32 as Real * Real::MAX,
         ];
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias =
             ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
 
@@ -778,22 +789,22 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             #[cfg(feature = "dim2")]
             let s_ang_dist = self.ang_err.im;
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
             let s_target_ang = motor_params.target_pos.sin();
-            rhs += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != 0.0 {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs += dvel - motor_params.target_vel;
 
         constraint.rhs_wo_bias = rhs;
         constraint.rhs = rhs;
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint
     }
@@ -805,6 +816,11 @@ impl JointVelocityConstraintBuilder<Real> {
         // TODO: orthogonalization doesn’t seem to give good results for multibodies?
         const ORTHOGONALIZE: bool = false;
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let ndofs2 = constraints[0].ndofs2;
 
         // Use the modified Gramm-Schmidt orthogonalization.
@@ -815,8 +831,10 @@ impl JointVelocityConstraintBuilder<Real> {
             let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
 
             let dot_jj = jac_j2.dot(&w_jac_j2);
-            let inv_dot_jj = crate::utils::inv(dot_jj);
-            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
+            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
+            c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
+            c_j.cfm_gain = cfm_gain;
 
             if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
                 // Don't remove constraints with limited forces from the others

src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs

@@ -1,6 +1,8 @@
 use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
 use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
+};
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
 use crate::utils::{WDot, WReal};
 
@@ -12,10 +14,9 @@ use {
 
 #[derive(Copy, Clone, PartialEq, Debug)]
 pub struct MotorParameters<N: WReal> {
-    pub stiffness: N,
-    pub damping: N,
-    pub gamma: N,
-    // pub keep_lhs: bool,
+    pub erp_inv_dt: N,
+    pub cfm_coeff: N,
+    pub cfm_gain: N,
     pub target_pos: N,
     pub target_vel: N,
     pub max_impulse: N,
@@ -24,10 +25,9 @@ pub struct MotorParameters<N: WReal> {
 impl<N: WReal> Default for MotorParameters<N> {
     fn default() -> Self {
         Self {
-            stiffness: N::zero(),
-            damping: N::zero(),
-            gamma: N::zero(),
-            // keep_lhs: true,
+            erp_inv_dt: N::zero(),
+            cfm_coeff: N::zero(),
+            cfm_gain: N::zero(),
             target_pos: N::zero(),
             target_vel: N::zero(),
             max_impulse: N::zero(),
@@ -72,6 +72,8 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
     pub inv_lhs: N,
     pub rhs: N,
     pub rhs_wo_bias: N,
+    pub cfm_gain: N,
+    pub cfm_coeff: N,
 
     pub im1: Vector<N>,
     pub im2: Vector<N>,
@@ -91,6 +93,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
             ang_jac1: na::zero(),
             ang_jac2: na::zero(),
             inv_lhs: N::zero(),
+            cfm_gain: N::zero(),
+            cfm_coeff: N::zero(),
             rhs: N::zero(),
             rhs_wo_bias: N::zero(),
             im1: na::zero(),
@@ -105,7 +109,7 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
             self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
 
         let rhs = dlinvel + dangvel + self.rhs;
-        let total_impulse = (self.impulse + self.inv_lhs * rhs)
+        let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
             .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
         let delta_impulse = total_impulse - self.impulse;
         self.impulse = total_impulse;
@@ -133,13 +137,14 @@ impl JointVelocityConstraint<Real, 1> {
         body2: &SolverBody<Real, 1>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         out: &mut [Self],
     ) -> usize {
         let mut len = 0;
         let locked_axes = joint.locked_axes.bits();
-        let motor_axes = joint.motor_axes.bits();
-        let limit_axes = joint.limit_axes.bits();
+        let motor_axes = joint.motor_axes.bits() & !locked_axes;
+        let limit_axes = joint.limit_axes.bits() & !locked_axes;
+        let coupled_axes = joint.coupled_axes.bits();
 
         let builder = JointVelocityConstraintBuilder::new(
             frame1,
@@ -149,13 +154,62 @@ impl JointVelocityConstraint<Real, 1> {
             locked_axes,
         );
 
-        for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] =
-                    builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
+        let start = len;
+        for i in DIM..SPATIAL_DIM {
+            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
+                out[len] = builder.motor_angular(
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
+                let limits = if limit_axes & (1 << i) != 0 {
+                    Some([joint.limits[i].min, joint.limits[i].max])
+                } else {
+                    None
+                };
+
+                out[len] = builder.motor_linear(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    &joint.motors[i].motor_params(params.dt),
+                    limits,
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
+
+        if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+            // TODO: coupled angular motor constraint.
+        }
+
+        if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
+            // TODO: coupled linear limit constraint.
+            // out[len] = builder.motor_linear_coupled(
+            //     params,
+            //     [joint_id],
+            //     body1,
+            //     body2,
+            //     limit_axes & coupled_axes,
+            //     &joint.limits,
+            //     WritebackId::Limit(0), // TODO: writeback
+            // );
+            // len += 1;
+        }
+        JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
+
+        let start = len;
         for i in DIM..SPATIAL_DIM {
             if locked_axes & (1 << i) != 0 {
                 out[len] = builder.lock_angular(
@@ -169,52 +223,16 @@ impl JointVelocityConstraint<Real, 1> {
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_linear(
-                    params,
-                    [joint_id],
-                    body1,
-                    body2,
-                    locked_axes >> DIM,
-                    i,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
-                );
-                len += 1;
-            }
-        }
-        for i in DIM..SPATIAL_DIM {
-            if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_angular(
-                    [joint_id],
-                    body1,
-                    body2,
-                    i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
-                );
+            if locked_axes & (1 << i) != 0 {
+                out[len] =
+                    builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
                 len += 1;
             }
         }
 
-        for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear(
-                    params,
-                    [joint_id],
-                    body1,
-                    body2,
-                    i,
-                    [joint.limits[i].min, joint.limits[i].max],
-                    WritebackId::Limit(i),
-                );
-                len += 1;
-            }
-        }
         for i in DIM..SPATIAL_DIM {
-            if limit_axes & (1 << i) != 0 {
+            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
                 out[len] = builder.limit_angular(
                     params,
                     [joint_id],
@@ -227,8 +245,40 @@ impl JointVelocityConstraint<Real, 1> {
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
+                out[len] = builder.limit_linear(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+            // TODO: coupled angular limit constraint.
+        }
+
+        if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
+            // TODO: coupled linear limit constraint.
+            out[len] = builder.limit_linear_coupled(
+                params,
+                [joint_id],
+                body1,
+                body2,
+                limit_axes & coupled_axes,
+                &joint.limits,
+                WritebackId::Limit(0), // TODO: writeback
+            );
+            len += 1;
+        }
+        JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
 
-        JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]);
         len
     }
 
@@ -349,6 +399,8 @@ pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
     pub ang_jac2: AngVector<N>,
 
     pub inv_lhs: N,
+    pub cfm_coeff: N,
+    pub cfm_gain: N,
     pub rhs: N,
     pub rhs_wo_bias: N,
 
@@ -367,6 +419,8 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
             lin_jac: Vector::zeros(),
             ang_jac2: na::zero(),
             inv_lhs: N::zero(),
+            cfm_coeff: N::zero(),
+            cfm_gain: N::zero(),
             rhs: N::zero(),
             rhs_wo_bias: N::zero(),
             im2: na::zero(),
@@ -379,7 +433,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
         let dangvel = mj_lambda2.angular;
 
         let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
-        let total_impulse = (self.impulse + self.inv_lhs * dvel)
+        let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse))
             .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
         let delta_impulse = total_impulse - self.impulse;
         self.impulse = total_impulse;
@@ -404,13 +458,14 @@ impl JointVelocityGroundConstraint<Real, 1> {
         body2: &SolverBody<Real, 1>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         out: &mut [Self],
     ) -> usize {
         let mut len = 0;
-        let locked_axes = joint.locked_axes.bits() as u8;
-        let motor_axes = joint.motor_axes.bits() as u8;
-        let limit_axes = joint.limit_axes.bits() as u8;
+        let locked_axes = joint.locked_axes.bits();
+        let motor_axes = joint.motor_axes.bits() & !locked_axes;
+        let limit_axes = joint.limit_axes.bits() & !locked_axes;
+        let coupled_axes = joint.coupled_axes.bits();
 
         let builder = JointVelocityConstraintBuilder::new(
             frame1,
@@ -420,19 +475,68 @@ impl JointVelocityGroundConstraint<Real, 1> {
             locked_axes,
         );
 
+        let start = len;
+        for i in DIM..SPATIAL_DIM {
+            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_ground(
+                    [joint_id],
+                    body1,
+                    body2,
+                    i - DIM,
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
+                );
+                len += 1;
+            }
+        }
         for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_ground(
+            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
+                let limits = if limit_axes & (1 << i) != 0 {
+                    Some([joint.limits[i].min, joint.limits[i].max])
+                } else {
+                    None
+                };
+
+                out[len] = builder.motor_linear_ground(
                     params,
                     [joint_id],
                     body1,
                     body2,
                     i,
-                    WritebackId::Dof(i),
+                    &joint.motors[i].motor_params(params.dt),
+                    limits,
+                    WritebackId::Motor(i),
                 );
                 len += 1;
             }
         }
+
+        if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+            // TODO: coupled angular motor constraint.
+        }
+
+        if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
+            /*
+            // TODO: coupled linear motor constraint.
+            out[len] = builder.motor_linear_coupled_ground(
+                params,
+                [joint_id],
+                body1,
+                body2,
+                motor_axes & coupled_axes,
+                &joint.motors,
+                limit_axes & coupled_axes,
+                &joint.limits,
+                WritebackId::Limit(0), // TODO: writeback
+            );
+            len += 1;
+            */
+            todo!()
+        }
+
+        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
+
+        let start = len;
         for i in DIM..SPATIAL_DIM {
             if locked_axes & (1 << i) != 0 {
                 out[len] = builder.lock_angular_ground(
@@ -446,50 +550,22 @@ impl JointVelocityGroundConstraint<Real, 1> {
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_linear_ground(
-                    [joint_id],
-                    body1,
-                    body2,
-                    i,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
-                );
-                len += 1;
-            }
-        }
-        for i in DIM..SPATIAL_DIM {
-            if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_angular_ground(
-                    [joint_id],
-                    body1,
-                    body2,
-                    i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
-                );
-                len += 1;
-            }
-        }
-
-        for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear_ground(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_ground(
                     params,
                     [joint_id],
                     body1,
                     body2,
                     i,
-                    [joint.limits[i].min, joint.limits[i].max],
-                    WritebackId::Limit(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
         }
+
         for i in DIM..SPATIAL_DIM {
-            if limit_axes & (1 << i) != 0 {
+            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
                 out[len] = builder.limit_angular_ground(
                     params,
                     [joint_id],
@@ -502,8 +578,39 @@ impl JointVelocityGroundConstraint<Real, 1> {
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
+                out[len] = builder.limit_linear_ground(
+                    params,
+                    [joint_id],
+                    body1,
+                    body2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
+
+        if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+            // TODO: coupled angular limit constraint.
+        }
+
+        if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
+            out[len] = builder.limit_linear_coupled_ground(
+                params,
+                [joint_id],
+                body1,
+                body2,
+                limit_axes & coupled_axes,
+                &joint.limits,
+                WritebackId::Limit(0), // TODO: writeback
+            );
+            len += 1;
+        }
+        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]);
 
-        JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]);
         len
     }
 

src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs

@@ -3,8 +3,8 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
 };
 use crate::dynamics::solver::joint_constraint::SolverBody;
 use crate::dynamics::solver::MotorParameters;
-use crate::dynamics::{IntegrationParameters, JointIndex};
-use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
+use crate::dynamics::{IntegrationParameters, JointAxesMask, JointIndex, JointLimits, JointMotor};
+use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
 use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
 use na::SMatrix;
 
@@ -92,10 +92,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let min_enabled = dist.simd_lt(limits[0]);
         let max_enabled = limits[1].simd_lt(dist);
 
-        let erp_inv_dt = N::splat(params.erp_inv_dt());
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         let rhs_bias =
             ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
         constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
+        constraint.cfm_coeff = cfm_coeff;
         constraint.impulse_bounds = [
             N::splat(-Real::INFINITY).select(min_enabled, zero),
             N::splat(Real::INFINITY).select(max_enabled, zero),
@@ -104,39 +106,114 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         constraint
     }
 
+    pub fn limit_linear_coupled<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_coupled_axes: u8,
+        limits: &[JointLimits<N>],
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        let zero = N::zero();
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+        let mut limit = N::zero();
+
+        for i in 0..DIM {
+            if limited_coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+                limit += limits[i].max * limits[i].max;
+            }
+        }
+
+        limit = limit.simd_sqrt();
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
+
+        ang_jac1 = body1.sqrt_ii * ang_jac1;
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
+        let rhs = rhs_wo_bias + rhs_bias;
+        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
+
+        JointVelocityConstraint {
+            joint_id,
+            mj_lambda1: body1.mj_lambda,
+            mj_lambda2: body2.mj_lambda,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
+            rhs,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
     pub fn motor_linear<const LANES: usize>(
         &self,
         params: &IntegrationParameters,
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
-        _locked_ang_axes: u8,
         motor_axis: usize,
         motor_params: &MotorParameters<N>,
+        limits: Option<[N; 2]>,
         writeback_id: WritebackId,
     ) -> JointVelocityConstraint<N, LANES> {
+        let inv_dt = N::splat(params.inv_dt());
         let mut constraint =
             self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id);
 
-        // if locked_ang_axes & (1 << motor_axis) != 0 {
-        //     // FIXME: check that this also works for cases
-        //     // when not all the angular axes are locked.
-        //     constraint.ang_jac1 = na::zero();
-        //     constraint.ang_jac2 = na::zero();
-        // }
-
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             let dist = self.lin_err.dot(&constraint.lin_jac);
-            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
-                + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
-            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-        }
+        let mut target_vel = motor_params.target_vel;
+        if let Some(limits) = limits {
+            let dist = self.lin_err.dot(&constraint.lin_jac);
+            target_vel =
+                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
+        };
 
+        let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
+        rhs_wo_bias += dvel - target_vel;
+
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
         constraint.rhs_wo_bias = rhs_wo_bias;
@@ -164,10 +241,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
             + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
-        let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
 
         ang_jac1 = body1.sqrt_ii * ang_jac1;
         ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -184,6 +262,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -220,12 +300,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(limited_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
             - (s_limits[0] - s_ang).simd_max(zero))
-            * N::splat(erp_inv_dt);
+            * erp_inv_dt;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -242,6 +323,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -264,20 +347,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let ang_jac = self.basis.column(_motor_axis).into_owned();
 
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             #[cfg(feature = "dim2")]
             let s_ang_dist = self.ang_err.im;
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
             let s_target_ang = motor_params.target_pos.simd_sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs_wo_bias +=
-                (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -294,6 +374,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
             writeback_id,
@@ -315,13 +397,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let ang_jac = self.ang_basis.column(locked_axis).into_owned();
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         #[cfg(feature = "dim2")]
-        let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -338,6 +421,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -349,6 +434,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         constraints: &mut [JointVelocityConstraint<N, LANES>],
     ) {
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let imsum = constraints[0].im1 + constraints[0].im2;
 
         // Use the modified Gram-Schmidt orthogonalization.
@@ -357,8 +447,10 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                 + c_j.ang_jac1.gdot(c_j.ang_jac1)
                 + c_j.ang_jac2.gdot(c_j.ang_jac2);
+            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
             let inv_dot_jj = crate::utils::simd_inv(dot_jj);
-            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
+            c_j.cfm_gain = cfm_gain;
 
             if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
                 // Don't remove constraints with limited forces from the others
@@ -369,6 +461,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
             for i in (j + 1)..len {
                 let (c_i, c_j) = constraints.index_mut_const(i, j);
+
                 let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                     + c_i.ang_jac1.gdot(c_j.ang_jac1)
                     + c_i.ang_jac2.gdot(c_j.ang_jac2);
@@ -396,6 +489,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let zero = N::zero();
         let lin_jac = self.basis.column(limited_axis).into_owned();
         let dist = self.lin_err.dot(&lin_jac);
+
         let min_enabled = dist.simd_lt(limits[0]);
         let max_enabled = limits[1].simd_lt(dist);
 
@@ -412,11 +506,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
             + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
-        let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero))
-            * N::splat(erp_inv_dt);
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias =
+            ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
 
         ang_jac2 = body2.sqrt_ii * ang_jac2;
 
@@ -429,21 +524,97 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac,
             ang_jac2,
             inv_lhs: zero, // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
         }
     }
 
+    pub fn limit_linear_coupled_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_coupled_axes: u8,
+        limits: &[JointLimits<N>],
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        let zero = N::zero();
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+        let mut limit = N::zero();
+
+        for i in 0..DIM {
+            if limited_coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+                limit += limits[i].max * limits[i].max;
+            }
+        }
+
+        limit = limit.simd_sqrt();
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
+        let rhs = rhs_wo_bias + rhs_bias;
+        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
+            rhs,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
     pub fn motor_linear_ground<const LANES: usize>(
         &self,
+        params: &IntegrationParameters,
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
         motor_axis: usize,
         motor_params: &MotorParameters<N>,
+        limits: Option<[N; 2]>,
         writeback_id: WritebackId,
     ) -> JointVelocityGroundConstraint<N, LANES> {
+        let inv_dt = N::splat(params.inv_dt());
+
         let lin_jac = self.basis.column(motor_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
         #[cfg(feature = "dim2")]
@@ -452,16 +623,21 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
 
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             let dist = self.lin_err.dot(&lin_jac);
-            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
-                + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-        }
+        let mut target_vel = motor_params.target_vel;
+        if let Some(limits) = limits {
+            let dist = self.lin_err.dot(&lin_jac);
+            target_vel =
+                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
+        };
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        rhs_wo_bias += dvel - target_vel;
 
         ang_jac2 = body2.sqrt_ii * ang_jac2;
 
@@ -474,12 +650,89 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
             writeback_id,
         }
     }
 
+    pub fn motor_linear_coupled_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        motor_coupled_axes: u8,
+        motors: &[MotorParameters<N>],
+        limited_coupled_axes: u8,
+        limits: &[JointLimits<N>],
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        todo!()
+        /*
+        let zero = N::zero();
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+        let mut limit = N::zero();
+
+        for i in 0..DIM {
+            if limited_coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+                limit += limits[i].max * limits[i].max;
+            }
+        }
+
+        limit = limit.simd_sqrt();
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
+        let rhs = rhs_wo_bias + rhs_bias;
+        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
+            rhs,
+            rhs_wo_bias,
+            writeback_id,
+        }
+        */
+    }
+
     pub fn lock_linear_ground<const LANES: usize>(
         &self,
         params: &IntegrationParameters,
@@ -498,10 +751,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
             + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
-        let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
 
         ang_jac2 = body2.sqrt_ii * ang_jac2;
 
@@ -514,6 +768,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -536,20 +792,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let ang_jac = self.basis.column(_motor_axis).into_owned();
 
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             #[cfg(feature = "dim2")]
             let s_ang_dist = self.ang_err.im;
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
             let s_target_ang = motor_params.target_pos.simd_sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs_wo_bias +=
-                (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 
@@ -562,6 +815,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac: na::zero(),
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
             writeback_id,
@@ -598,12 +853,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(limited_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
             - (s_limits[0] - s_ang).simd_max(zero))
-            * N::splat(erp_inv_dt);
+            * erp_inv_dt;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 
@@ -616,6 +872,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac: na::zero(),
             ang_jac2,
             inv_lhs: zero, // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -636,13 +894,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(locked_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         #[cfg(feature = "dim2")]
-        let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 
@@ -655,6 +914,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac: na::zero(),
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -666,6 +927,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
     ) {
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let imsum = constraints[0].im2;
 
         // Use the modified Gram-Schmidt orthogonalization.
@@ -673,18 +939,23 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             let c_j = &mut constraints[j];
             let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                 + c_j.ang_jac2.gdot(c_j.ang_jac2);
-            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
+            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
+            let inv_dot_jj = crate::utils::simd_inv(dot_jj + cfm_gain);
             c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            c_j.cfm_gain = cfm_gain;
 
-            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
+            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)]
+                || c_j.cfm_gain != N::zero()
+            {
                 // Don't remove constraints with limited forces from the others
                 // because they may not deliver the necessary forces to fulfill
                 // the removed parts of other constraints.
                 continue;
             }
 
-            for i in (j + 1)..len {
+            for i in j + 1..len {
                 let (c_i, c_j) = constraints.index_mut_const(i, j);
+
                 let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                     + c_i.ang_jac2.gdot(c_j.ang_jac2);
                 let coeff = dot_ij * inv_dot_jj;

src/dynamics/solver/velocity_constraint.rs

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
-use crate::utils::{WAngularInertia, WBasis, WCross, WDot, WReal};
+use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot, WReal};
 
 use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
 
@@ -239,10 +239,11 @@ impl VelocityConstraint {
                         .transform_vector(dp2.gcross(-force_dir1));
 
                     let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
-                    let projected_mass = 1.0
-                        / (force_dir1.dot(&imsum.component_mul(&force_dir1))
+                    let projected_mass = utils::inv(
+                        force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
-                            + gcross2.gdot(gcross2));
+                            + gcross2.gdot(gcross2),
+                    );
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
@@ -250,7 +251,7 @@ impl VelocityConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias = /* is_resting
                         * */  erp_inv_dt
                         * (manifold_point.dist + params.allowed_linear_error).min(0.0);
@@ -285,8 +286,11 @@ impl VelocityConstraint {
                         constraint.elements[k].tangent_part.gcross1[j] = gcross1;
                         constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                         constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        constraint.elements[k].tangent_part.r[j] =
-                            if cfg!(feature = "dim2") { 1.0 / r } else { r };
+                        constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
+                            utils::inv(r)
+                        } else {
+                            r
+                        };
                     }
 
                     #[cfg(feature = "dim3")]

src/dynamics/solver/velocity_constraint_wide.rs

@@ -9,7 +9,7 @@ use crate::math::{
 };
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
-use crate::utils::{WAngularInertia, WCross, WDot};
+use crate::utils::{self, WAngularInertia, WCross, WDot};
 use num::Zero;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -47,7 +47,6 @@ impl WVelocityConstraint {
         }
 
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
 
@@ -135,16 +134,17 @@ impl WVelocityConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
                     let imsum = im1 + im2;
-                    let projected_mass = SimdReal::splat(1.0)
-                        / (force_dir1.dot(&imsum.component_mul(&force_dir1))
+                    let projected_mass = utils::simd_inv(
+                        force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
-                            + gcross2.gdot(gcross2));
+                            + gcross2.gdot(gcross2),
+                    );
 
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
                     rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
 
@@ -174,7 +174,7 @@ impl WVelocityConstraint {
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                     constraint.elements[k].tangent_part.rhs[j] = rhs;
                     constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
-                        SimdReal::splat(1.0) / r
+                        utils::simd_inv(r)
                     } else {
                         r
                     };

src/dynamics/solver/velocity_ground_constraint.rs

@@ -5,7 +5,7 @@ use super::{
 use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
-use crate::utils::{WAngularInertia, WCross, WDot};
+use crate::utils::{self, WAngularInertia, WCross, WDot};
 
 use crate::data::{BundleSet, ComponentSet};
 use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
@@ -153,9 +153,10 @@ impl VelocityGroundConstraint {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let projected_mass = 1.0
-                        / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
-                            + gcross2.gdot(gcross2));
+                    let projected_mass = utils::inv(
+                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                            + gcross2.gdot(gcross2),
+                    );
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
@@ -163,7 +164,7 @@ impl VelocityGroundConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias = /* is_resting
                         * */ erp_inv_dt
                         * (manifold_point.dist + params.allowed_linear_error).min(0.0);
@@ -194,8 +195,11 @@ impl VelocityGroundConstraint {
 
                         constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                         constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        constraint.elements[k].tangent_part.r[j] =
-                            if cfg!(feature = "dim2") { 1.0 / r } else { r };
+                        constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
+                            utils::inv(r)
+                        } else {
+                            r
+                        };
                     }
 
                     #[cfg(feature = "dim3")]

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -10,7 +10,7 @@ use crate::math::{
 };
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
-use crate::utils::{WAngularInertia, WCross, WDot};
+use crate::utils::{self, WAngularInertia, WCross, WDot};
 use num::Zero;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -42,7 +42,6 @@ impl WVelocityGroundConstraint {
             + ComponentSet<RigidBodyMassProps>,
     {
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
 
@@ -142,14 +141,15 @@ impl WVelocityGroundConstraint {
                 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
-                    let projected_mass = SimdReal::splat(1.0)
-                        / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
+                    let projected_mass = utils::simd_inv(
+                        force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2),
+                    );
 
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
                     rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
 
@@ -174,7 +174,7 @@ impl WVelocityGroundConstraint {
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                     constraint.elements[k].tangent_part.rhs[j] = rhs;
                     constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
-                        SimdReal::splat(1.0) / r
+                        utils::simd_inv(r)
                     } else {
                         r
                     };

src/geometry/collider.rs

@@ -802,3 +802,9 @@ impl ColliderBuilder {
         )
     }
 }
+
+impl Into<Collider> for ColliderBuilder {
+    fn into(self) -> Collider {
+        self.build()
+    }
+}

src/geometry/collider_set.rs

@@ -129,7 +129,8 @@ impl ColliderSet {
     }
 
     /// Inserts a new collider to this set and retrieve its handle.
-    pub fn insert(&mut self, mut coll: Collider) -> ColliderHandle {
+    pub fn insert(&mut self, coll: impl Into<Collider>) -> ColliderHandle {
+        let mut coll = coll.into();
         // Make sure the internal links are reset, they may not be
         // if this rigid-body was obtained by cloning another one.
         coll.reset_internal_references();
@@ -142,10 +143,11 @@ impl ColliderSet {
     /// Inserts a new collider to this set, attach it to the given rigid-body, and retrieve its handle.
     pub fn insert_with_parent(
         &mut self,
-        mut coll: Collider,
+        coll: impl Into<Collider>,
         parent_handle: RigidBodyHandle,
         bodies: &mut RigidBodySet,
     ) -> ColliderHandle {
+        let mut coll = coll.into();
         // Make sure the internal links are reset, they may not be
         // if this collider was obtained by cloning another one.
         coll.reset_internal_references();