Complete the implementation of non-simd joint motor for the revolute joint.

6 files changed, 258 insertions, 309 deletions

diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
index 0208dda..f391873 100644
--- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
@@ -32,7 +32,8 @@ pub(crate) struct GenericVelocityConstraint {
 
     r1: Vector<Real>,
     r2: Vector<Real>,
-    basis: Rotation<Real>,
+    basis1: Rotation<Real>,
+    basis2: Rotation<Real>,
     dependant_set_mask: SpacialVector<Real>,
 
     vel: GenericConstraintPart,
@@ -44,22 +45,22 @@ impl GenericVelocityConstraint {
         max_velocity: &SpatialVector<Real>,
         r1: &Vector<Real>,
         r2: &Vector<Real>,
-        basis: &Rotation<Real>,
+        basis1: &Rotation<Real>,
+        basis2: &Rotation<Real>,
         rb1: &RigidBody,
         rb2: &RigidBody,
     ) -> SpatialVector<Real> {
-        let lin_dvel = -rb1.linvel - rb1.angvel.gcross(*r1) + rb2.linvel + rb2.angvel.gcross(*r2);
-        let ang_dvel = -rb1.angvel + rb2.angvel;
-
-        let lin_dvel2 = basis.inverse_transform_vector(&lin_dvel);
-        let ang_dvel2 = basis.inverse_transform_vector(&ang_dvel);
+        let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+            + basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
+        let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+            + basis2.inverse_transform_vector(&rb2.angvel);
 
         let min_linvel = min_velocity.xyz();
         let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned();
         let max_linvel = max_velocity.xyz();
         let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned();
-        let lin_rhs = lin_dvel2 - lin_dvel2.sup(&min_linvel).inf(&max_linvel);
-        let ang_rhs = ang_dvel2 - ang_dvel2.sup(&min_angvel).inf(&max_angvel);
+        let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel);
+        let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel);
 
         #[cfg(feature = "dim2")]
         return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
@@ -120,6 +121,32 @@ impl GenericVelocityConstraint {
         }
     }
 
+    pub fn invert_partial_delassus_matrix(
+        min_impulse: &Vector<Real>,
+        max_impulse: &Vector<Real>,
+        dependant_set_mask: &mut Vector<Real>,
+        mut delassus: na::Matrix3<Real>,
+    ) -> na::Matrix3<Real> {
+        // Adjust the Delassus matrix to take force limits into account.
+        // If a DoF has a force limit, then we need to make its
+        // constraint independent from the others because otherwise
+        // the force clamping will cause errors to propagate in the
+        // other constraints.
+        for i in 0..3 {
+            if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX {
+                let diag = delassus[(i, i)];
+                delassus.column_mut(i).fill(0.0);
+                delassus.row_mut(i).fill(0.0);
+                delassus[(i, i)] = diag;
+                dependant_set_mask[i] = 0.0;
+            } else {
+                dependant_set_mask[i] = 1.0;
+            }
+        }
+
+        delassus.try_inverse().unwrap()
+    }
+
     pub fn compute_position_error(
         joint: &GenericJoint,
         anchor1: &Isometry<Real>,
@@ -169,32 +196,6 @@ impl GenericVelocityConstraint {
         }
     }
 
