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') 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 6886f8f2073b3251e3b080523d8032961b68e52a Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 23 Mar 2024 10:51:50 +0100 Subject: feat: add RigidBody::predict_position_using_velocity Fix #601 --- src/dynamics/rigid_body.rs | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index b27b58e..2b07f37 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -817,6 +817,17 @@ impl RigidBody { .integrate_forces_and_velocities(dt, &self.forces, &self.vels, &self.mprops) } + /// Predicts the next position of this rigid-body, by integrating only its velocity + /// by a time of `dt`. + /// + /// The forces that were applied to this rigid-body since the last physics step will + /// be ignored by this function. Use [`Self::predict_position_using_velocity_and_forces`] + /// instead to take forces into account. + pub fn predict_position_using_velocity(&self, dt: Real) -> Isometry { + self.vels + .integrate(dt, &self.pos.position, &self.mprops.local_mprops.local_com) + } + pub(crate) fn update_world_mass_properties(&mut self) { self.mprops.update_world_mass_properties(&self.pos.position); } -- cgit From cd9fb8342d115bad557ca05aa16fb961fe28b1b5 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 23 Mar 2024 13:11:37 +0100 Subject: feat: add RigidBody::copy_from and Collider::copy_from MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Closes #595 --- src/dynamics/rigid_body.rs | 55 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 2b07f37..5af4131 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -74,6 +74,61 @@ impl RigidBody { self.ids = Default::default(); } + /// Copy all the characteristics from `other` to `self`. + /// + /// If you have a mutable reference to a rigid-body `rigid_body: &mut RigidBody`, attempting to + /// assign it a whole new rigid-body instance, e.g., `*rigid_body = RigidBodyBuilder::dynamic().build()`, + /// will crash due to some internal indices being overwritten. Instead, use + /// `rigid_body.copy_from(&RigidBodyBuilder::dynamic().build())`. + /// + /// This method will allow you to set most characteristics of this rigid-body from another + /// rigid-body instance without causing any breakage. + /// + /// This method **cannot** be used for editing the list of colliders attached to this rigid-body. + /// Therefore, the list of colliders attached to `self` won’t be replaced by the one attached + /// to `other`. + /// + /// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has + /// a parent, its position is directly controlled by the parent rigid-body). + pub fn copy_from(&mut self, other: &RigidBody) { + // NOTE: we deconstruct the rigid-body struct to be sure we don’t forget to + // add some copies here if we add more field to RigidBody in the future. + let RigidBody { + pos, + mprops, + integrated_vels, + vels, + damping, + forces, + ccd, + ids: _ids, // Internal ids must not be overwritten. + colliders: _colliders, // This function cannot be used to edit collider sets. + activation, + changes: _changes, // Will be set to ALL. + body_type, + dominance, + enabled, + additional_solver_iterations, + user_data, + } = other; + + self.pos = *pos; + self.mprops = mprops.clone(); + self.integrated_vels = *integrated_vels; + self.vels = *vels; + self.damping = *damping; + self.forces = *forces; + self.ccd = *ccd; + self.activation = *activation; + self.body_type = *body_type; + self.dominance = *dominance; + self.enabled = *enabled; + self.additional_solver_iterations = *additional_solver_iterations; + self.user_data = *user_data; + + self.changes = RigidBodyChanges::all(); + } + /// Set the additional number of solver iterations run for this rigid-body and /// everything interacting with it. /// -- 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') 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') 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 --- src/dynamics/integration_parameters.rs | 8 +- .../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 +- 10 files changed, 431 insertions(+), 22 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 13b3fde..2788349 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,6 +1,9 @@ use crate::math::Real; use std::num::NonZeroUsize; +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)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -50,6 +53,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, /// 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`). @@ -194,7 +199,7 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.6, + erp: 0.8, damping_ratio: 1.0, joint_erp: 1.0, joint_damping_ratio: 1.0, @@ -202,6 +207,7 @@ impl Default for IntegrationParameters { max_penetration_correction: Real::MAX, prediction_distance: 0.002, num_internal_pgs_iterations: 1, + max_internal_stabilization_iterations: 10, num_additional_friction_iterations: 4, num_solver_iterations: NonZeroUsize::new(4).unwrap(), // TODO: what is the optimal value for min_island_size? 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/so