diff options
Diffstat (limited to 'src/dynamics/solver')
18 files changed, 1212 insertions, 312 deletions
diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index b5fdb6d..6809c37 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -25,6 +25,7 @@ use { crate::math::SIMD_WIDTH, }; +#[derive(Debug)] pub struct ConstraintsCounts { pub num_constraints: usize, pub num_jacobian_lines: usize, @@ -441,6 +442,17 @@ impl ContactConstraintsSet { assert_eq!(curr_start, total_num_constraints); } + pub fn warmstart( + &mut self, + solver_vels: &mut [SolverVel<Real>], + generic_solver_vels: &mut DVector<Real>, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.warmstart(jac, solver_vels, generic_solver_vels); + } + } + pub fn solve_restitution( &mut self, solver_vels: &mut [SolverVel<Real>], 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 7b1f8ea..e254995 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -153,8 +153,9 @@ impl GenericOneBodyConstraintBuilder { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r, + r_mat_elts: [0.0; 2], }; } @@ -230,13 +231,8 @@ impl GenericOneBodyConstraintBuilder { .unwrap() .local_to_world; - self.inner.update_with_positions( - params, - solved_dt, - pos2, - self.ccd_thickness, - &mut constraint.inner, - ); + self.inner + .update_with_positions(params, solved_dt, pos2, &mut constraint.inner); } } @@ -258,6 +254,24 @@ impl GenericOneBodyConstraint { } } + pub fn warmstart( + &mut self, + jacobians: &DVector<Real>, + generic_solver_vels: &mut DVector<Real>, + ) { + 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<Real>, 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 @@ -8,6 +8,40 @@ use na::SimdPartialOrd; impl OneBodyConstraintTangentPart<Real> { #[inline] + pub fn generic_warmstart( + &mut self, + j_id2: usize, + jacobians: &DVector<Real>, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector<Real>, + ) { + #[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, j_id2: usize, @@ -72,6 +106,22 @@ impl OneBodyConstraintTangentPart<Real> { impl OneBodyConstraintNormalPart<Real> { #[inline] + pub fn generic_warmstart( + &mut self, + j_id2: usize, + jacobians: &DVector<Real>, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector<Real>, + ) { + 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, cfm_factor: Real, @@ -100,6 +150,42 @@ impl OneBodyConstraintNormalPart<Real> { impl OneBodyConstraintElement<Real> { #[inline] + pub fn generic_warmstart_group( + elements: &mut [Self], + jacobians: &DVector<Real>, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + solver_vel2: usize, + solver_vels: &mut DVector<Real>, + ) { + 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, elements: &mut [Self], 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 073f585..dd63d1f 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -201,15 +201,17 @@ impl GenericTwoBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - total_impulse: na::zero(), - impulse: na::zero(), + impulse_accumulator: na::zero(), + impulse: manifold_point.warmstart_impulse, r, + r_mat_elts: [0.0; 2], }; } // 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]); @@ -340,14 +342,8 @@ impl GenericTwoBodyConstraintBuilder { .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position); - self.inner.update_with_positions( - params, - solved_dt, - pos1, - pos2, - self.ccd_thickness, - &mut constraint.inner, - ); + self.inner + .update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner); } } @@ -373,6 +369,50 @@ impl GenericTwoBodyConstraint { } } + pub fn warmstart( + &mut self, + jacobians: &DVector<Real>, + solver_vels: &mut [SolverVel<Real>], + generic_solver_vels: &mut DVector<Real>, + ) { + let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1]) + } else { + GenericRhs::GenericId(self.inner.solver_vel1) + }; + + let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2]) + } else { + GenericRhs::GenericId(self.inner.solver_vel2) + }; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + TwoBodyConstraintElement::generic_warmstart_group( + elements, + jacobians, + &self.inner.dir1, + #[cfg(feature = "dim3")] + &self.inner.tangent1, + &self.inner.im1, + &self.inner.im2, + self.ndofs1, + self.ndofs2, + self.j_id, + &mut solver_vel1, + &mut solver_vel2, + generic_solver_vels, + ); + + if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { + solver_vels[self.inner.solver_vel1] = solver_vel1; + } + + if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { + solver_vels[self.inner.solver_vel2] = solver_vel2; + } + } + pub fn solve( &mut self, jacobians: &DVector<Real>, 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 @@ -89,6 +89,95 @@ impl GenericRhs { impl TwoBodyConstraintTangentPart<Real> { #[inline] + pub fn generic_warmstart( + &mut self, + j_id: usize, + jacobians: &DVector<Real>, + tangents1: [&Vector<Real>; DIM - 1], + im1: &Vector<Real>, + im2: &Vector<Real>, + ndofs1: usize, + ndofs2: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector<Real>, + ) { + 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, j_id: usize, @@ -241,6 +330,45 @@ impl TwoBodyConstraintTangentPart<Real> { impl TwoBodyConstraintNormalPart<Real> { #[inline] + pub fn generic_warmstart( + &mut self, + j_id: usize, + jacobians: &DVector<Real>, + dir1: &Vector<Real>, + im1: &Vector<Real>, + im2: &Vector<Real>, + ndofs1: usize, + ndofs2: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector<Real>, + ) { + 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, cfm_factor: Real, @@ -291,6 +419,74 @@ impl TwoBodyConstraintNormalPart<Real> { impl TwoBodyConstraintElement<Real> { #[inline] + pub fn generic_warmstart_group( + elements: &mut [Self], + jacobians: &DVector<Real>, + dir1: &Vector<Real>, + #[cfg(feature = "dim3")] tangent1: &Vector<Real>, + im1: &Vector<Real>, + im2: &Vector<Real>, + // 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<Real>, + ) { + 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, elements: &mut [Self], diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index be108a4..5e6e86b 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -3,8 +3,10 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; #[cfg(feature = "dim2")] use crate::utils::SimdBasis; use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy}; +use na::Matrix2; use parry::math::Isometry; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::solver_body::SolverBody; use crate::dynamics::solver::SolverVel; use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity}; @@ -130,10 +132,11 @@ impl OneBodyConstraintBuilder { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let projected_mass = utils::inv( - force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + gcross2.gdot(gcross2), - ); + let projected_lin_mass = + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)); + let projected_ang_mass = gcross2.gdot(gcross2); + + let projected_mass = utils::inv(projected_lin_mass + projected_ang_mass); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; @@ -148,15 +151,17 @@ impl OneBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - impulse: na::zero(), - total_impulse: na::zero(), + impulse: manifold_point.warmstart_impulse, + impulse_accumulator: na::zero(), r: projected_mass, + r_mat_elts: [0.0; 2], }; } // 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 @@ -205,6 +210,44 @@ impl OneBodyConstraintBuilder { builder.infos[k] = infos; } } + + if BLOCK_SOLVER_ENABLED { + // Coupling between consecutive pairs. + for k in 0..manifold_points.len() / 2 { + let k0 = k * 2; + let k1 = k * 2 + 1; + + let mut r_mat = Matrix2::zeros(); + let r0 = constraint.elements[k0].normal_part.r; + let r1 = constraint.elements[k1].normal_part.r; + r_mat.m12 = force_dir1 + .dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + constraint.elements[k0] + .normal_part + .gcross2 + .gdot(constraint.elements[k1].normal_part.gcross2); + r_mat.m21 = r_mat.m12; + r_mat.m11 = utils::inv(r0); + r_mat.m22 = utils::inv(r1); + + if let Some(inv) = r_mat.try_inverse() { + constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22]; + constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12]; + } else { + // If inversion failed, the contacts are redundant. + // Ignore the one with the smallest depth (it is too late to + // have the constraint removed from the constraint set, so just + // set the mass (r) matrix elements to 0. + constraint.elements[k0].normal_part.r_mat_elts = + if manifold_points[k0].dist <= manifold_points[k1].dist { + [r0, 0.0] + } else { + [0.0, r1] + }; + constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2]; + } + } + } } } @@ -217,13 +260,7 @@ impl OneBodyConstraintBuilder { constraint: &mut OneBodyConstraint, ) { let rb2 = &bodies[constraint.solver_vel2]; - self.update_with_positions( - params, - solved_dt, - &rb2.position, - rb2.ccd_thickness, - constraint, - ) + self.update_with_positions(params, solved_dt, &rb2.position, constraint) } // TODO: this code is SOOOO similar to TwoBodyConstraint::update. @@ -233,12 +270,11 @@ impl OneBodyConstraintBuilder { params: &IntegrationParameters, solved_dt: Real, rb2_pos: &Isometry<Real>, - ccd_thickness: Real, constraint: &mut OneBodyConstraint, ) { - let cfm_factor = params.cfm_factor(); + let cfm_factor = params.contact_cfm_factor(); let inv_dt = params.inv_dt(); - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.contact_erp_inv_dt(); let all_infos = &self.infos[..constraint.num_contacts as usize]; let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; @@ -247,7 +283,6 @@ impl OneBodyConstraintBuilder { let new_pos1 = self .vels1 .integrate(solved_dt, &rb1.position, &rb1.local_com); - let mut is_fast_contact = false; #[cfg(feature = "dim2")] let tangents1 = constraint.dir1.orthonormal_basis(); @@ -266,23 +301,20 @@ impl OneBodyConstraintBuilder { // Normal part. { let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt; - let rhs_bias = erp_inv_dt - * (dist + params.allowed_linear_error) - .clamp(-params.max_penetration_correction, 0.0); + let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error())) + .clamp(-params.max_corrective_velocity(), 0.0); let new_rhs = rhs_wo_bias + rhs_bias; - let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse; - is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5); element.normal_part.rhs_wo_bias = rhs_wo_bias; element.normal_part.rhs = new_rhs; - element.normal_part.total_impulse = total_impulse; - element.normal_part.impulse = na::zero(); + element.normal_part.impulse_accumulator += element.normal_part.impulse; + element.normal_part.impulse *= params.warmstart_coefficient; } // Tangent part. { - element.tangent_part.total_impulse += element.tangent_part.impulse; - element.tangent_part.impulse = na::zero(); + element.tangent_part.impulse_accumulator += element.tangent_part.impulse; + element.tangent_part.impulse *= params.warmstart_coefficient; for j in 0..DIM - 1 { let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; @@ -291,7 +323,7 @@ impl OneBodyConstraintBuilder { } } - constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + constraint.cfm_factor = cfm_factor; } } @@ -328,6 +360,21 @@ impl OneBodyConstraint { } } + pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) { + 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<Real>], @@ -359,16 +406,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.impulse; - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.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 d9ff7f4..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,20 +1,17 @@ +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; #[derive(Copy, Clone, Debug)] pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> { pub gcross2: [AngVector<N>; DIM - 1], pub rhs: [N; DIM - 1], pub rhs_wo_bias: [N; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: na::Vector1<N>, - #[cfg(feature = "dim3")] - pub impulse: na::Vector2<N>, - #[cfg(feature = "dim2")] - pub total_impulse: na::Vector1<N>, - #[cfg(feature = "dim3")] - pub total_impulse: na::Vector2<N>, + pub impulse: TangentImpulse<N>, + pub impulse_accumulator: TangentImpulse<N>, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -28,7 +25,7 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> { rhs: [na::zero(); DIM - 1], rhs_wo_bias: [na::zero(); DIM - 1], impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), #[cfg(feature = "dim2")] r: [na::zero(); 1], #[cfg(feature = "dim3")] @@ -36,41 +33,31 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> { } } + /// Total impulse applied across all the solver substeps. #[inline] - pub fn apply_limit( + pub fn total_impulse(&self) -> TangentImpulse<N> { + self.impulse_accumulator + self.impulse + } + + #[inline] + pub fn warmstart( &mut self, tangents1: [&Vector<N>; DIM - 1], im2: &Vector<N>, - limit: N, solver_vel2: &mut SolverVel<N>, - ) where - AngVector<N>: SimdDot<AngVector<N>, Result = N>, - { + ) { #[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]; } } @@ -137,8 +124,9 @@ pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> { pub rhs: N, pub rhs_wo_bias: N, pub impulse: N, - pub total_impulse: N, + pub impulse_accumulator: N, pub r: N, + pub r_mat_elts: [N; 2], } impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> { @@ -148,11 +136,24 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> { rhs: na::zero(), rhs_wo_bias: na::zero(), |