1 files changed, 47 insertions, 2 deletions
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
index 4fa8694..be839f0 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -202,7 +202,7 @@ impl GenericTwoBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse_accumulator: na::zero(),
-                        impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         r,
                         r_mat_elts: [0.0; 2],
                     };
@@ -210,7 +210,8 @@ impl GenericTwoBodyConstraintBuilder {
 
                 // Tangent parts.
                 {
-                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -374,6 +375,50 @@ impl GenericTwoBodyConstraint {
         }
     }
 
+    pub fn warmstart(
+        &mut self,
+        jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel1)
+        };
+
+        let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel2)
+        };
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        TwoBodyConstraintElement::generic_warmstart_group(
+            elements,
+            jacobians,
+            &self.inner.dir1,
+            #[cfg(feature = "dim3")]
+            &self.inner.tangent1,
+            &self.inner.im1,
+            &self.inner.im2,
+            self.ndofs1,
+            self.ndofs2,
+            self.j_id,
+            &mut solver_vel1,
+            &mut solver_vel2,
+            generic_solver_vels,
+        );
+
+        if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
+            solver_vels[self.inner.solver_vel1] = solver_vel1;
+        }
+
+        if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
+            solver_vels[self.inner.solver_vel2] = solver_vel2;
+        }
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,