-    pub fn invert_partial_delassus_matrix(
-        min_impulse: &Vector<Real>,
-        max_impulse: &Vector<Real>,
-        dependant_set_mask: &mut Vector<Real>,
-        mut delassus: na::Matrix3<Real>,
-    ) -> na::Matrix3<Real> {
-        // Adjust the Delassus matrix to take force limits into account.
-        // If a DoF has a force limit, then we need to make its
-        // constraint independent from the others because otherwise
-        // the force clamping will cause errors to propagate in the
-        // other constraints.
-        for i in 0..3 {
-            if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX {
-                let diag = delassus[(i, i)];
-                delassus.column_mut(i).fill(0.0);
-                delassus.row_mut(i).fill(0.0);
-                delassus[(i, i)] = diag;
-                dependant_set_mask[i] = 0.0;
-            } else {
-                dependant_set_mask[i] = 1.0;
-            }
-        }
-
-        delassus.try_inverse().unwrap()
-    }
-
     pub fn from_params(
         params: &IntegrationParameters,
         joint_id: JointIndex,
@@ -204,7 +205,8 @@ impl GenericVelocityConstraint {
     ) -> Self {
         let anchor1 = rb1.position * joint.local_anchor1;
         let anchor2 = rb2.position * joint.local_anchor2;
-        let basis = anchor1.rotation;
+        let basis1 = anchor1.rotation;
+        let basis2 = anchor2.rotation;
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
         let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
@@ -219,7 +221,7 @@ impl GenericVelocityConstraint {
         let mut max_velocity = joint.max_velocity;
         let mut dependant_set_mask = SpacialVector::repeat(1.0);
 
-        let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis)
+        let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1)
             * params.inv_dt()
             * params.joint_erp;
 
@@ -236,19 +238,28 @@ impl GenericVelocityConstraint {
             }
         }
 
-        let rhs =
-            Self::compute_velocity_error(&min_velocity, &max_velocity, &r1, &r2, &basis, rb1, rb2);
+        let rhs = Self::compute_velocity_error(
+            &min_velocity,
+            &max_velocity,
+            &r1,
+            &r2,
+            &basis1,
+            &basis2,
+            rb1,
+            rb2,
+        );
         let rhs_lin = rhs.xyz();
         let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into();
 
         // TODO: we should keep the SdpMatrix3 type.
-        let rotmat = basis.to_rotation_matrix().into_inner();
-        let rmat1 = r1.gcross_matrix() * rotmat;
-        let rmat2 = r2.gcross_matrix() * rotmat;
+        let rotmat1 = basis1.to_rotation_matrix().into_inner();
+        let rotmat2 = basis2.to_rotation_matrix().into_inner();
+        let rmat1 = r1.gcross_matrix() * rotmat1;
+        let rmat2 = r2.gcross_matrix() * rotmat2;
         let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1)
             + ii2.quadform(&rmat2).add_diagonal(im2))
         .into_matrix();
-        let delassus11 = (ii1.quadform(&rotmat) + ii2.quadform(&rotmat)).into_matrix();
+        let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix();
 
         let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
             &min_pos_impulse.xyz(),
@@ -288,7 +299,8 @@ impl GenericVelocityConstraint {
             inv_lhs_ang,
             r1,
             r2,
-            basis,
+            basis1,
+            basis2,
             dependant_set_mask,
             vel: GenericConstraintPart {
                 lin_impulse,
@@ -307,21 +319,20 @@ impl GenericVelocityConstraint {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
-        let lin_impulse = self.basis * self.vel.lin_impulse;
-        #[cfg(feature = "dim2")]
-        let ang_impulse = self.basis * self.vel.impulse[2];
-        #[cfg(feature = "dim3")]
-        let ang_impulse = self.basis * self.vel.ang_impulse;
+        let lin_impulse1 = self.basis1 * self.vel.lin_impulse;
+        let ang_impulse1 = self.basis1 * self.vel.ang_impulse;
+        let lin_impulse2 = self.basis2 * self.vel.lin_impulse;
+        let ang_impulse2 = self.basis2 * self.vel.ang_impulse;
 
-        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.linear += self.im1 * lin_impulse1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -441,6 +452,7 @@ impl GenericVelocityGroundConstraint {
             &r1,
             &r2,
             &basis,
+            &basis,
             rb1,
             rb2,
         );
@@ -585,28 +597,30 @@ impl GenericConstraintPart {
             let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
-            let dvel = parent.basis.inverse_transform_vector(
-                &(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)
-                    + mj_lambda2.linear
-                    + ang_vel2.gcross(parent.r2)),
-            );
+            let dvel = parent
+                .basis1
+                .inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)))
+                + parent
+                    .basis2
+                    .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
 
             let err = dvel + self.rhs_lin;
 
             new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
                 .sup(&self.min_lin_impulse)
                 .inf(&self.max_lin_impulse);
