From f58b4f7c195ab7acf0778ea65c46ebf37ac8188c Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 21 Apr 2024 18:55:11 +0200 Subject: feat: add warmstarting to contact constraints resolution --- .../contact_constraint/contact_constraints_set.rs | 11 ++ .../generic_one_body_constraint.rs | 18 ++ .../generic_one_body_constraint_element.rs | 86 +++++++++ .../generic_two_body_constraint.rs | 49 +++++- .../generic_two_body_constraint_element.rs | 196 +++++++++++++++++++++ .../contact_constraint/one_body_constraint.rs | 38 ++-- .../one_body_constraint_element.rs | 100 ++++------- .../contact_constraint/one_body_constraint_simd.rs | 59 +++++-- .../contact_constraint/two_body_constraint.rs | 60 +++++-- .../two_body_constraint_element.rs | 131 +++++++------- .../contact_constraint/two_body_constraint_simd.rs | 69 ++++++-- src/dynamics/solver/velocity_solver.rs | 56 +----- 12 files changed, 629 insertions(+), 244 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index 4d88949..6809c37 100644 --- 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], + generic_solver_vels: &mut DVector, + ) { + 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], diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs index d99ad60..301ce71 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -259,6 +259,24 @@ impl GenericOneBodyConstraint { } } + pub fn warmstart( + &mut self, + jacobians: &DVector, + generic_solver_vels: &mut DVector, + ) { + let solver_vel2 = self.inner.solver_vel2; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + OneBodyConstraintElement::generic_warmstart_group( + elements, + jacobians, + self.ndofs2, + self.j_id, + solver_vel2, + generic_solver_vels, + ); + } + pub fn solve( &mut self, jacobians: &DVector, diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs index 1003a9a..4928f36 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs @@ -7,6 +7,40 @@ use na::DVector; use na::SimdPartialOrd; impl OneBodyConstraintTangentPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + #[cfg(feature = "dim2")] + { + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse[0], + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } + + #[cfg(feature = "dim3")] + { + let j_step = ndofs2 * 2; + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse[0], + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse[1], + &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), + 1.0, + ); + } + } + #[inline] pub fn generic_solve( &mut self, @@ -71,6 +105,22 @@ impl OneBodyConstraintTangentPart { } impl OneBodyConstraintNormalPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse, + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } + #[inline] pub fn generic_solve( &mut self, @@ -99,6 +149,42 @@ impl OneBodyConstraintNormalPart { } impl OneBodyConstraintElement { + #[inline] + pub fn generic_warmstart_group( + elements: &mut [Self], + jacobians: &DVector, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + let j_step = ndofs2 * 2 * DIM; + + // Solve penetration. + let mut nrm_j_id = j_id; + + for element in elements.iter_mut() { + element.normal_part.generic_warmstart( + nrm_j_id, + jacobians, + ndofs2, + solver_vel2, + solver_vels, + ); + nrm_j_id += j_step; + } + + // Solve friction. + let mut tng_j_id = j_id + ndofs2 * 2; + + for element in elements.iter_mut() { + let part = &mut element.tangent_part; + part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels); + tng_j_id += j_step; + } + } + #[inline] pub fn generic_solve_group( cfm_factor: Real, 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, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + 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, diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs index 40740a0..389b603 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs @@ -88,6 +88,95 @@ impl GenericRhs { } impl TwoBodyConstraintTangentPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id: usize, + jacobians: &DVector, + tangents1: [&Vector; DIM - 1], + im1: &Vector, + im2: &Vector, + ndofs1: usize, + ndofs2: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, + ) { + let j_id1 = j_id1(j_id, ndofs1, ndofs2); + let j_id2 = j_id2(j_id, ndofs1, ndofs2); + #[cfg(feature = "dim3")] + let j_step = j_step(ndofs1, ndofs2); + + #[cfg(feature = "dim2")] + { + solver_vel1.apply_impulse( + j_id1, + ndofs1, + self.impulse[0], + jacobians, + tangents1[0], + &self.gcross1[0], + solver_vels, + im1, + ); + solver_vel2.apply_impulse( + j_id2, + ndofs2, + self.impulse[0], + jacobians, + &-tangents1[0], + &self.gcross2[0], + solver_vels, + im2, + ); + } + + #[cfg(feature = "dim3")] + { + solver_vel1.apply_impulse( + j_id1, + ndofs1, + self.impulse[0], + jacobians, + tangents1[0], + &self.gcross1[0], + solver_vels, + im1, + ); + solver_vel1.apply_impulse( + j_id1 + j_step, + ndofs1, + self.impulse[1], + jacobians, + tangents1[1], + &self.gcross1[1], + solver_vels, + im1, + ); + + solver_vel2.apply_impulse( + j_id2, + ndofs2, + self.impulse[0], + jacobians, + &-tangents1[0], + &self.gcross2[0], + solver_vels, + im2, + ); + solver_vel2.apply_impulse( + j_id2 + j_step, + ndofs2, + self.impulse[1], + jacobians, + &-tangents1[1], + &self.gcross2[1], + solver_vels, + im2, + ); + } + } + #[inline] pub fn generic_solve( &mut self, @@ -240,6 +329,45 @@ impl TwoBodyConstraintTangentPart { } impl TwoBodyConstraintNormalPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id: usize, + jacobians: &DVector, + dir1: &Vector, + im1: &Vector, + im2: &Vector, + ndofs1: usize, + ndofs2: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, + ) { + let j_id1 = j_id1(j_id, ndofs1, ndofs2); + let j_id2 = j_id2(j_id, ndofs1, ndofs2); + + solver_vel1.apply_impulse( + j_id1, + ndofs1, + self.impulse, + jacobians, + dir1, + &self.gcross1, + solver_vels, + im1, + ); + solver_vel2.apply_impulse( + j_id2, + ndofs2, + self.impulse, + jacobians, + &-dir1, + &self.gcross2, + solver_vels, + im2, + ); + } + #[inline] pub fn generic_solve( &mut self, @@ -290,6 +418,74 @@ impl TwoBodyConstraintNormalPart { } impl TwoBodyConstraintElement { + #[inline] + pub fn generic_warmstart_group( + elements: &mut [Self], + jacobians: &DVector, + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: &Vector, + im2: &Vector, + // ndofs is 0 for a non-multibody body, or a multibody with zero + // degrees of freedom. + ndofs1: usize, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, + ) { + let j_step = j_step(ndofs1, ndofs2) * DIM; + + // Solve penetration. + { + let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2); + + for element in elements.iter_mut() { + element.normal_part.generic_warmstart( + nrm_j_id, + jacobians, + dir1, + im1, + im2, + ndofs1, + ndofs2, + solver_vel1, + solver_vel2, + solver_vels, + ); + nrm_j_id += j_step; + } + } + + // Solve friction. + { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2); + + for element in elements.iter_mut() { + let part = &mut element.tangent_part; + part.generic_warmstart( + tng_j_id, + jacobians, + tangents1, + im1, + im2, + ndofs1, + ndofs2, + solver_vel1, + solver_vel2, + solver_vels, + ); + tng_j_id += j_step; + } + } + } + #[inline] pub fn generic_solve_group( cfm_factor: Real, diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 357f1c4..c31d2aa 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -151,7 +151,7 @@ impl OneBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - impulse: na::zero(), + impulse: manifold_point.warmstart_impulse, impulse_accumulator: na::zero(), r: projected_mass, r_mat_elts: [0.0; 2], @@ -160,7 +160,8 @@ impl OneBodyConstraintBuilder { // Tangent parts. { - constraint.elements[k].tangent_part.impulse = na::zero(); + constraint.elements[k].tangent_part.impulse = + manifold_point.warmstart_tangent_impulse; for j in 0..DIM - 1 { let gcross2 = mprops2 @@ -317,13 +318,13 @@ impl OneBodyConstraintBuilder { element.normal_part.rhs_wo_bias = rhs_wo_bias; element.normal_part.rhs = new_rhs; element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse = na::zero(); + element.normal_part.impulse *= params.warmstart_coefficient; } // Tangent part. { element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse = na::zero(); + element.tangent_part.impulse *= params.warmstart_coefficient; for j in 0..DIM - 1 { let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; @@ -369,6 +370,21 @@ impl OneBodyConstraint { } } + pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel2 = solver_vels[self.solver_vel2]; + + OneBodyConstraintElement::warmstart_group( + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im2, + &mut solver_vel2, + ); + + solver_vels[self.solver_vel2] = solver_vel2; + } + pub fn solve( &mut self, solver_vels: &mut [SolverVel], @@ -400,17 +416,11 @@ impl OneBodyConstraint { for k in 0..self.num_contacts as usize { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = - self.elements[k].tangent_part.total_impulse()[0]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); - } + active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse; + active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; + active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); + active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); } } diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs index e5ae140..7ec8c5d 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -1,9 +1,7 @@ -use crate::dynamics::integration_parameters::{ - BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY, -}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart; use crate::dynamics::solver::SolverVel; -use crate::math::{AngVector, Vector, DIM}; +use crate::math::{AngVector, TangentImpulse, Vector, DIM}; use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; use na::Vector2; @@ -12,14 +10,8 @@ pub(crate) struct OneBodyConstraintTangentPart { pub gcross2: [AngVector; DIM - 1], pub rhs: [N; DIM - 1], pub rhs_wo_bias: [N; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: na::Vector1, - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - #[cfg(feature = "dim2")] - pub impulse_accumulator: na::Vector1, - #[cfg(feature = "dim3")] - pub impulse_accumulator: na::Vector2, + pub impulse: TangentImpulse, + pub impulse_accumulator: TangentImpulse, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -43,57 +35,29 @@ impl OneBodyConstraintTangentPart { /// Total impulse applied across all the solver substeps. #[inline] - #[cfg(feature = "dim2")] - pub fn total_impulse(&self) -> na::Vector1 { + pub fn total_impulse(&self) -> TangentImpulse { self.impulse_accumulator + self.impulse } - /// Total impulse applied across all the solver substeps. #[inline] - #[cfg(feature = "dim3")] - pub fn total_impulse(&self) -> na::Vector2 { - self.impulse_accumulator + self.impulse - } - - #[inline] - pub fn apply_limit( + pub fn warmstart( &mut self, tangents1: [&Vector; DIM - 1], im2: &Vector, - limit: N, solver_vel2: &mut SolverVel, - ) where - AngVector: SimdDot, Result = N>, - { - if DISABLE_FRICTION_LIMIT_REAPPLY { - return; - } - + ) { #[cfg(feature = "dim2")] { - let new_impulse = self.impulse[0].simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2[0] * dlambda; + solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]; + solver_vel2.angular += self.gcross2[0] * self.impulse[0]; } #[cfg(feature = "dim3")] { - let new_impulse = self.impulse; - let new_impulse = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - new_impulse.simd_cap_magnitude(limit) - }; - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] - + tangents1[1].component_mul(im2) * -dlambda[1]; - solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0] + + tangents1[1].component_mul(im2) * -self.impulse[1]; + solver_vel2.angular += + self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1]; } } @@ -184,6 +148,12 @@ impl OneBodyConstraintNormalPart { self.impulse_accumulator + self.impulse } + #[inline] + pub fn warmstart(&mut self, dir1: &Vector, im2: &Vector, solver_vel2: &mut SolverVel) { + solver_vel2.linear += dir1.component_mul(im2) * -self.impulse; + solver_vel2.angular += self.gcross2 * self.impulse; + } + #[inline] pub fn solve( &mut self, @@ -257,6 +227,25 @@ impl OneBodyConstraintElement { } } + #[inline] + pub fn warmstart_group( + elements: &mut [Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im2: &Vector, + solver_vel2: &mut SolverVel, + ) { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + + for element in elements.iter_mut() { + element.normal_part.warmstart(dir1, im2, solver_vel2); + element.tangent_part.warmstart(tangents1, im2, solver_vel2); + } + } + #[inline] pub fn solve_group( cfm_factor: N, @@ -293,13 +282,6 @@ impl OneBodyConstraintElement { im2, solver_vel2, ); - - // There is one constraint left to solve if there isn’t an even number. - for i in 0..2 { - let limit = limit * elements[i].normal_part.impulse; - let part = &mut elements[i].tangent_part; - part.apply_limit(tangents1, im2, limit, solver_vel2); - } } if elements.len() % 2 == 1 { @@ -307,18 +289,12 @@ impl OneBodyConstraintElement { element .normal_part .solve(cfm_factor, dir1, im2, solver_vel2); - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.apply_limit(tangents1, im2, limit, solver_vel2); } } else { for element in elements.iter_mut() { element .normal_part .solve(cfm_factor, dir1, im2, solver_vel2); - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.apply_limit(tangents1, im2, limit, solver_vel2); } } } diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs index 8819cea..77dfc42 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs @@ -8,8 +8,8 @@ use crate::dynamics::{ }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; #[cfg(feature = "dim2")] use crate::utils::SimdBasis; @@ -120,6 +120,11 @@ impl SimdOneBodyConstraintBuilder { let is_bouncy = SimdReal::from(gather![ |ii| manifold_points[ii][k].is_bouncy() as u32 as Real ]); + let warmstart_impulse = + SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]); + let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points + [ii][k] + .warmstart_tangent_impulse]); let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); let point = Point::from(gather![|ii| manifold_points[ii][k].point]); @@ -155,7 +160,7 @@ impl SimdOneBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - impulse: na::zero(), + impulse: warmstart_impulse, impulse_accumulator: na::zero(), r: projected_mass, r_mat_elts: [SimdReal::zero(); 2], @@ -163,7 +168,7 @@ impl SimdOneBodyConstraintBuilder { } // tangent parts. - constraint.elements[k].tangent_part.impulse = na::zero(); + constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse; for j in 0..DIM - 1 { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); @@ -263,6 +268,7 @@ impl SimdOneBodyConstraintBuilder { let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); + let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]]; let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]); @@ -309,13 +315,13 @@ impl SimdOneBodyConstraintBuilder { element.normal_part.rhs_wo_bias = rhs_wo_bias; element.normal_part.rhs = new_rhs; element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse = na::zero(); + element.normal_part.impulse *= warmstart_coeff; } // tangent parts. { element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse = na::zero(); + element.tangent_part.impulse *= warmstart_coeff; for j in 0..DIM - 1 { let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; @@ -344,6 +350,27 @@ pub(crate) struct OneBodyConstraintSimd { } impl OneBodyConstraintSimd { + pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel2 = SolverVel { + linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), + angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), + }; + + OneBodyConstraintElement::warmstart_group( + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im2, + &mut solver_vel2, + ); + + for ii in 0..SIMD_WIDTH { + solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); + } + } + pub fn solve( &mut self, solver_vels: &mut [SolverVel], @@ -377,27 +404,21 @@ impl OneBodyConstraintSimd { // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint. pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { + let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse; let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into(); - #[cfg(feature = "dim2")] - let tangent_impulses: [_; SIMD_WIDTH] = - self.elements[k].tangent_part.total_impulse()[0].into(); - #[cfg(feature = "dim3")] let tangent_impulses = self.elements[k].tangent_part.total_impulse(); for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let contact_id = self.manifold_contact_id[k][ii]; let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.impulse = impulses[ii]; - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = tangent_impulses[ii]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = tangent_impulses.extract(ii); - } + active_contact.data.warmstart_impulse = warmstart_impulses[ii]; + active_contact.data.warmstart_tangent_impulse = + warmstart_tangent_impulses.extract(ii); + active_contact.data.impulse = impulses[ii]; + active_contact.data.tangent_impulse = tangent_impulses.extract(ii); } } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 24a5730..f0d95d9 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -25,6 +25,25 @@ impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> { Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), } } + pub fn warmstart( + &mut self, + generic_jacobians: &DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + match self { + Self::OneBody(c) => c.warmstart(solver_vels), + Self::TwoBodies(c) => c.warmstart(solver_vels), + Self::GenericOneBody(c) => c.warmstart(generic_jacobians, generic_solver_vels), + Self::GenericTwoBodies(c) => { + c.warmstart(generic_jacobians, solver_vels, generic_solver_vels) + } + #[cfg(feature = "simd-is-enabled")] + Self::SimdOneBody(c) => c.warmstart(solver_vels), + #[cfg(feature = "simd-is-enabled")] + Self::SimdTwoBodies(c) => c.warmstart(solver_vels), + } + } pub fn solve_restitution( &mut self, @@ -224,7 +243,7 @@ impl TwoBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - impulse: na::zero(), + impulse: manifold_point.warmstart_impulse, impulse_accumulator: na::zero(), r: projected_mass, r_mat_elts: [0.0; 2], @@ -233,7 +252,8 @@ impl TwoBodyConstraintBuilder { // Tangent parts. { - constraint.elements[k].tangent_part.impulse = na::zero(); + constraint.elements[k].tangent_part.impulse = + manifold_point.warmstart_tangent_impulse; for j in 0..DIM - 1 { let gcross1 = mprops1 @@ -398,13 +418,13 @@ impl TwoBodyConstraintBuilder { element.normal_part.rhs_wo_bias = rhs_wo_bias; element.normal_part.rhs = new_rhs; element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse = na::zero(); + element.normal_part.impulse *= params.warmstart_coefficient; } // Tangent part. { element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse = na::zero(); + element.tangent_part.impulse *= params.warmstart_coefficient; for j in 0..DIM - 1 { let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; @@ -418,6 +438,25 @@ impl TwoBodyConstraintBuilder { } impl TwoBodyConstraint { + pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel1 = solver_vels[self.solver_vel1]; + let mut solver_vel2 = solver_vels[self.solver_vel2]; + + TwoBodyConstraintElement::warmstart_group( + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + + solver_vels[self.solver_vel1] = solver_vel1; + solver_vels[self.solver_vel2] = solver_vel2; + } + pub fn solve( &mut self, solver_vels: &mut [SolverVel], @@ -452,17 +491,10 @@ impl TwoBodyConstraint { for k in 0..self.num_contacts as usize { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse; + active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); - - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = - self.elements[k].tangent_part.total_impulse()[0]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); - } + active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs index e5cd6d5..7e17e73 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs @@ -1,9 +1,7 @@ -use crate::dynamics::integration_parameters::{ - BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY, -}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::contact_constraint::OneBodyConstraintNormalPart; use crate::dynamics::solver::SolverVel; -use crate::math::{AngVector, Vector, DIM}; +use crate::math::{AngVector, TangentImpulse, Vector, DIM}; use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; use na::{Matrix2, Vector2}; use num::Zero; @@ -47,67 +45,41 @@ impl TwoBodyConstraintTangentPart { /// Total impulse applied across all the solver substeps. #[inline] - #[cfg(feature = "dim2")] - pub fn total_impulse(&self) -> na::Vector1 { + pub fn total_impulse(&self) -> TangentImpulse { self.impulse_accumulator + self.impulse } - /// Total impulse applied across all the solver substeps. #[inline] - #[cfg(feature = "dim3")] - pub fn total_impulse(&self) -> na::Vector2 { - self.impulse_accumulator + self.impulse - } - - #[inline] - pub fn apply_limit( + pub fn warmstart( &mut self, tangents1: [&Vector; DIM - 1], im1: &Vector, im2: &Vector, - limit: N, solver_vel1: &mut SolverVel, solver_vel2: &mut SolverVel, ) where AngVector: SimdDot, Result = N>, { - if DISABLE_FRICTION_LIMIT_REAPPLY { - return; - } - #[cfg(feature = "dim2")] { - let new_impulse = self.impulse[0].simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda; - solver_vel1.angular += self.gcross1[0] * dlambda; + solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]; + solver_vel1.angular += self.gcross1[0] * self.impulse[0]; - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2[0] * dlambda; + solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]; + solver_vel2.angular += self.gcross2[0] * self.impulse[0]; } #[cfg(feature = "dim3")] { - let new_impulse = self.impulse; - let new_impulse = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - new_impulse.simd_cap_magnitude(limit) - }; - - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0] - + tangents1[1].component_mul(im1) * dlambda[1]; - solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; - - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] - + tangents1[1].component_mul(im2) * -dlambda[1]; - solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0] + + tangents1[1].component_mul(im1) * self.impulse[1]; + solver_vel1.angular += + self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1]; + + solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0] + + tangents1[1].component_mul(im2) * -self.impulse[1]; + solver_vel2.angular += + self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1]; } } @@ -220,6 +192,22 @@ impl TwoBodyConstraintNormalPart { self.impulse_accumulator + self.impulse } + #[inline] + pub fn warmstart( + &mut self, + dir1: &Vector, + im1: &Vector, + im2: &Vector, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) { + solver_vel1.linear += dir1.component_mul(im1) * self.impulse; + solver_vel1.angular += self.gcross1 * self.impulse; + + solver_vel2.linear += dir1.component_mul(im2) * -self.impulse; + solver_vel2.angular += self.gcross2 * self.impulse; + } + #[inline] pub fn solve( &mut self, @@ -339,6 +327,31 @@ impl TwoBodyConstraintElement { } } + #[inline] + pub fn warmstart_group( + elements: &mut [Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: &Vector, + im2: &Vector, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + + for element in elements.iter_mut() { + element + .normal_part + .warmstart(dir1, im1, im2, solver_vel1, solver_vel2); + element + .tangent_part + .warmstart(tangents1, im1, im2, solver_vel1, solver_vel2); + } + } + #[inline] pub fn solve_group( cfm_factor: N, @@ -350,19 +363,13 @@ impl TwoBodyConstraintElement { limit: N, solver_vel1: &mut SolverVel, solver_vel2: &mut SolverVel, - solve_normal: bool, + solve_restitution: bool, solve_friction: bool, ) where Vector: SimdBasis, AngVector: SimdDot, Result = N>, { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - // Solve penetration. - if solve_normal { + if solve_restitution { if BLOCK_SOLVER_ENABLED { for elements in elements.chunks_exact_mut(2) { let [element_a, element_b] = elements else { @@ -379,12 +386,6 @@ impl TwoBodyConstraintElement { solver_vel1, solver_vel2, ); - - for i in 0..2 { - let limit = limit * elements[i].normal_part.impulse; - let part = &mut elements[i].tangent_part; - part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2); - } } // There is one constraint left to solve if there isn’t an even number. @@ -393,24 +394,22 @@ impl TwoBodyConstraintElement { element .normal_part .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2); - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2); } } else { for element in elements.iter_mut() { element .normal_part .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2); - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2); } } } - // Solve friction. if solve_friction { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + for element in elements.iter_mut() { let limit = limit * element.normal_part.impulse; let part = &mut element.tangent_part; diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs index 9b05fd3..a6c387d 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs @@ -8,8 +8,8 @@ use crate::dynamics::{ }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; #[cfg(feature = "dim2")] use crate::utils::SimdBasis; @@ -104,6 +104,11 @@ impl TwoBodyConstraintBuilderSimd { let is_bouncy = SimdReal::from(gather![ |ii| manifold_points[ii][k].is_bouncy() as u32 as Real ]); + let warmstart_impulse = + SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]); + let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points + [ii][k] + .warmstart_tangent_impulse]); let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); let point = Point::from(gather![|ii| manifold_points[ii][k].point]); @@ -140,7 +145,7 @@ impl TwoBodyConstraintBuilderSimd { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - impulse: SimdReal::splat(0.0), + impulse: warmstart_impulse, impulse_accumulator: SimdReal::splat(0.0), r: projected_mass, r_mat_elts: [SimdReal::zero(); 2], @@ -148,7 +153,7 @@ impl TwoBodyConstraintBuilderSimd { } // tangent parts. - constraint.elements[k].tangent_part.impulse = na::zero(); + constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse; for j in 0..DIM - 1 { let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); @@ -253,6 +258,7 @@ impl TwoBodyConstraintBuilderSimd { let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); + let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]]; let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]]; @@ -297,13 +303,13 @@ impl TwoBodyConstraintBuilderSimd { element.normal_part.rhs_wo_bias = rhs_wo_bias; element.normal_part.rhs = new_rhs; element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse = na::zero(); + element.normal_part.impulse *= warmstart_coeff; } // tangent parts. { element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse = na::zero(); + element.tangent_part.impulse *= warmstart_coeff; for j in 0..DIM - 1 { let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; @@ -334,6 +340,38 @@ pub(crate) struct TwoBodyConstraintSimd { } impl TwoBodyConstraintSimd { + pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel1 = SolverVel { + linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]), + angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]), + }; + + let mut solver_vel2 = SolverVel { + linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), + angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), + }; + + TwoBodyConstraintElement::warmstart_group( + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + + for ii in 0..SIMD_WIDTH { + solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii); + solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); + solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); + } + } + pub fn solve( &mut self, solver_vels: &mut [SolverVel], @@ -377,27 +415,20 @@ impl TwoBodyConstraintSimd { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { + let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse; let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into(); - #[cfg(feature = "dim2")] - let tangent_impulses: [_; SIMD_WIDTH] = - self.elements[k].tangent_part.total_impulse()[0].into(); - #[cfg(feature = "dim3")] let tangent_impulses = self.elements[k].tangent_part.total_impulse(); for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let contact_id = self.manifold_contact_id[k][ii]; let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.warmstart_impulse = warmstart_impulses[ii]; + active_contact.data.warmstart_tangent_impulse = + warmstart_tangent_impulses.extract(ii); active_contact.data.impulse = impulses[ii]; - - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = tangent_impulses[ii]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = tangent_impulses.extract(ii); - } + active_contact.data.tangent_impulse = tangent_impulses.extract(ii); } } } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 85d2211..7ea48ab 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,5 +1,4 @@ use super::{JointConstraintTypes, SolverConstraintsSet}; -use crate::dynamics::integration_parameters::DISABLE_FRICTION_LIMIT_REAPPLY; use crate::dynamics::solver::solver_body::SolverBody; use crate::dynamics::{ solver::{ContactConstraintTypes, SolverVel}, @@ -180,6 +179,10 @@ impl VelocitySolver { joint_constraints.update(params, multibodies, &self.solver_bodies); contact_constraints.update(params, substep_id, multibodies, &self.solver_bodies); + if params.warmstart_coefficient != 0.0 { + contact_constraints.warmstart(&mut self.solver_vels, &mut self.generic_solver_vels); + } + for _ in 0..params.num_internal_pgs_iterations { joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels); contact_constraints @@ -203,60 +206,17 @@ impl VelocitySolver { /* * Resolution without bias. */ - let compute_max_dlinvel = |vels: &[SolverVel]| { - vels.iter() - .map(|v| v.linear.norm()) - .max_by_key(|v| OrderedFloat(*v)) - .unwrap_or_default() - }; - - let mut prev_dlinvel = f32::MAX; - let mut prev_solver_vels = self.solver_vels.clone(); - - for kk in 0..params.max_internal_stabilization_iterations { - prev_solver_vels.clone_from_slice(&self.solver_vels); + for _ in 0..params.num_internal_stabilization_iterations { joint_constraints .solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels); contact_constraints.solve_restitution_wo_bias( &mut self.solver_vels, &mut self.generic_solver_vels, ); - - if DISABLE_FRICTION_LIMIT_REAPPLY { - contact_constraints - .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels); - } - - for (prev, new) in prev_solver_vels.iter_mut().zip(self.solver_vels.iter()) { - *prev -= *new; - } - - let new_max_linvel = compute_max_dlinvel(&self.solver_vels); - - println!(">> {} >> max_linvel: {}", kk, new_max_linvel); - - if new_max_linvel > prev_dlinvel { - break; - } - - prev_dlinvel = new_max_linvel; - - if prev_solver_vels - .iter() - .zip(self.solver_vels.iter()) - .all(|(diff, vels)| { - diff.linear.norm() < 1.0e-3 - || diff.linear.norm() <= 0.2 * vels.linear.norm() - }) - { - break; - } - - // if (new_max_dlinvel - max_dlinvel).abs() <= 0.2 * max_dlinvel { - // println!("Num effective stab steps: {}", kk + 1); - // break; - // } } + + contact_constraints + .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels); } } -- cgit