From b00113ed2f9e4824a254027b57c9b4a07c4c2307 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 23 Mar 2024 10:15:07 +0100 Subject: fix: implement linear-coupled-motor constraint between two dynamic bodies Fix #602 --- .../contact_constraint/contact_constraints_set.rs | 1 + .../joint_constraint/joint_constraint_builder.rs | 76 ++++++++++++++++++++++ .../joint_constraint/joint_velocity_constraint.rs | 43 ++++++------ 3 files changed, 98 insertions(+), 22 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 b5fdb6d..4d88949 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, diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 00bead1..44716b2 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -551,6 +551,82 @@ impl JointTwoBodyConstraintHelper { constraint } + pub fn motor_linear_coupled( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &JointSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + motor_params: &MotorParameters, + limits: Option<[N; 2]>, + writeback_id: WritebackId, + ) -> JointTwoBodyConstraint { + let inv_dt = N::splat(params.inv_dt()); + + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = na::zero(); + + for i in 0..DIM { + if coupled_axes & (1 << i) != 0 { + let coeff = self.basis.column(i).dot(&self.lin_err); + lin_jac += self.basis.column(i) * coeff; + #[cfg(feature = "dim2")] + { + ang_jac1 += self.cmat1_basis[i] * coeff; + ang_jac2 += self.cmat2_basis[i] * coeff; + } + #[cfg(feature = "dim3")] + { + ang_jac1 += self.cmat1_basis.column(i) * coeff; + ang_jac2 += self.cmat2_basis.column(i) * coeff; + } + } + } + + let dist = lin_jac.norm(); + let inv_dist = crate::utils::simd_inv(dist); + lin_jac *= inv_dist; + ang_jac1 *= inv_dist; + ang_jac2 *= inv_dist; + + let mut rhs_wo_bias = N::zero(); + if motor_params.erp_inv_dt != N::zero() { + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; + } + + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + target_vel = + target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); + }; + + rhs_wo_bias += -target_vel; + + ang_jac1 = body1.sqrt_ii * ang_jac1; + ang_jac2 = body2.sqrt_ii * ang_jac2; + + JointTwoBodyConstraint { + joint_id, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, + im1: body1.im, + im2: body2.im, + impulse: N::zero(), + impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], + lin_jac, + ang_jac1, + ang_jac2, + inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, + rhs: rhs_wo_bias, + rhs_wo_bias, + writeback_id, + } + } + pub fn lock_linear( &self, params: &IntegrationParameters, diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index e184e3d..60c42d3 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -217,28 +217,26 @@ impl JointTwoBodyConstraint { } if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { - // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 { - // let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 { - // Some([ - // joint.limits[first_coupled_lin_axis_id].min, - // joint.limits[first_coupled_lin_axis_id].max, - // ]) - // } else { - // None - // }; - // - // out[len] = builder.motor_linear_coupled - // params, - // [joint_id], - // body1, - // body2, - // coupled_axes, - // &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), - // limits, - // WritebackId::Motor(first_coupled_lin_axis_id), - // ); - // len += 1; - // } + let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { + Some([ + joint.limits[first_coupled_lin_axis_id].min, + joint.limits[first_coupled_lin_axis_id].max, + ]) + } else { + None + }; + + out[len] = builder.motor_linear_coupled( + params, + [joint_id], + body1, + body2, + coupled_axes, + &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), + limits, + WritebackId::Motor(first_coupled_lin_axis_id), + ); + len += 1; } JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); @@ -350,6 +348,7 @@ impl JointTwoBodyConstraint { } } } + #[cfg(feature = "simd-is-enabled")] impl JointTwoBodyConstraint { pub fn lock_axes( -- cgit From 09af4313fa650664792d43def4dc8cda4efa9731 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 23 Mar 2024 16:00:19 +0100 Subject: Fix reported contact impulse in the contact force event Address https://github.com/dimforge/rapier.js/issues/261 --- .../generic_one_body_constraint.rs | 2 +- .../generic_two_body_constraint.rs | 2 +- .../contact_constraint/one_body_constraint.rs | 14 +++++----- .../one_body_constraint_element.rs | 30 ++++++++++++++++++---- .../contact_constraint/one_body_constraint_simd.rs | 14 +++++----- .../contact_constraint/two_body_constraint.rs | 14 +++++----- .../two_body_constraint_element.rs | 30 ++++++++++++++++++---- .../contact_constraint/two_body_constraint_simd.rs | 14 +++++----- 8 files changed, 80 insertions(+), 40 deletions(-) (limited to 'src/dynamics/solver') 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..cfdff55 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -153,7 +153,7 @@ impl GenericOneBodyConstraintBuilder { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r, }; } 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..f0a7d64 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -201,7 +201,7 @@ impl GenericTwoBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), impulse: na::zero(), r, }; diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index be108a4..c9b35f3 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -149,7 +149,7 @@ impl OneBodyConstraintBuilder { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r: projected_mass, }; } @@ -270,18 +270,17 @@ impl OneBodyConstraintBuilder { * (dist + params.allowed_linear_error) .clamp(-params.max_penetration_correction, 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_accumulator += element.normal_part.impulse; element.normal_part.impulse = na::zero(); } // Tangent part. { - element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse_accumulator += element.tangent_part.impulse; element.tangent_part.impulse = na::zero(); for j in 0..DIM - 1 { @@ -359,15 +358,16 @@ 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; + active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); #[cfg(feature = "dim2")] { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; + 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.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..79b207b 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -12,9 +12,9 @@ pub(crate) struct OneBodyConstraintTangentPart { #[cfg(feature = "dim3")] pub impulse: na::Vector2, #[cfg(feature = "dim2")] - pub total_impulse: na::Vector1, + pub impulse_accumulator: na::Vector1, #[cfg(feature = "dim3")] - pub total_impulse: na::Vector2, + pub impulse_accumulator: na::Vector2, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -28,7 +28,7 @@ impl OneBodyConstraintTangentPart { 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,6 +36,20 @@ impl OneBodyConstraintTangentPart { } } + /// Total impulse applied across all the solver substeps. + #[inline] + #[cfg(feature = "dim2")] + pub fn total_impulse(&self) -> na::Vector1 { + 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( &mut self, @@ -137,7 +151,7 @@ pub(crate) struct OneBodyConstraintNormalPart { pub rhs: N, pub rhs_wo_bias: N, pub impulse: N, - pub total_impulse: N, + pub impulse_accumulator: N, pub r: N, } @@ -148,11 +162,17 @@ impl OneBodyConstraintNormalPart { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r: na::zero(), } } + /// Total impulse applied across all the solver substeps. + #[inline] + pub fn total_impulse(&self) -> N { + self.impulse_accumulator + self.impulse + } + #[inline] pub fn solve( &mut self, 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 cd2ea56..03c1abe 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs @@ -154,7 +154,7 @@ impl SimdOneBodyConstraintBuilder { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r: projected_mass, }; } @@ -259,19 +259,18 @@ impl SimdOneBodyConstraintBuilder { .simd_clamp(-max_penetration_correction, SimdReal::zero()) * erp_inv_dt; 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 * dt).simd_gt(ccd_thickness * SimdReal::splat(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_accumulator += element.normal_part.impulse; element.normal_part.impulse = na::zero(); } // tangent parts. { - element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse_accumulator += element.tangent_part.impulse; element.tangent_part.impulse = na::zero(); for j in 0..DIM - 1 { @@ -334,11 +333,12 @@ 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 impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + 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.impulse[0].into(); + 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.impulse; + 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]]; diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 0a0ebd6..8abf5c5 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -223,7 +223,7 @@ impl TwoBodyConstraintBuilder { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r: projected_mass, }; } @@ -348,18 +348,17 @@ impl TwoBodyConstraintBuilder { * (dist + params.allowed_linear_error) .clamp(-params.max_penetration_correction, 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_accumulator += element.normal_part.impulse; element.normal_part.impulse = na::zero(); } // Tangent part. { - element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse_accumulator += element.tangent_part.impulse; element.tangent_part.impulse = na::zero(); for j in 0..DIM - 1 { @@ -408,15 +407,16 @@ 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.impulse = self.elements[k].normal_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.impulse[0]; + 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.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 8c720b9..75fc739 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs @@ -13,9 +13,9 @@ pub(crate) struct TwoBodyConstraintTangentPart { #[cfg(feature = "dim3")] pub impulse: na::Vector2, #[cfg(feature = "dim2")] - pub total_impulse: na::Vector1, + pub impulse_accumulator: na::Vector1, #[cfg(feature = "dim3")] - pub total_impulse: na::Vector2, + pub impulse_accumulator: na::Vector2, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -30,7 +30,7 @@ impl TwoBodyConstraintTangentPart { 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")] @@ -38,6 +38,20 @@ impl TwoBodyConstraintTangentPart { } } + /// Total impulse applied across all the solver substeps. + #[inline] + #[cfg(feature = "dim2")] + pub fn total_impulse(&self) -> na::Vector1 { + 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( &mut self, @@ -166,7 +180,7 @@ pub(crate) struct TwoBodyConstraintNormalPart { pub rhs: N, pub rhs_wo_bias: N, pub impulse: N, - pub total_impulse: N, + pub impulse_accumulator: N, pub r: N, } @@ -178,11 +192,17 @@ impl TwoBodyConstraintNormalPart { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r: na::zero(), } } + /// Total impulse applied across all the solver substeps. + #[inline] + pub fn total_impulse(&self) -> N { + self.impulse_accumulator + self.impulse + } + #[inline] pub fn solve( &mut self, 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 ee176d6..2de6ee2 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs @@ -138,7 +138,7 @@ impl TwoBodyConstraintBuilderSimd { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: SimdReal::splat(0.0), - total_impulse: SimdReal::splat(0.0), + impulse_accumulator: SimdReal::splat(0.0), r: projected_mass, }; } @@ -241,19 +241,18 @@ impl TwoBodyConstraintBuilderSimd { .simd_clamp(-max_penetration_correction, SimdReal::zero()) * erp_inv_dt; 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 * dt).simd_gt(ccd_thickness * SimdReal::splat(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_accumulator += element.normal_part.impulse; element.normal_part.impulse = na::zero(); } // tangent parts. { - element.tangent_part.total_impulse += element.tangent_part.impulse; + element.tangent_part.impulse_accumulator += element.tangent_part.impulse; element.tangent_part.impulse = na::zero(); for j in 0..DIM - 1 { @@ -328,11 +327,12 @@ impl TwoBodyConstraintSimd { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { - let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + 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.impulse[0].into(); + 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.impulse; + 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]]; -- cgit From 996400726927fb952999afbc36db6e2bfba7d44e Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 7 Apr 2024 22:16:58 +0200 Subject: feat: add some additional perf counters --- src/dynamics/solver/island_solver.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 2953f98..159bfa7 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -41,7 +41,7 @@ impl IslandSolver { joint_indices: &[JointIndex], multibodies: &mut MultibodyJointSet, ) { - counters.solver.velocity_resolution_time.resume(); + counters.solver.velocity_assembly_time.resume(); let num_solver_iterations = base_params.num_solver_iterations.get() + islands.active_island_additional_solver_iterations(island_id); @@ -76,8 +76,10 @@ impl IslandSolver { &mut self.contact_constraints, &mut self.joint_constraints, ); + counters.solver.velocity_assembly_time.pause(); // SOLVE + counters.solver.velocity_resolution_time.resume(); self.velocity_solver.solve_constraints( ¶ms, num_solver_iterations, @@ -86,8 +88,10 @@ impl IslandSolver { &mut self.contact_constraints, &mut self.joint_constraints, ); + counters.solver.velocity_resolution_time.pause(); // WRITEBACK + counters.solver.velocity_writeback_time.resume(); self.joint_constraints.writeback_impulses(impulse_joints); self.contact_constraints.writeback_impulses(manifolds); self.velocity_solver.writeback_bodies( @@ -98,6 +102,6 @@ impl IslandSolver { bodies, multibodies, ); - counters.solver.velocity_resolution_time.pause(); + counters.solver.velocity_writeback_time.pause(); } } -- cgit From 3ddf2441ea6c43aa98718e0ce8650c3b804062d4 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 14 Apr 2024 15:53:35 +0200 Subject: feat: add exact mlcp solver for pais of 2 constraints --- .../generic_one_body_constraint.rs | 1 + .../generic_two_body_constraint.rs | 1 + .../contact_constraint/one_body_constraint.rs | 52 +++++++- .../one_body_constraint_element.rs | 97 +++++++++++++- .../contact_constraint/one_body_constraint_simd.rs | 44 +++++++ .../contact_constraint/two_body_constraint.rs | 47 ++++++- .../two_body_constraint_element.rs | 144 ++++++++++++++++++++- .../contact_constraint/two_body_constraint_simd.rs | 50 +++++++ src/dynamics/solver/solver_vel.rs | 9 +- 9 files changed, 424 insertions(+), 21 deletions(-) (limited to 'src/dynamics/solver') 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 cfdff55..d99ad60 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -155,6 +155,7 @@ impl GenericOneBodyConstraintBuilder { impulse: na::zero(), impulse_accumulator: na::zero(), r, + r_mat_elts: [0.0; 2], }; } 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 f0a7d64..4fa8694 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -204,6 +204,7 @@ impl GenericTwoBodyConstraintBuilder { impulse_accumulator: na::zero(), impulse: na::zero(), r, + r_mat_elts: [0.0; 2], }; } diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index c9b35f3..357f1c4 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}; @@ -126,14 +128,15 @@ impl OneBodyConstraintBuilder { // Normal part. let normal_rhs_wo_bias; { - let gcross2 = mprops2 + let mut gcross2 = mprops2 .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; @@ -151,6 +154,7 @@ impl OneBodyConstraintBuilder { impulse: na::zero(), impulse_accumulator: na::zero(), r: projected_mass, + r_mat_elts: [0.0; 2], }; } @@ -205,6 +209,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]; + } + } + } } } 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 79b207b..e5ae140 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -1,6 +1,11 @@ +use crate::dynamics::integration_parameters::{ + BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY, +}; +use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart; use crate::dynamics::solver::SolverVel; use crate::math::{AngVector, Vector, DIM}; use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; +use na::Vector2; #[derive(Copy, Clone, Debug)] pub(crate) struct OneBodyConstraintTangentPart { @@ -60,6 +65,10 @@ impl OneBodyConstraintTangentPart { ) where AngVector: SimdDot, Result = N>, { + if DISABLE_FRICTION_LIMIT_REAPPLY { + return; + } + #[cfg(feature = "dim2")] { let new_impulse = self.impulse[0].simd_clamp(-limit, limit); @@ -153,6 +162,7 @@ pub(crate) struct OneBodyConstraintNormalPart { pub impulse: N, pub impulse_accumulator: N, pub r: N, + pub r_mat_elts: [N; 2], } impl OneBodyConstraintNormalPart { @@ -164,6 +174,7 @@ impl OneBodyConstraintNormalPart { impulse: na::zero(), impulse_accumulator: na::zero(), r: na::zero(), + r_mat_elts: [N::zero(); 2], } } @@ -192,6 +203,44 @@ impl OneBodyConstraintNormalPart { solver_vel2.linear += dir1.component_mul(im2) * -dlambda; solver_vel2.angular += self.gcross2 * dlambda; } + + #[inline] + pub fn solve_pair( + constraint_a: &mut Self, + constraint_b: &mut Self, + cfm_factor: N, + dir1: &Vector, + im2: &Vector, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + let dvel_a = -dir1.dot(&solver_vel2.linear) + + constraint_a.gcross2.gdot(solver_vel2.angular) + + constraint_a.rhs; + let dvel_b = -dir1.dot(&solver_vel2.linear) + + constraint_b.gcross2.gdot(solver_vel2.angular) + + constraint_b.rhs; + + let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse); + let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints( + Vector2::new(dvel_a, dvel_b), + prev_impulse, + constraint_a.r, + constraint_b.r, + constraint_a.r_mat_elts, + constraint_b.r_mat_elts, + cfm_factor, + ); + + let dlambda = new_impulse - prev_impulse; + + constraint_a.impulse = new_impulse.x; + constraint_b.impulse = new_impulse.y; + + solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y); + solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y; + } } #[derive(Copy, Clone, Debug)] @@ -230,13 +279,47 @@ impl OneBodyConstraintElement { // Solve penetration. if solve_normal { - 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); + if BLOCK_SOLVER_ENABLED { + for elements in elements.chunks_exact_mut(2) { + let [element_a, element_b] = elements else { + unreachable!() + }; + + OneBodyConstraintNormalPart::solve_pair( + &mut element_a.normal_part, + &mut element_b.normal_part, + cfm_factor, + dir1, + 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 { + let element = elements.last_mut().unwrap(); + 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 03c1abe..8819cea 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs @@ -1,4 +1,5 @@ use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::solver_body::SolverBody; use crate::dynamics::solver::{ContactPointInfos, SolverVel}; use crate::dynamics::{ @@ -15,6 +16,7 @@ use crate::utils::SimdBasis; use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot}; use num::Zero; use parry::math::SimdBool; +use parry::utils::SdpMatrix2; use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] @@ -156,6 +158,7 @@ impl SimdOneBodyConstraintBuilder { impulse: na::zero(), impulse_accumulator: na::zero(), r: projected_mass, + r_mat_elts: [SimdReal::zero(); 2], }; } @@ -200,6 +203,47 @@ impl SimdOneBodyConstraintBuilder { builder.infos[k] = infos; } } + + if BLOCK_SOLVER_ENABLED { + // Coupling between consecutive pairs. + for k in 0..num_points / 2 { + let k0 = k * 2; + let k1 = k * 2 + 1; + + let r0 = constraint.elements[k0].normal_part.r; + let r1 = constraint.elements[k1].normal_part.r; + + let mut r_mat = SdpMatrix2::zero(); + r_mat.m12 = force_dir1.dot(&im2.component_mul(&force_dir1)) + + constraint.elements[k0] + .normal_part + .gcross2 + .gdot(constraint.elements[k1].normal_part.gcross2); + r_mat.m11 = utils::simd_inv(r0); + r_mat.m22 = utils::simd_inv(r1); + + let (inv, det) = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + r_mat.inverse_and_get_determinant_unchecked() + }; + let is_invertible = det.simd_gt(SimdReal::zero()); + + // 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 = [ + inv.m11.select(is_invertible, r0), + inv.m22.select(is_invertible, SimdReal::zero()), + ]; + constraint.elements[k1].normal_part.r_mat_elts = [ + inv.m12.select(is_invertible, SimdReal::zero()), + r_mat.m12.select(is_invertible, SimdReal::zero()), + ]; + } + } } } diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs index 8abf5c5..24a5730 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs @@ -2,11 +2,13 @@ use super::{ContactConstraintTypes, ContactPointInfos}; use crate::dynamics::solver::SolverVel; use crate::dynamics::solver::{AnyConstraintMut, SolverBody}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot}; -use na::DVector; +use na::{DVector, Matrix2}; +use num::Pow; use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; @@ -225,6 +227,7 @@ impl TwoBodyConstraintBuilder { impulse: na::zero(), impulse_accumulator: na::zero(), r: projected_mass, + r_mat_elts: [0.0; 2], }; } @@ -284,6 +287,48 @@ impl TwoBodyConstraintBuilder { builder.infos[k] = infos; constraint.manifold_contact_id[k] = manifold_point.contact_id; } + + 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 imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; + let r0 = constraint.elements[k0].normal_part.r; + let r1 = constraint.elements[k1].normal_part.r; + r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) + + constraint.elements[k0] + .normal_part + .gcross1 + .gdot(constraint.elements[k1].normal_part.gcross1) + + 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]; + } + } + } } } 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 75fc739..e5cd6d5 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs @@ -1,6 +1,13 @@ +use crate::dynamics::integration_parameters::{ + BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY, +}; +use crate::dynamics::solver::contact_constraint::OneBodyConstraintNormalPart; use crate::dynamics::solver::SolverVel; use crate::math::{AngVector, Vector, DIM}; use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; +use na::{Matrix2, Vector2}; +use num::Zero; +use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] pub(crate) struct TwoBodyConstraintTangentPart { @@ -64,6 +71,10 @@ impl TwoBodyConstraintTangentPart { ) where AngVector: SimdDot, Result = N>, { + if DISABLE_FRICTION_LIMIT_REAPPLY { + return; + } + #[cfg(feature = "dim2")] { let new_impulse = self.impulse[0].simd_clamp(-limit, limit); @@ -182,6 +193,11 @@ pub(crate) struct TwoBodyConstraintNormalPart { pub impulse: N, pub impulse_accumulator: N, pub r: N, + // For coupled constraint pairs, even constraints store the + // diagonal of the projected mass matrix. Odd constraints + // store the off-diagonal element of the projected mass matrix, + // as well as the off-diagonal element of the inverse projected mass matrix. + pub r_mat_elts: [N; 2], } impl TwoBodyConstraintNormalPart { @@ -194,6 +210,7 @@ impl TwoBodyConstraintNormalPart { impulse: na::zero(), impulse_accumulator: na::zero(), r: na::zero(), + r_mat_elts: [N::zero(); 2], } } @@ -229,6 +246,83 @@ impl TwoBodyConstraintNormalPart { solver_vel2.linear += dir1.component_mul(im2) * -dlambda; solver_vel2.angular += self.gcross2 * dlambda; } + + #[inline(always)] + pub(crate) fn solve_mlcp_two_constraints( + dvel: Vector2, + prev_impulse: Vector2, + r_a: N, + r_b: N, + [r_mat11, r_mat22]: [N; 2], + [r_mat12, r_mat_inv12]: [N; 2], + cfm_factor: N, + ) -> Vector2 { + let r_dvel = Vector2::new( + r_mat11 * dvel.x + r_mat12 * dvel.y, + r_mat12 * dvel.x + r_mat22 * dvel.y, + ); + let new_impulse0 = prev_impulse - r_dvel; + let new_impulse1 = Vector2::new(prev_impulse.x - r_a * dvel.x, N::zero()); + let new_impulse2 = Vector2::new(N::zero(), prev_impulse.y - r_b * dvel.y); + let new_impulse3 = Vector2::new(N::zero(), N::zero()); + + let keep0 = new_impulse0.x.simd_ge(N::zero()) & new_impulse0.y.simd_ge(N::zero()); + let keep1 = new_impulse1.x.simd_ge(N::zero()) + & (dvel.y + r_mat_inv12 * new_impulse1.x).simd_ge(N::zero()); + let keep2 = new_impulse2.y.simd_ge(N::zero()) + & (dvel.x + r_mat_inv12 * new_impulse2.y).simd_ge(N::zero()); + let keep3 = dvel.x.simd_ge(N::zero()) & dvel.y.simd_ge(N::zero()); + + let selected3 = (new_impulse3 * cfm_factor).select(keep3, prev_impulse); + let selected2 = (new_impulse2 * cfm_factor).select(keep2, selected3); + let selected1 = (new_impulse1 * cfm_factor).select(keep1, selected2); + (new_impulse0 * cfm_factor).select(keep0, selected1) + } + + #[inline] + pub fn solve_pair( + constraint_a: &mut Self, + constraint_b: &mut Self, + cfm_factor: N, + dir1: &Vector, + im1: &Vector, + im2: &Vector, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + let dvel_lin = dir1.dot(&solver_vel1.linear) - dir1.dot(&solver_vel2.linear); + let dvel_a = dvel_lin + + constraint_a.gcross1.gdot(solver_vel1.angular) + + constraint_a.gcross2.gdot(solver_vel2.angular) + + constraint_a.rhs; + let dvel_b = dvel_lin + + constraint_b.gcross1.gdot(solver_vel1.angular) + + constraint_b.gcross2.gdot(solver_vel2.angular) + + constraint_b.rhs; + + let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse); + let new_impulse = Self::solve_mlcp_two_constraints( + Vector2::new(dvel_a, dvel_b), + prev_impulse, + constraint_a.r, + constraint_b.r, + constraint_a.r_mat_elts, + constraint_b.r_mat_elts, + cfm_factor, + ); + + let dlambda = new_impulse - prev_impulse; + + constraint_a.impulse = new_impulse.x; + constraint_b.impulse = new_impulse.y; + + solver_vel1.linear += dir1.component_mul(im1) * (dlambda.x + dlambda.y); + solver_vel1.angular += constraint_a.gcross1 * dlambda.x + constraint_b.gcross1 * dlambda.y; + solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y); + solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y; + } } #[derive(Copy, Clone, Debug)] @@ -269,13 +363,49 @@ impl TwoBodyConstraintElement { // Solve penetration. if solve_normal { - 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); + if BLOCK_SOLVER_ENABLED { + for elements in elements.chunks_exact_mut(2) { + let [element_a, element_b] = elements else { + unreachable!() + }; + + TwoBodyConstraintNormalPart::solve_pair( + &mut element_a.normal_part, + &mut element_b.normal_part, + cfm_factor, + dir1, + im1, + im2, + 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. + if elements.len() % 2 == 1 { + let element = elements.last_mut().unwrap(); + 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); + } } } 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 2de6ee2..9b05fd3 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs +++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs @@ -1,4 +1,5 @@ use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::solver_body::SolverBody; use crate::dynamics::solver::{ContactPointInfos, SolverVel}; use crate::dynamics::{ @@ -13,8 +14,10 @@ use crate::math::{ #[cfg(feature = "dim2")] use crate::utils::SimdBasis; use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot}; +use na::Matrix2; use num::Zero; use parry::math::SimdBool; +use parry::utils::SdpMatrix2; use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] @@ -140,6 +143,7 @@ impl TwoBodyConstraintBuilderSimd { impulse: SimdReal::splat(0.0), impulse_accumulator: SimdReal::splat(0.0), r: projected_mass, + r_mat_elts: [SimdReal::zero(); 2], }; } @@ -186,6 +190,52 @@ impl TwoBodyConstraintBuilderSimd { builder.infos[k] = infos; } + + if BLOCK_SOLVER_ENABLED { + // Coupling between consecutive pairs. + for k in 0..num_points / 2 { + let k0 = k * 2; + let k1 = k * 2 + 1; + + let imsum = im1 + im2; + let r0 = constraint.elements[k0].normal_part.r; + let r1 = constraint.elements[k1].normal_part.r; + + let mut r_mat = SdpMatrix2::zero(); + r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) + + constraint.elements[k0] + .normal_part + .gcross1 + .gdot(constraint.elements[k1].normal_part.gcross1) + + constraint.elements[k0] + .normal_part + .gcross2 + .gdot(constraint.elements[k1].normal_part.gcross2); + r_mat.m11 = utils::simd_inv(r0); + r_mat.m22 = utils::simd_inv(r1); + + let (inv, det) = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + r_mat.inverse_and_get_determinant_unchecked() + }; + let is_invertible = det.simd_gt(SimdReal::zero()); + + // 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 = [ + inv.m11.select(is_invertible, r0), + inv.m22.select(is_invertible, SimdReal::zero()), + ]; + constraint.elements[k1].normal_part.r_mat_elts = [ + inv.m12.select(is_invertible, SimdReal::zero()), + r_mat.m12.select(is_invertible, SimdReal::zero()), + ]; + } + } } } diff --git a/src/dynamics/solver/solver_vel.rs b/src/dynamics/solver/solver_vel.rs index 48c76b8..da69fb8 100644 --- a/src/dynamics/solver/solver_vel.rs +++ b/src/dynamics/solver/solver_vel.rs @@ -1,7 +1,7 @@ use crate::math::{AngVector, Vector, SPATIAL_DIM}; use crate::utils::SimdRealCopy; use na::{DVectorView, DVectorViewMut, Scalar}; -use std::ops::{AddAssign, Sub}; +use std::ops::{AddAssign, Sub, SubAssign}; #[derive(Copy, Clone, Debug, Default)] #[repr(C)] @@ -47,6 +47,13 @@ impl AddAssign for SolverVel { } } +impl SubAssign for SolverVel { + fn sub_assign(&mut self, rhs: Self) { + self.linear -= rhs.linear; + self.angular -= rhs.angular; + } +} + impl Sub for SolverVel { type Output = Self; -- cgit From 9c5c14070d0a0b0283f3943c0f95552c395f2b97 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 14 Apr 2024 15:55:20 +0200 Subject: feat: add variable constraints stabilization iteration number --- src/dynamics/solver/solver_constraints_set.rs | 1 + src/dynamics/solver/velocity_solver.rs | 59 +++++++++++++++++++++++++-- 2 files changed, 57 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/solver_constraints_set.rs b/src/dynamics/solver/solver_constraints_set.rs index 4404241..4259098 100644 --- a/src/dynamics/solver/solver_constraints_set.rs +++ b/src/dynamics/solver/solver_constraints_set.rs @@ -25,6 +25,7 @@ pub(crate) trait ConstraintTypes { type SimdBuilderTwoBodies; } +#[derive(Debug)] pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> { OneBody(&'a mut Constraints::OneBody), TwoBodies(&'a mut Constraints::TwoBodies), diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 928b427..85d2211 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,4 +1,5 @@ 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}, @@ -10,6 +11,7 @@ use crate::math::Real; use crate::prelude::RigidBodyVelocity; use crate::utils::SimdAngularInertia; use na::DVector; +use ordered_float::OrderedFloat; pub(crate) struct VelocitySolver { pub solver_bodies: Vec, @@ -201,9 +203,60 @@ impl VelocitySolver { /* * Resolution without bias. */ - 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); + 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); + 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; + // } + } } } -- cgit 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, +