-            let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse);
+            let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse);
+            let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse);
 
-            mj_lambda1.linear += parent.im1 * effective_impulse;
+            mj_lambda1.linear += parent.im1 * effective_impulse1;
             mj_lambda1.angular += parent
                 .ii1_sqrt
-                .transform_vector(parent.r1.gcross(effective_impulse));
+                .transform_vector(parent.r1.gcross(effective_impulse1));
 
-            mj_lambda2.linear -= parent.im2 * effective_impulse;
+            mj_lambda2.linear -= parent.im2 * effective_impulse2;
             mj_lambda2.angular -= parent
                 .ii2_sqrt
-                .transform_vector(parent.r2.gcross(effective_impulse));
+                .transform_vector(parent.r2.gcross(effective_impulse2));
         }
 
         /*
@@ -618,18 +632,18 @@ impl GenericConstraintPart {
             let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
-            let dvel = parent
-                .basis
-                .inverse_transform_vector(&(ang_vel2 - ang_vel1));
+            let dvel = parent.basis2.inverse_transform_vector(&ang_vel2)
+                - parent.basis1.inverse_transform_vector(&ang_vel1);
             let err = dvel + self.rhs_ang;
 
             new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
                 .sup(&self.min_ang_impulse)
                 .inf(&self.max_ang_impulse);
-            let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse);
+            let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse);
+            let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse);
 
-            mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse);
-            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse);
+            mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1);
+            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2);
         }
 
         (new_lin_impulse, new_ang_impulse)
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index 12b8f77..ebc3c4a 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -14,9 +14,9 @@ use super::{
 #[cfg(feature = "dim3")]
 #[cfg(feature = "simd-is-enabled")]
 use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
-use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
-    GenericVelocityConstraint, GenericVelocityGroundConstraint,
-};
+// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
+//     GenericVelocityConstraint, GenericVelocityGroundConstraint,
+// };
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
@@ -38,12 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint {
     WFixedConstraint(WFixedVelocityConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedVelocityGroundConstraint),
-    GenericConstraint(GenericVelocityConstraint),
-    GenericGroundConstraint(GenericVelocityGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WGenericConstraint(WGenericVelocityConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WGenericGroundConstraint(WGenericVelocityGroundConstraint),
+    // GenericConstraint(GenericVelocityConstraint),
+    // GenericGroundConstraint(GenericVelocityGroundConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericConstraint(WGenericVelocityConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericGroundConstraint(WGenericVelocityGroundConstraint),
     PrismaticConstraint(PrismaticVelocityConstraint),
     PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -89,9 +89,9 @@ impl AnyJointVelocityConstraint {
             JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
                 PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
             ),
-            JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
-                GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
-            ),
+            // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
+            //     GenericVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            // ),
             #[cfg(feature = "dim3")]
             JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
                 RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
@@ -167,11 +167,11 @@ impl AnyJointVelocityConstraint {
             JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
                 FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
             ),
-            JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
-                GenericVelocityGroundConstraint::from_params(
-                    params, joint_id, rb1, rb2, p, flipped,
-                ),
-            ),
+            // JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
+            //     GenericVelocityGroundConstraint::from_params(
+            //         params, joint_id, rb1, rb2, p, flipped,
+            //     ),
+            // ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointVelocityConstraint::PrismaticGroundConstraint(
                     PrismaticVelocityGroundConstraint::from_params(
@@ -180,10 +180,8 @@ impl AnyJointVelocityConstraint {
                 )
             }
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
-                RevoluteVelocityGroundConstraint::from_params(
-                    params, joint_id, rb1, rb2, p, flipped,
-                ),
+            JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
+                params, joint_id, rb1, rb2, p, flipped,
             ),
         }
     }
@@ -267,12 +265,12 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
-            AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
@@ -307,12 +305,12 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
-            AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
+            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
@@ -355,16 +353,16 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)
             }
-            AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
-            AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
-                c.writeback_impulses(joints_all)
-            }
+            // AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
+            // AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
+            //     c.writeback_impulses(joints_all)
+            // }
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
+            //     c.writeback_impulses(joints_all)
+            // }
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)
diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
index a13cc27..40f5d5a 100644
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
@@ -1,7 +1,6 @@
 use super::{
     BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
-    FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint,
-    PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+    FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
 };
 #[cfg(feature = "dim3")]
 use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
@@ -33,12 +32,12 @@ pub(crate) enum AnyJointPositionConstraint {
     WFixedJoint(WFixedPositionConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedPositionGroundConstraint),
-    GenericJoint(GenericPositionConstraint),
-    GenericGroundConstraint(GenericPositionGroundConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WGenericJoint(WGenericPositionConstraint),
-    #[cfg(feature = "simd-is-enabled")]
-    WGenericGroundConstraint(WGenericPositionGroundConstraint),
+    // GenericJoint(GenericPositionConstraint),
+    // GenericGroundConstraint(GenericPositionGroundConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericJoint(WGenericPositionConstraint),
+    // #[cfg(feature = "simd-is-enabled")]
+    // WGenericGroundConstraint(WGenericPositionGroundConstraint),
     PrismaticJoint(PrismaticPositionConstraint),
     PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -69,9 +68,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
                 FixedPositionConstraint::from_params(rb1, rb2, p),
             ),
-            JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
-                GenericPositionConstraint::from_params(rb1, rb2, p),
-            ),
+            // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
+            //     GenericPositionConstraint::from_params(rb1, rb2, p),
+            // ),
             JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
                 PrismaticPositionConstraint::from_params(rb1, rb2, p),
             ),
@@ -140,9 +139,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
                 FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
             ),
-            JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
-                GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
-            ),
+            // JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
+            //     GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            // ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointPositionConstraint::PrismaticGroundConstraint(
                     PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -219,12 +218,12 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
-            AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
-            #[cfg(feature = "simd-is-enabled")]
-            AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
+            // AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
+            // AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
+            // #[cfg(feature = "simd-is-enabled")]
+            // AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]
diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs
index b8e833e..9196e69 100644
--- a/src/dynamics/solver/joint_constraint/mod.rs
+++ b/src/dynamics/solver/joint_constraint/mod.rs
@@ -18,20 +18,20 @@ pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocity
 pub(self) use fixed_velocity_constraint_wide::{
     WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
 };
-pub(self) use generic_position_constraint::{
-    GenericPositionConstraint, GenericPositionGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use generic_position_constraint_wide::{
-    WGenericPositionConstraint, WGenericPositionGroundConstraint,
-};
-pub(self) use generic_velocity_constraint::{
-    GenericVelocityConstraint, GenericVelocityGroundConstraint,
-};
-#[cfg(feature = "simd-is-enabled")]
-pub(self) use generic_velocity_constraint_wide::{
-    WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
-};
+// pub(self) use generic_position_constraint::{
+//     GenericPositionConstraint, GenericPositionGroundConstraint,
+// };
+// #[cfg(feature = "simd-is-enabled")]
+// pub(self) use generic_position_constraint_wide::{
+//     WGenericPositionConstraint, WGenericPositionGroundConstraint,
+// };
+// pub(self) use generic_velocity_constraint::{
+//     GenericVelocityConstraint, GenericVelocityGroundConstraint,
+// };
+// #[cfg(feature = "simd-is-enabled")]
+// pub(self) use generic_velocity_constraint_wide::{
+//     WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
+// };
 
 pub(crate) use joint_constraint::AnyJointVelocityConstraint;
 pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
@@ -78,12 +78,12 @@ mod fixed_position_constraint_wide;
 mod fixed_velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod fixed_velocity_constraint_wide;
-mod generic_position_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod generic_position_constraint_wide;
-mod generic_velocity_constraint;
-#[cfg(feature = "simd-is-enabled")]
-mod generic_velocity_constraint_wide;
+// mod generic_position_constraint;
+// #[cfg(feature = "simd-is-enabled")]
+// mod generic_position_constraint_wide;
+// mod generic_velocity_constraint;
+// #[cfg(feature = "simd-is-enabled")]
+// mod generic_velocity_constraint_wide;
 mod joint_constraint;
 mod joint_position_constraint;
 mod prismatic_position_constraint;
diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
index e468508..9075ed7 100644
--- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
@@ -59,72 +59,6 @@ impl RevolutePositionConstraint {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
-        let anchor1 = position1 * self.local_anchor1;
-        let anchor2 = position2 * self.local_anchor2;
-        let axis1 = position1 * self.local_axis1;
-        let axis2 = position2 * self.local_axis2;
-
-        let basis1 = Matrix3x2::from_columns(&[
-            position1 * self.local_basis1[0],
-            position1 * self.local_basis1[1],
-        ]);
-        let basis2 = Matrix3x2::from_columns(&[
-            position2 * self.local_basis2[0],
-            position2 * self.local_basis2[1],
-        ]);
-
-        let basis_filter1 = basis1 * basis1.transpose();
-        let basis_filter2 = basis2 * basis2.transpose();
-        let basis2 = basis_filter2 * basis1;
-
-        let r1 = anchor1 - position1 * self.local_com1;
-        let r2 = anchor2 - position2 * self.local_com2;
-        let r1_mat = basis_filter1 * r1.gcross_matrix();
-        let r2_mat = basis_filter2 * r2.gcross_matrix();
-
-        let mut lhs = Matrix5::zeros();
-        let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2)
-            + self.ii1.quadform(&r1_mat).add_diagonal(self.im1);
-        let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat)) + basis1.tr_mul(&(self.ii1 * r1_mat));
-        let lhs11 = (self.ii1.quadform3x2(&basis1) + self.ii2.quadform3x2(&basis2)).into_matrix();
-
-        // Note that cholesky won't read the upper-right part
-        // of lhs so we don't have to fill it.
-        lhs.fixed_slice_mut::<na::U3, na::U3>(0, 0)
-            .copy_from(&lhs00.into_matrix());
-        lhs.fixed_slice_mut::<na::U2, na::U3>(3, 0)
-            .copy_from(&lhs10);
-        lhs.fixed_slice_mut::<na::U2, na::U2>(3, 3)
-            .copy_from(&lhs11);
-
-        let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse();
-
-        let delta_tra = anchor2 - anchor1;
-        let lin_error = delta_tra * params.joint_erp;
-        let delta_rot =
-            Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
-
-        let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp;
-        let error = na::Vector5::new(
-            lin_error.x,
-            lin_error.y,
-            lin_error.z,
-            ang_error.x,
-            ang_error.y,
-        );
-        let impulse = inv_lhs * error;
-        let lin_impulse = impulse.fixed_rows::<na::U3>(0).into_owned();
-        let ang_impulse1 = basis1 * impulse.fixed_rows::<na::U2>(3).into_owned();
-        let ang_impulse2 = basis2 * impulse.fixed_rows::<na::U2>(3).into_owned();
-
-        let rot1 = self.ii1 * (r1_mat * lin_impulse + ang_impulse1);
-        let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2);
-        position1.rotation = Rotation::new(rot1) * position1.rotation;
-        position2.rotation = Rotation::new(-rot2) * position2.rotation;
-        position1.translation.vector += self.im1 * lin_impulse;
-        position2.translation.vector -= self.im2 * lin_impulse;
-
-        /*
         /*
          * Linear part.
          */
