Minor island solver simplification

2 files changed, 44 insertions, 104 deletions

diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 9f82182..c31cee1 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -11,7 +11,7 @@ use crate::dynamics::{
 };
 use crate::dynamics::{IslandManager, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::prelude::{MultibodyJointSet, RigidBodyActivation};
+use crate::prelude::MultibodyJointSet;
 
 pub struct IslandSolver {
     contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
@@ -51,113 +51,54 @@ impl IslandSolver {
             + ComponentSetMut<RigidBodyPosition>
             + ComponentSetMut<RigidBodyVelocity>
             + ComponentSetMut<RigidBodyMassProps>
-            + ComponentSetMut<RigidBodyActivation>
             + ComponentSet<RigidBodyDamping>
             + ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyType>,
     {
-        let mut has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
-        if !has_constraints {
-            // Check if the multibody_joints have internal constraints.
-            for handle in islands.active_island(island_id) {
-                if let Some(link) = multibody_joints.rigid_body_link(*handle) {
-                    let multibody = multibody_joints.get_multibody(link.multibody).unwrap();
-
-                    if (link.id == 0 || link.id == 1 && !multibody.root_is_dynamic)
-                        && multibody.has_active_internal_constraints()
-                    {
-                        has_constraints = true;
-                        break;
-                    }
-                }
-            }
+        // Init the solver id for multibody_joints.
+        // We need that for building the constraints.
+        let mut solver_id = 0;
+        for (_, multibody) in multibody_joints.multibodies.iter_mut() {
+            multibody.solver_id = solver_id;
+            solver_id += multibody.ndofs();
         }
 
-        if has_constraints {
-            // Init the solver id for multibody_joints.
-            // We need that for building the constraints.
-            let mut solver_id = 0;
-            for (_, multibody) in multibody_joints.multibodies.iter_mut() {
-                multibody.solver_id = solver_id;
-                solver_id += multibody.ndofs();
-            }
-
-            counters.solver.velocity_assembly_time.resume();
-            self.contact_constraints.init(
-                island_id,
-                params,
-                islands,
-                bodies,
-                multibody_joints,
-                manifolds,
-                manifold_indices,
-            );
-            self.joint_constraints.init(
-                island_id,
-                params,
-                islands,
-                bodies,
-                multibody_joints,
-                impulse_joints,
-                joint_indices,
-            );
-            counters.solver.velocity_assembly_time.pause();
-
-            counters.solver.velocity_resolution_time.resume();
-            self.velocity_solver.solve(
-                island_id,
-                params,
-                islands,
-                bodies,
-                multibody_joints,
-                manifolds,
-                impulse_joints,
-                &mut self.contact_constraints.velocity_constraints,
-                &mut self.contact_constraints.generic_velocity_constraints,
-                &self.contact_constraints.generic_jacobians,
-                &mut self.joint_constraints.velocity_constraints,
-                &self.joint_constraints.generic_jacobians,
-            );
-            counters.solver.velocity_resolution_time.pause();
-        } else {
-            self.contact_constraints.clear();
-            self.joint_constraints.clear();
-            counters.solver.velocity_update_time.resume();
-
-            for handle in islands.active_island(island_id) {
-                if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() {
-                    let multibody = multibody_joints
-                        .get_multibody_mut_internal(link.multibody)
-                        .unwrap();
+        counters.solver.velocity_assembly_time.resume();
+        self.contact_constraints.init(
+            island_id,
+            params,
+            islands,
+            bodies,
+            multibody_joints,
+            manifolds,
+            manifold_indices,
+        );
+        self.joint_constraints.init(
+            island_id,
+            params,
+            islands,
+            bodies,
+            multibody_joints,
+            impulse_joints,
+            joint_indices,
+        );
+        counters.solver.velocity_assembly_time.pause();
 
-                    if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                        let accels = &multibody.accelerations;
-                        multibody.velocities.axpy(params.dt, accels, 1.0);
-                        multibody.integrate(params.dt);
-                        multibody.forward_kinematics(bodies, false);
-                    }
-                } else {
-                    // Since we didn't run the velocity solver we need to integrate the accelerations here
-                    let (poss, vels, forces, damping, mprops): (
-                        &RigidBodyPosition,
-                        &RigidBodyVelocity,
-                        &RigidBodyForces,
-                        &RigidBodyDamping,
-                        &RigidBodyMassProps,
-                    ) = bodies.index_bundle(handle.0);
-
-                    let mut new_poss = *poss;
-                    let new_vels = forces
-                        .integrate(params.dt, vels, mprops)
-                        .apply_damping(params.dt, &damping);
-                    new_poss.next_position =
-                        vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
-
-                    bodies.set_internal(handle.0, new_vels);
-                    bodies.set_internal(handle.0, new_poss);
-                }
-            }
-            counters.solver.velocity_update_time.pause();
-        }
+        counters.solver.velocity_resolution_time.resume();
+        self.velocity_solver.solve(
+            island_id,
+            params,
+            islands,
+            bodies,
+            multibody_joints,
+            manifolds,
+            impulse_joints,
+            &mut self.contact_constraints.velocity_constraints,
+            &mut self.contact_constraints.generic_velocity_constraints,
+            &self.contact_constraints.generic_jacobians,
+            &mut self.joint_constraints.velocity_constraints,
+            &self.joint_constraints.generic_jacobians,
+        );
+        counters.solver.velocity_resolution_time.pause();
     }
 }
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index c5c6ede..3d4377f 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -9,7 +9,7 @@ use crate::dynamics::{
 use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
 use crate::geometry::ContactManifold;
 use crate::math::Real;
-use crate::prelude::{RigidBodyActivation, RigidBodyDamping, RigidBodyPosition};
+use crate::prelude::{RigidBodyDamping, RigidBodyPosition};
 use crate::utils::WAngularInertia;
 use na::DVector;
 
@@ -47,7 +47,6 @@ impl VelocitySolver {
             + ComponentSetMut<RigidBodyVelocity>
             + ComponentSetMut<RigidBodyMassProps>
             + ComponentSetMut<RigidBodyPosition>
-            + ComponentSetMut<RigidBodyActivation>
             + ComponentSet<RigidBodyDamping>,
     {
         let cfm_factor = params.cfm_factor();