feat: add warmstarting to contact constraints resolution

2024-04-21 18:55:11 +02:00
parent da79d6fb5b
commit f58b4f7c19
32 changed files with 1540 additions and 342 deletions
--- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
+++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
@@ -442,6 +442,17 @@ impl ContactConstraintsSet {
        assert_eq!(curr_start, total_num_constraints);
    }

+    pub fn warmstart(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let (jac, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.warmstart(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
    pub fn solve_restitution(
        &mut self,
        solver_vels: &mut [SolverVel<Real>],