@@ -134,7 +68,8 @@ impl RevolutePositionConstraint {
 
             let r1 = anchor1 - position1 * self.local_com1;
             let r2 = anchor2 - position2 * self.local_com2;
-            // TODO: don't the the "to_matrix".
+
+            // TODO: don't do the "to_matrix".
             let lhs = (self
                 .ii2
                 .quadform(&r2.gcross_matrix())
@@ -174,7 +109,6 @@ impl RevolutePositionConstraint {
             position2.rotation =
                 Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
         }
-         */
 
         positions[self.position1 as usize] = position1;
         positions[self.position2 as usize] = position2;
@@ -249,61 +183,6 @@ impl RevolutePositionGroundConstraint {
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
-        let anchor1 = self.anchor1;
-        let anchor2 = position2 * self.local_anchor2;
-        let axis1 = self.axis1;
-        let axis2 = position2 * self.local_axis2;
-
-        let basis1 = Matrix3x2::from_columns(&self.basis1[..]);
-        let basis2 = Matrix3x2::from_columns(&[
-            position2 * self.local_basis2[0],
-            position2 * self.local_basis2[1],
-        ]);
-
-        let basis_filter2 = basis2 * basis2.transpose();
-        let basis2 = basis_filter2 * basis1;
-
-        let r2 = anchor2 - position2 * self.local_com2;
-        let r2_mat = basis_filter2 * r2.gcross_matrix();
-
-        let mut lhs = Matrix5::zeros();
-        let lhs00 = self.ii2.quadform(&r2_mat).add_diagonal(self.im2);
-        let lhs10 = basis2.tr_mul(&(self.ii2 * r2_mat));
-        let lhs11 = self.ii2.quadform3x2(&basis2).into_matrix();
-
-        // Note that cholesky won't read the upper-right part
-        // of lhs so we don't have to fill it.
-        lhs.fixed_slice_mut::<na::U3, na::U3>(0, 0)
-            .copy_from(&lhs00.into_matrix());
-        lhs.fixed_slice_mut::<na::U2, na::U3>(3, 0)
-            .copy_from(&lhs10);
-        lhs.fixed_slice_mut::<na::U2, na::U2>(3, 3)
-            .copy_from(&lhs11);
-
-        let inv_lhs = na::Cholesky::new_unchecked(lhs).inverse();
-
-        let delta_tra = anchor2 - anchor1;
-        let lin_error = delta_tra * params.joint_erp;
-        let delta_rot =
-            Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
-
-        let ang_error = basis1.tr_mul(&delta_rot.scaled_axis()) * params.joint_erp;
-        let error = na::Vector5::new(
-            lin_error.x,
-            lin_error.y,
-            lin_error.z,
-            ang_error.x,
-            ang_error.y,
-        );
-        let impulse = inv_lhs * error;
-        let lin_impulse = impulse.fixed_rows::<na::U3>(0).into_owned();
-        let ang_impulse2 = basis2 * impulse.fixed_rows::<na::U2>(3).into_owned();
-
-        let rot2 = self.ii2 * (r2_mat * lin_impulse + ang_impulse2);
-        position2.rotation = Rotation::new(-rot2) * position2.rotation;
-        position2.translation.vector -= self.im2 * lin_impulse;
-
-        /*
         /*
          * Linear part.
          */
@@ -338,7 +217,6 @@ impl RevolutePositionGroundConstraint {
             let ang_error = delta_rot.scaled_axis() * params.joint_erp;
             position2.rotation = Rotation::new(-ang_error) * position2.rotation;
         }
-        */
 
         positions[self.position2 as usize] = position2;
     }
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
index 650a9e5..1219c39 100644
--- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
@@ -1,11 +1,11 @@
 use crate::dynamics::solver::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel};
 use crate::dynamics::{
-    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint,
-    RigidBody,
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
 use crate::math::{AngularInertia, Real, Rotation, Vector};
 use crate::na::UnitQuaternion;
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use downcast_rs::Downcast;
 use na::{Cholesky, Matrix3, Matrix3x2, Matrix5, RealField, Vector5, U2, U3};
 
 #[derive(Debug)]
@@ -26,6 +26,7 @@ pub(crate) struct RevoluteVelocityConstraint {
     motor_rhs: Real,
     motor_impulse: Real,
     motor_max_impulse: Real,
+    motor_angle: Real, // Exists only to write it back into the joint.