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 --- src/dynamics/integration_parameters.rs | 130 +++++++------- .../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 +----- 13 files changed, 694 insertions(+), 309 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 093ebe9..80ca92c 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -4,7 +4,6 @@ use std::num::NonZeroUsize; // TODO: enabling the block solver in 3d introduces a lot of jitters in // the 3D domino demo. So for now we dont enable it in 3D. pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2"); -pub(crate) static DISABLE_FRICTION_LIMIT_REAPPLY: bool = false; /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug)] @@ -26,12 +25,12 @@ pub struct IntegrationParameters { /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) /// will be compensated for during the velocity solve. - /// (default `0.8`). + /// (default `0.1`). pub erp: Real, /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations /// before stabilization). - /// (default `0.25`). + /// (default `20.0`). pub damping_ratio: Real, /// 0-1: multiplier for how much of the joint violation @@ -40,14 +39,21 @@ pub struct IntegrationParameters { pub joint_erp: Real, /// The fraction of critical damping applied to the joint for constraints regularization. - /// (default `0.25`). + /// (default `1.0`). pub joint_damping_ratio: Real, - /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). + /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the + /// initial solution (instead of 0) at the next simulation step. + /// + /// This should generally be set to 1. Can be set to 0 if using a large [`Self::erp`] value. + /// (default `1.0`). + pub warmstart_coefficient: Real, + + /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, /// Maximum amount of penetration the solver will attempt to resolve in one timestep. pub max_penetration_correction: Real, - /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`). pub prediction_distance: Real, /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). pub num_solver_iterations: NonZeroUsize, @@ -55,8 +61,8 @@ pub struct IntegrationParameters { pub num_additional_friction_iterations: usize, /// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`). pub num_internal_pgs_iterations: usize, - /// The maximum number of stabilization iterations run at each solver iterations (default: `10`). - pub max_internal_stabilization_iterations: usize, + /// The number of stabilization iterations run at each solver iterations (default: `2`). + pub num_internal_stabilization_iterations: usize, /// Minimum number of dynamic bodies in each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). @@ -64,51 +70,6 @@ pub struct IntegrationParameters { } impl IntegrationParameters { - /// Configures the integration parameters to match the old PGS solver - /// from Rapier version <= 0.17. - /// - /// This solver was slightly faster than the new one but resulted - /// in less stable joints and worse convergence rates. - /// - /// This should only be used for comparison purpose or if you are - /// experiencing problems with the new solver. - /// - /// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will - /// still create solver iterations based on the new "small-steps" PGS solver. - /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], - /// [`Self::joint_damping_ratio`] to their former default values. - pub fn switch_to_standard_pgs_solver(&mut self) { - self.num_internal_pgs_iterations *= self.num_solver_iterations.get(); - self.num_solver_iterations = NonZeroUsize::new(1).unwrap(); - self.erp = 0.8; - self.damping_ratio = 0.25; - self.joint_erp = 1.0; - self.joint_damping_ratio = 1.0; - } - - /// Configures the integration parameters to match the new "small-steps" PGS solver - /// from Rapier version >= 0.18. - /// - /// The "small-steps" PGS solver is the default one given by [`Self::default()`] so - /// calling this function is generally not needed unless - /// [`Self::switch_to_standard_pgs_solver()`] was called. - /// - /// This solver results in more stable joints and significantly better convergence - /// rates but is slightly slower in its default settings. - /// - /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], - /// [`Self::joint_damping_ratio`] to their default values. - pub fn switch_to_small_steps_pgs_solver(&mut self) { - self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap(); - self.num_internal_pgs_iterations = 1; - - let default = Self::default(); - self.erp = default.erp; - self.damping_ratio = default.damping_ratio; - self.joint_erp = default.joint_erp; - self.joint_damping_ratio = default.joint_damping_ratio; - } - /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz). /// /// This is zero if `self.dt` is zero. @@ -161,7 +122,7 @@ impl IntegrationParameters { // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass // / (dt * inv_erp_minus_one); // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); - // NOTE: This simplies to cfm = cfm_coefff / projected_mass: + // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio); @@ -180,13 +141,14 @@ impl IntegrationParameters { // new_impulse = cfm_factor * (old_impulse - m * delta_vel) // // The value returned by this function is this cfm_factor that can be used directly - // in the constraints solver. + // in the constraint solver. 1.0 / (1.0 + cfm_coeff) } /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization pub fn joint_cfm_coeff(&self) -> Real { // Compute CFM assuming a critically damped spring multiplied by the damping ratio. + // The logic is similar to `Self::cfm_factor`. let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0; inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) @@ -194,23 +156,23 @@ impl IntegrationParameters { * self.joint_damping_ratio * self.joint_damping_ratio) } -} -impl Default for IntegrationParameters { - fn default() -> Self { + /// Initialize the simulation paramaters with settings matching the TGS-soft solver + /// with warmstarting. + /// + /// This is the default configuration, equivalent to [`IntegrationParameters::default()`]. + pub fn tgs_soft() -> Self { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.8, - damping_ratio: 1.0, + erp: 0.1, + damping_ratio: 20.0, joint_erp: 1.0, joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, - max_penetration_correction: Real::MAX, - prediction_distance: 0.002, + warmstart_coefficient: 1.0, num_internal_pgs_iterations: 1, - max_internal_stabilization_iterations: 10, - num_additional_friction_iterations: 4, + num_internal_stabilization_iterations: 2, + num_additional_friction_iterations: 0, num_solver_iterations: NonZeroUsize::new(4).unwrap(), // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with @@ -218,7 +180,45 @@ impl Default for IntegrationParameters { // However we don't want it to be too small and end up with // tons of islands, reducing SIMD parallelism opportunities. min_island_size: 128, + allowed_linear_error: 0.001, + max_penetration_correction: Real::MAX, + prediction_distance: 0.002, max_ccd_substeps: 1, } } + + /// Initialize the simulation paramaters with settings matching the TGS-soft solver + /// **without** warmstarting. + /// + /// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless + /// warmstarting proves to be undesirable for your use-case. + pub fn tgs_soft_without_warmstart() -> Self { + Self { + erp: 0.8, + damping_ratio: 1.0, + warmstart_coefficient: 0.0, + num_additional_friction_iterations: 4, + ..Self::tgs_soft() + } + } + + /// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17. + /// + /// This exists mainly for testing and comparison purpose. + pub fn pgs_legacy() -> Self { + Self { + erp: 0.8, + damping_ratio: 0.25, + warmstart_coefficient: 0.0, + num_additional_friction_iterations: 4, + num_solver_iterations: NonZeroUsize::new(1).unwrap(), + ..Self::tgs_soft() + } + } +} + +impl Default for IntegrationParameters { + fn default() -> Self { + Self::tgs_soft() + } } 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