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

1 files changed, 41 insertions, 43 deletions

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)