Fix warnings and add comments.

4 files changed, 106 insertions, 99 deletions

diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 7f49ec3..451f930 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -1,14 +1,17 @@
 use crate::data::ComponentSet;
-#[cfg(feature = "parallel")]
-use crate::dynamics::RigidBodyHandle;
-use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyIds};
+use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
+
 #[cfg(feature = "simd-is-enabled")]
 use {
     crate::data::BundleSet,
     crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
     vec_map::VecMap,
 };
+
+#[cfg(feature = "parallel")]
+use crate::dynamics::{MultibodyJointSet, RigidBodyHandle};
+
 #[cfg(feature = "parallel")]
 pub(crate) trait PairInteraction {
     fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>);
@@ -195,16 +198,16 @@ impl InteractionGroups {
         }
     }
 
-    #[cfg(not(feature = "parallel"))]
-    pub fn clear(&mut self) {
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            self.buckets.clear();
-            self.body_masks.clear();
-            self.grouped_interactions.clear();
-        }
-        self.nongrouped_interactions.clear();
-    }
+    // #[cfg(not(feature = "parallel"))]
+    // pub fn clear(&mut self) {
+    //     #[cfg(feature = "simd-is-enabled")]
+    //     {
+    //         self.buckets.clear();
+    //         self.body_masks.clear();
+    //         self.grouped_interactions.clear();
+    //     }
+    //     self.nongrouped_interactions.clear();
+    // }
 
     // TODO: there is a lot of duplicated code with group_manifolds here.
     // But we don't refactor just now because we may end up with distinct
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index f571bc6..e1150d5 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -7,15 +7,19 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
 };
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    ImpulseJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, RigidBodyIds,
+    ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
     RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
 use crate::math::{Real, SPATIAL_DIM};
 use crate::prelude::MultibodyJointSet;
 use na::DVector;
 
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
+
+#[cfg(feature = "parallel")]
+use crate::dynamics::JointAxesMask;
+
 pub enum AnyJointVelocityConstraint {
     JointConstraint(JointVelocityConstraint<Real, 1>),
     JointGroundConstraint(JointVelocityGroundConstraint<Real, 1>),
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
index 1c65eb4..439030e 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
@@ -5,9 +5,12 @@ use crate::dynamics::solver::joint_constraint::SolverBody;
 use crate::dynamics::solver::MotorParameters;
 use crate::dynamics::{IntegrationParameters, JointIndex, JointLimits};
 use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
-use crate::utils::{IndexMut2, WBasis, WCross, WCrossMatrix, WDot, WQuat, WReal};
+use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
 use na::SMatrix;
 
+#[cfg(feature = "dim3")]
+use crate::utils::WBasis;
+
 #[derive(Debug, Copy, Clone)]
 pub struct JointVelocityConstraintBuilder<N: WReal> {
     pub basis: Matrix<N>,
@@ -660,79 +663,76 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         }
     }
 
-    pub fn motor_linear_coupled_ground<const LANES: usize>(
-        &self,
-        _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 motor_linear_coupled_ground<const LANES: usize>(
+    //     &self,
+    //     _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> {
+    //     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,
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index c03789a..081ea9c 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -43,15 +43,15 @@ impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
         }
     }
 
-    pub fn clear(&mut self) {
-        self.not_ground_interactions.clear();
-        self.ground_interactions.clear();
-        self.generic_not_ground_interactions.clear();
-        self.generic_ground_interactions.clear();
-        self.interaction_groups.clear();
-        self.ground_interaction_groups.clear();
-        self.velocity_constraints.clear();
-    }
+    // pub fn clear(&mut self) {
+    //     self.not_ground_interactions.clear();
+    //     self.ground_interactions.clear();
+    //     self.generic_not_ground_interactions.clear();
+    //     self.generic_ground_interactions.clear();
+    //     self.interaction_groups.clear();
+    //     self.ground_interaction_groups.clear();
+    //     self.velocity_constraints.clear();
+    // }
 }
 
 impl SolverConstraints<AnyVelocityConstraint> {