Merge pull request #579 from dimforge/joints-improvements

Feat: implement a "small-steps" velocity-based constraints solver + joint improvements

1 files changed, 53 insertions, 40 deletions

diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index edd17f8..2953f98 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,16 +1,15 @@
-use super::VelocitySolver;
+use super::{JointConstraintsSet, VelocitySolver};
 use crate::counters::Counters;
-use crate::dynamics::solver::{
-    AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
-};
+use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
 use crate::dynamics::IslandManager;
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::prelude::MultibodyJointSet;
+use parry::math::Real;
 
 pub struct IslandSolver {
-    contact_constraints: SolverConstraints<AnyVelocityConstraint>,
-    joint_constraints: SolverConstraints<AnyJointVelocityConstraint>,
+    contact_constraints: ContactConstraintsSet,
+    joint_constraints: JointConstraintsSet,
     velocity_solver: VelocitySolver,
 }
 
@@ -23,8 +22,8 @@ impl Default for IslandSolver {
 impl IslandSolver {
     pub fn new() -> Self {
         Self {
-            contact_constraints: SolverConstraints::new(),
-            joint_constraints: SolverConstraints::new(),
+            contact_constraints: ContactConstraintsSet::new(),
+            joint_constraints: JointConstraintsSet::new(),
             velocity_solver: VelocitySolver::new(),
         }
     }
@@ -33,57 +32,71 @@ impl IslandSolver {
         &mut self,
         island_id: usize,
         counters: &mut Counters,
-        params: &IntegrationParameters,
+        base_params: &IntegrationParameters,
         islands: &IslandManager,
         bodies: &mut RigidBodySet,
         manifolds: &mut [&mut ContactManifold],
         manifold_indices: &[ContactManifoldIndex],
         impulse_joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
-        multibody_joints: &mut MultibodyJointSet,
+        multibodies: &mut MultibodyJointSet,
     ) {
-        // 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_resolution_time.resume();
+        let num_solver_iterations = base_params.num_solver_iterations.get()
+            + islands.active_island_additional_solver_iterations(island_id);
 
-        counters.solver.velocity_assembly_time.resume();
-        self.contact_constraints.init(
+        let mut params = *base_params;
+        params.dt /= num_solver_iterations as Real;
+        params.damping_ratio /= num_solver_iterations as Real;
+        // params.joint_damping_ratio /= num_solver_iterations as Real;
+
+        /*
+         *
+         * Bellow this point, the `params` is using the "small step" settings.
+         *
+         */
+        // INIT
+        self.velocity_solver
+            .init_solver_velocities_and_solver_bodies(
+                &params,
+                island_id,
+                islands,
+                bodies,
+                multibodies,
+            );
+        self.velocity_solver.init_constraints(
             island_id,
-            params,
             islands,
             bodies,
-            multibody_joints,
+            multibodies,
             manifolds,
             manifold_indices,
-        );
-        self.joint_constraints.init(
-            island_id,
-            params,
-            islands,
-            bodies,
-            multibody_joints,
             impulse_joints,
             joint_indices,
+            &mut self.contact_constraints,
+            &mut self.joint_constraints,
         );
-        counters.solver.velocity_assembly_time.pause();
 
-        counters.solver.velocity_resolution_time.resume();
-        self.velocity_solver.solve(
-            island_id,
-            params,
+        // SOLVE
+        self.velocity_solver.solve_constraints(
+            &params,
+            num_solver_iterations,
+            bodies,
+            multibodies,
+            &mut self.contact_constraints,
+            &mut self.joint_constraints,
+        );
+
+        // WRITEBACK
+        self.joint_constraints.writeback_impulses(impulse_joints);
+        self.contact_constraints.writeback_impulses(manifolds);
+        self.velocity_solver.writeback_bodies(
+            base_params,
+            num_solver_iterations,
             islands,
+            island_id,
             bodies,
-            multibody_joints,
-            manifolds,
-            impulse_joints,
-            &mut self.contact_constraints.velocity_constraints,
-            &self.contact_constraints.generic_jacobians,
-            &mut self.joint_constraints.velocity_constraints,
-            &self.joint_constraints.generic_jacobians,
+            multibodies,
         );
         counters.solver.velocity_resolution_time.pause();
     }