From 9b87f06a856c4d673642e210f8b0986cfdbac3af Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 21 Jan 2024 21:02:23 +0100 Subject: feat: implement new "small-steps" solver + joint improvements --- src/dynamics/solver/categorization.rs | 32 +- .../contact_constraint/contact_constraints_set.rs | 528 ++++++++ .../generic_one_body_constraint.rs | 292 ++++ .../generic_one_body_constraint_element.rs | 147 ++ .../generic_two_body_constraint.rs | 433 ++++++ .../generic_two_body_constraint_element.rs | 367 +++++ src/dynamics/solver/contact_constraint/mod.rs | 29 + .../contact_constraint/one_body_constraint.rs | 382 ++++++ .../one_body_constraint_element.rs | 232 ++++ .../contact_constraint/one_body_constraint_simd.rs | 372 ++++++ .../contact_constraint/two_body_constraint.rs | 470 +++++++ .../two_body_constraint_element.rs | 271 ++++ .../contact_constraint/two_body_constraint_simd.rs | 370 ++++++ src/dynamics/solver/delta_vel.rs | 57 - src/dynamics/solver/generic_velocity_constraint.rs | 378 ------ .../solver/generic_velocity_constraint_element.rs | 349 ----- .../solver/generic_velocity_ground_constraint.rs | 240 ---- .../generic_velocity_ground_constraint_element.rs | 143 -- src/dynamics/solver/interaction_groups.rs | 18 +- src/dynamics/solver/island_solver.rs | 93 +- .../joint_constraint/any_joint_constraint.rs | 97 ++ .../solver/joint_constraint/joint_constraint.rs | 540 -------- .../joint_constraint/joint_constraint_builder.rs | 1401 ++++++++++++++++++++ .../joint_constraint/joint_constraints_set.rs | 446 +++++++ .../joint_constraint/joint_generic_constraint.rs | 552 ++++++++ .../joint_generic_constraint_builder.rs | 1286 ++++++++++++++++++ .../joint_generic_velocity_constraint.rs | 562 -------- .../joint_generic_velocity_constraint_builder.rs | 886 ------------- .../joint_constraint/joint_velocity_constraint.rs | 387 +++--- .../joint_velocity_constraint_builder.rs | 1127 ---------------- src/dynamics/solver/joint_constraint/mod.rs | 23 +- src/dynamics/solver/mod.rs | 81 +- src/dynamics/solver/parallel_island_solver.rs | 34 +- src/dynamics/solver/parallel_solver_constraints.rs | 169 ++- src/dynamics/solver/parallel_velocity_solver.rs | 88 +- src/dynamics/solver/solver_body.rs | 59 + src/dynamics/solver/solver_constraints.rs | 564 -------- src/dynamics/solver/solver_constraints_set.rs | 241 ++++ src/dynamics/solver/solver_vel.rs | 59 + src/dynamics/solver/velocity_constraint.rs | 441 ------ src/dynamics/solver/velocity_constraint_element.rs | 211 --- src/dynamics/solver/velocity_constraint_wide.rs | 294 ---- src/dynamics/solver/velocity_ground_constraint.rs | 281 ---- .../solver/velocity_ground_constraint_element.rs | 181 --- .../solver/velocity_ground_constraint_wide.rs | 277 ---- src/dynamics/solver/velocity_solver.rs | 406 +++--- 46 files changed, 8762 insertions(+), 7134 deletions(-) create mode 100644 src/dynamics/solver/contact_constraint/contact_constraints_set.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/mod.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs delete mode 100644 src/dynamics/solver/delta_vel.rs delete mode 100644 src/dynamics/solver/generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/generic_velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/generic_velocity_ground_constraint.rs delete mode 100644 src/dynamics/solver/generic_velocity_ground_constraint_element.rs create mode 100644 src/dynamics/solver/joint_constraint/any_joint_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_builder.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraints_set.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs create mode 100644 src/dynamics/solver/solver_body.rs delete mode 100644 src/dynamics/solver/solver_constraints.rs create mode 100644 src/dynamics/solver/solver_constraints_set.rs create mode 100644 src/dynamics/solver/solver_vel.rs delete mode 100644 src/dynamics/solver/velocity_constraint.rs delete mode 100644 src/dynamics/solver/velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint_element.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint_wide.rs (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index d110971..cef9c9e 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -6,10 +6,10 @@ pub(crate) fn categorize_contacts( multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - out_ground: &mut Vec, - out_not_ground: &mut Vec, - out_generic_ground: &mut Vec, - out_generic_not_ground: &mut Vec, + out_one_body: &mut Vec, + out_two_body: &mut Vec, + out_generic_one_body: &mut Vec, + out_generic_two_body: &mut Vec, ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; @@ -26,14 +26,14 @@ pub(crate) fn categorize_contacts( .is_some() { if manifold.data.relative_dominance != 0 { - out_generic_ground.push(*manifold_i); + out_generic_one_body.push(*manifold_i); } else { - out_generic_not_ground.push(*manifold_i); + out_generic_two_body.push(*manifold_i); } } else if manifold.data.relative_dominance != 0 { - out_ground.push(*manifold_i) + out_one_body.push(*manifold_i) } else { - out_not_ground.push(*manifold_i) + out_two_body.push(*manifold_i) } } } @@ -43,10 +43,10 @@ pub(crate) fn categorize_joints( multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], - ground_joints: &mut Vec, - nonground_joints: &mut Vec, - generic_ground_joints: &mut Vec, - generic_nonground_joints: &mut Vec, + one_body_joints: &mut Vec, + two_body_joints: &mut Vec, + generic_one_body_joints: &mut Vec, + generic_two_body_joints: &mut Vec, ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; @@ -57,14 +57,14 @@ pub(crate) fn categorize_joints( || multibody_joints.rigid_body_link(joint.body2).is_some() { if !rb1.is_dynamic() || !rb2.is_dynamic() { - generic_ground_joints.push(*joint_i); + generic_one_body_joints.push(*joint_i); } else { - generic_nonground_joints.push(*joint_i); + generic_two_body_joints.push(*joint_i); } } else if !rb1.is_dynamic() || !rb2.is_dynamic() { - ground_joints.push(*joint_i); + one_body_joints.push(*joint_i); } else { - nonground_joints.push(*joint_i); + two_body_joints.push(*joint_i); } } } diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs new file mode 100644 index 0000000..4d02270 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -0,0 +1,528 @@ +use crate::dynamics::solver::categorization::categorize_contacts; +use crate::dynamics::solver::contact_constraint::{ + GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint, + GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder, + TwoBodyConstraint, TwoBodyConstraintBuilder, +}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::solver_vel::SolverVel; +use crate::dynamics::solver::{reset_buffer, ConstraintTypes, SolverConstraintsSet}; +use crate::dynamics::{ + ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet, + RigidBodySet, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Real, MAX_MANIFOLD_POINTS}; +use na::DVector; +use parry::math::DIM; + +#[cfg(feature = "simd-is-enabled")] +use { + crate::dynamics::solver::contact_constraint::{ + OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd, + TwoBodyConstraintSimd, + }, + crate::math::SIMD_WIDTH, +}; + +pub struct ConstraintsCounts { + pub num_constraints: usize, + pub num_jacobian_lines: usize, +} + +impl ConstraintsCounts { + pub fn from_contacts(manifold: &ContactManifold) -> Self { + let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; + Self { + num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + + rest as usize, + num_jacobian_lines: manifold.data.solver_contacts.len() * DIM, + } + } + + pub fn from_joint(joint: &ImpulseJoint) -> Self { + let joint = &joint.data; + let locked_axes = joint.locked_axes.bits(); + let motor_axes = joint.motor_axes.bits() & !locked_axes; + let limit_axes = joint.limit_axes.bits() & !locked_axes; + let coupled_axes = joint.coupled_axes.bits(); + + let num_constraints = (motor_axes & !coupled_axes).count_ones() as usize + + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize + + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize + + locked_axes.count_ones() as usize + + (limit_axes & !coupled_axes).count_ones() as usize + + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize + + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize; + Self { + num_constraints, + num_jacobian_lines: num_constraints, + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct ContactConstraintTypes; + +impl ConstraintTypes for ContactConstraintTypes { + type OneBody = OneBodyConstraint; + type TwoBodies = TwoBodyConstraint; + type GenericOneBody = GenericOneBodyConstraint; + type GenericTwoBodies = GenericTwoBodyConstraint; + #[cfg(feature = "simd-is-enabled")] + type SimdOneBody = OneBodyConstraintSimd; + #[cfg(feature = "simd-is-enabled")] + type SimdTwoBodies = TwoBodyConstraintSimd; + + type BuilderOneBody = OneBodyConstraintBuilder; + type BuilderTwoBodies = TwoBodyConstraintBuilder; + type GenericBuilderOneBody = GenericOneBodyConstraintBuilder; + type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderOneBody = SimdOneBodyConstraintBuilder; + #[cfg(feature = "simd-is-enabled")] + type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd; +} + +pub type ContactConstraintsSet = SolverConstraintsSet; + +impl ContactConstraintsSet { + pub fn init_constraint_groups( + &mut self, + island_id: usize, + islands: &IslandManager, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.two_body_interactions.clear(); + self.one_body_interactions.clear(); + self.generic_two_body_interactions.clear(); + self.generic_one_body_interactions.clear(); + + categorize_contacts( + bodies, + multibody_joints, + manifolds, + manifold_indices, + &mut self.one_body_interactions, + &mut self.two_body_interactions, + &mut self.generic_one_body_interactions, + &mut self.generic_two_body_interactions, + ); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_manifolds( + island_id, + islands, + bodies, + manifolds, + &self.two_body_interactions, + ); + + self.one_body_interaction_groups.clear_groups(); + self.one_body_interaction_groups.group_manifolds( + island_id, + islands, + bodies, + manifolds, + &self.one_body_interactions, + ); + + // NOTE: uncomment this do disable SIMD contact resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.simd_interactions); + // self.one_body_interaction_groups + // .nongrouped_interactions + // .append(&mut self.one_body_interaction_groups.simd_interactions); + } + + pub fn init( + &mut self, + island_id: usize, + islands: &IslandManager, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.clear_constraints(); + self.clear_builders(); + + self.init_constraint_groups( + island_id, + islands, + bodies, + multibody_joints, + manifolds, + manifold_indices, + ); + + let mut jacobian_id = 0; + + #[cfg(feature = "simd-is-enabled")] + { + self.simd_compute_constraints(bodies, manifolds); + } + self.compute_constraints(bodies, manifolds); + self.compute_generic_constraints(bodies, multibody_joints, manifolds, &mut jacobian_id); + + #[cfg(feature = "simd-is-enabled")] + { + self.simd_compute_one_body_constraints(bodies, manifolds); + } + self.compute_one_body_constraints(bodies, manifolds); + self.compute_generic_one_body_constraints( + bodies, + multibody_joints, + manifolds, + &mut jacobian_id, + ); + } + + #[cfg(feature = "simd-is-enabled")] + fn simd_compute_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.simd_velocity_constraints_builder, + total_num_constraints, + ); + reset_buffer(&mut self.simd_velocity_constraints, total_num_constraints); + } + + let mut curr_start = 0; + + for manifolds_i in self + .interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + { + let num_to_add = + ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints; + let manifold_id = gather![|ii| manifolds_i[ii]]; + let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; + + TwoBodyConstraintBuilderSimd::generate( + manifold_id, + manifolds, + bodies, + &mut self.simd_velocity_constraints_builder[curr_start..], + &mut self.simd_velocity_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .interaction_groups + .nongrouped_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.velocity_constraints_builder, + total_num_constraints, + ); + reset_buffer(&mut self.velocity_constraints, total_num_constraints); + } + + let mut curr_start = 0; + + for manifold_i in &self.interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + TwoBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + &mut self.velocity_constraints_builder[curr_start..], + &mut self.velocity_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_generic_constraints( + &mut self, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds_all: &[&mut ContactManifold], + jacobian_id: &mut usize, + ) { + let total_num_constraints = self + .generic_two_body_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + + self.generic_velocity_constraints_builder.resize( + total_num_constraints, + GenericTwoBodyConstraintBuilder::invalid(), + ); + self.generic_velocity_constraints + .resize(total_num_constraints, GenericTwoBodyConstraint::invalid()); + + let mut curr_start = 0; + + for manifold_i in &self.generic_two_body_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + GenericTwoBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + multibody_joints, + &mut self.generic_velocity_constraints_builder[curr_start..], + &mut self.generic_velocity_constraints[curr_start..], + &mut self.generic_jacobians, + jacobian_id, + ); + + curr_start += num_to_add; + } + + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_generic_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + multibody_joints: &MultibodyJointSet, + manifolds_all: &[&mut ContactManifold], + jacobian_id: &mut usize, + ) { + let total_num_constraints = self + .generic_one_body_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + self.generic_velocity_one_body_constraints_builder.resize( + total_num_constraints, + GenericOneBodyConstraintBuilder::invalid(), + ); + self.generic_velocity_one_body_constraints + .resize(total_num_constraints, GenericOneBodyConstraint::invalid()); + + let mut curr_start = 0; + + for manifold_i in &self.generic_one_body_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + GenericOneBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + multibody_joints, + &mut self.generic_velocity_one_body_constraints_builder[curr_start..], + &mut self.generic_velocity_one_body_constraints[curr_start..], + &mut self.generic_jacobians, + jacobian_id, + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + #[cfg(feature = "simd-is-enabled")] + fn simd_compute_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .one_body_interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[i[0]]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.simd_velocity_one_body_constraints_builder, + total_num_constraints, + ); + reset_buffer( + &mut self.simd_velocity_one_body_constraints, + total_num_constraints, + ); + } + + let mut curr_start = 0; + + for manifolds_i in self + .one_body_interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + { + let num_to_add = + ConstraintsCounts::from_contacts(&manifolds_all[manifolds_i[0]]).num_constraints; + let manifold_id = gather![|ii| manifolds_i[ii]]; + let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; + SimdOneBodyConstraintBuilder::generate( + manifold_id, + manifolds, + bodies, + &mut self.simd_velocity_one_body_constraints_builder[curr_start..], + &mut self.simd_velocity_one_body_constraints[curr_start..], + ); + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + fn compute_one_body_constraints( + &mut self, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let total_num_constraints = self + .one_body_interaction_groups + .nongrouped_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(&manifolds_all[*i]).num_constraints) + .sum(); + + unsafe { + reset_buffer( + &mut self.velocity_one_body_constraints_builder, + total_num_constraints, + ); + reset_buffer( + &mut self.velocity_one_body_constraints, + total_num_constraints, + ); + } + + let mut curr_start = 0; + + for manifold_i in &self.one_body_interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + + OneBodyConstraintBuilder::generate( + *manifold_i, + manifold, + bodies, + &mut self.velocity_one_body_constraints_builder[curr_start..], + &mut self.velocity_one_body_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); + } + + pub fn solve_restitution( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.solve_restitution(jac, solver_vels, generic_solver_vels); + } + } + + pub fn solve_restitution_wo_bias( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.remove_bias(); + c.solve_restitution(jac, solver_vels, generic_solver_vels); + } + } + + pub fn solve_friction( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.solve_friction(jac, solver_vels, generic_solver_vels); + } + } + + pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) { + let (_, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.writeback_impulses(manifolds_all); + } + } + + pub fn update( + &mut self, + params: &IntegrationParameters, + small_step_id: usize, + multibodies: &MultibodyJointSet, + solver_bodies: &[SolverBody], + ) { + macro_rules! update_contacts( + ($builders: ident, $constraints: ident) => { + for (builder, constraint) in self.$builders.iter().zip(self.$constraints.iter_mut()) { + builder.update( + ¶ms, + small_step_id as Real * params.dt, + solver_bodies, + multibodies, + constraint, + ); + } + } + ); + + update_contacts!( + generic_velocity_constraints_builder, + generic_velocity_constraints + ); + update_contacts!(velocity_constraints_builder, velocity_constraints); + #[cfg(feature = "simd-is-enabled")] + update_contacts!(simd_velocity_constraints_builder, simd_velocity_constraints); + + update_contacts!( + generic_velocity_one_body_constraints_builder, + generic_velocity_one_body_constraints + ); + update_contacts!( + velocity_one_body_constraints_builder, + velocity_one_body_constraints + ); + #[cfg(feature = "simd-is-enabled")] + update_contacts!( + simd_velocity_one_body_constraints_builder, + simd_velocity_one_body_constraints + ); + } +} diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs new file mode 100644 index 0000000..7c39eab --- /dev/null +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -0,0 +1,292 @@ +use crate::dynamics::solver::OneBodyConstraint; +use crate::dynamics::{ + IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::SimdCross; + +use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::{ContactPointInfos, OneBodyConstraintBuilder}; +#[cfg(feature = "dim2")] +use crate::utils::SimdBasis; +use na::DVector; + +#[derive(Copy, Clone)] +pub(crate) struct GenericOneBodyConstraintBuilder { + link2: MultibodyLinkId, + ccd_thickness: Real, + inner: OneBodyConstraintBuilder, +} + +impl GenericOneBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + link2: MultibodyLinkId::default(), + ccd_thickness: 0.0, + inner: OneBodyConstraintBuilder::invalid(), + } + } + + pub fn generate( + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + out_builders: &mut [GenericOneBodyConstraintBuilder], + out_constraints: &mut [GenericOneBodyConstraint], + jacobians: &mut DVector, + jacobian_id: &mut usize, + ) { + let mut handle1 = manifold.data.rigid_body1; + let mut handle2 = manifold.data.rigid_body2; + let flipped = manifold.data.relative_dominance < 0; + + let (force_dir1, flipped_multiplier) = if flipped { + std::mem::swap(&mut handle1, &mut handle2); + (manifold.data.normal, -1.0) + } else { + (-manifold.data.normal, 1.0) + }; + + let (vels1, world_com1) = if let Some(handle1) = handle1 { + let rb1 = &bodies[handle1]; + (rb1.vels, rb1.mprops.world_com) + } else { + (RigidBodyVelocity::zero(), Point::origin()) + }; + + let rb1 = handle1 + .map(|h| SolverBody::from(&bodies[h])) + .unwrap_or_else(SolverBody::default); + + let rb2 = &bodies[handle2.unwrap()]; + let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); + + let link2 = *multibodies.rigid_body_link(handle2.unwrap()).unwrap(); + let (mb2, link_id2) = (&multibodies[link2.multibody], link2.id); + let solver_vel2 = mb2.solver_id; + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); + + let multibodies_ndof = mb2.ndofs(); + // For each solver contact we generate DIM constraints, and each constraints appends + // the multibodies jacobian and weighted jacobians + let required_jacobian_len = + *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; + + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } + + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let chunk_j_id = *jacobian_id; + + let builder = &mut out_builders[l]; + let constraint = &mut out_constraints[l]; + + builder.inner.rb1 = rb1; + builder.inner.vels1 = vels1; + + constraint.inner.dir1 = force_dir1; + constraint.inner.im2 = mprops2.effective_inv_mass; + constraint.inner.solver_vel2 = solver_vel2; + constraint.inner.manifold_id = manifold_id; + constraint.inner.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] + { + constraint.inner.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let point = manifold_point.point; + let dp1 = point - world_com1; + let dp2 = point - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + constraint.inner.limit = manifold_point.friction; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir2 = dp2.gcross(-force_dir1); + let inv_r2 = mb2 + .fill_jacobians( + link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0; + + let r = crate::utils::inv(inv_r2); + + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + + let proj_vel1 = vel1.dot(&force_dir1); + let proj_vel2 = vel2.dot(&force_dir1); + let dvel = proj_vel1 - proj_vel2; + // NOTE: we add proj_vel1 since it’s not accessible through solver_vel. + normal_rhs_wo_bias = + proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel; + + constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart { + gcross2: na::zero(), // Unused for generic constraints. + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse: na::zero(), + total_impulse: na::zero(), + r, + }; + } + + // Tangent parts. + { + constraint.inner.elements[k].tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let torque_dir2 = dp2.gcross(-tangents1[j]); + let inv_r2 = mb2 + .fill_jacobians( + link_id2, + -tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir2], + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0; + + let r = crate::utils::inv(inv_r2); + + let rhs_wo_bias = (vel1 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); + + constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias; + + // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. + constraint.inner.elements[k].tangent_part.r[j] = r; + } + } + + // Builder. + let infos = ContactPointInfos { + local_p1: rb1.position.inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_rhs_wo_bias, + }; + + builder.link2 = link2; + builder.ccd_thickness = rb2.ccd.ccd_thickness; + builder.inner.infos[k] = infos; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + } + + constraint.j_id = chunk_j_id; + constraint.ndofs2 = mb2.ndofs(); + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + _solver_bodies: &[SolverBody], + multibodies: &MultibodyJointSet, + constraint: &mut GenericOneBodyConstraint, + ) { + // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. + let pos2 = &multibodies[self.link2.multibody] + .link(self.link2.id) + .unwrap() + .local_to_world; + + self.inner.update_with_positions( + params, + solved_dt, + pos2, + self.ccd_thickness, + &mut constraint.inner, + ); + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct GenericOneBodyConstraint { + // We just build the generic constraint on top of the velocity constraint, + // adding some information we can use in the generic case. + pub inner: OneBodyConstraint, + pub j_id: usize, + pub ndofs2: usize, +} + +impl GenericOneBodyConstraint { + pub fn invalid() -> Self { + Self { + inner: OneBodyConstraint::invalid(), + j_id: usize::MAX, + ndofs2: usize::MAX, + } + } + + pub fn solve( + &mut self, + jacobians: &DVector, + generic_solver_vels: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let solver_vel2 = self.inner.solver_vel2 as usize; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + OneBodyConstraintElement::generic_solve_group( + self.inner.cfm_factor, + elements, + jacobians, + self.inner.limit, + self.ndofs2, + self.j_id, + solver_vel2, + generic_solver_vels, + solve_restitution, + solve_friction, + ); + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + self.inner.writeback_impulses(manifolds_all); + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.inner.remove_cfm_and_bias_from_rhs(); + } +} diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs new file mode 100644 index 0000000..1003a9a --- /dev/null +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs @@ -0,0 +1,147 @@ +use crate::dynamics::solver::{ + OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart, +}; +use crate::math::{Real, DIM}; +use na::DVector; +#[cfg(feature = "dim2")] +use na::SimdPartialOrd; + +impl OneBodyConstraintTangentPart { + #[inline] + pub fn generic_solve( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + limit: Real, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + #[cfg(feature = "dim2")] + { + let dvel_0 = jacobians + .rows(j_id2, ndofs2) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + + self.rhs[0]; + + let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + dlambda, + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } + + #[cfg(feature = "dim3")] + { + let j_step = ndofs2 * 2; + let dvel_0 = jacobians + .rows(j_id2, ndofs2) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + + self.rhs[0]; + let dvel_1 = jacobians + .rows(j_id2 + j_step, ndofs2) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + + self.rhs[1]; + + let new_impulse = na::Vector2::new( + self.impulse[0] - self.r[0] * dvel_0, + self.impulse[1] - self.r[1] * dvel_1, + ); + let new_impulse = new_impulse.cap_magnitude(limit); + + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + dlambda[0], + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + dlambda[1], + &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), + 1.0, + ); + } + } +} + +impl OneBodyConstraintNormalPart { + #[inline] + pub fn generic_solve( + &mut self, + cfm_factor: Real, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + let dvel = jacobians + .rows(j_id2, ndofs2) + .dot(&solver_vels.rows(solver_vel2, ndofs2)) + + self.rhs; + + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + dlambda, + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } +} + +impl OneBodyConstraintElement { + #[inline] + pub fn generic_solve_group( + cfm_factor: Real, + elements: &mut [Self], + jacobians: &DVector, + limit: Real, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let j_step = ndofs2 * 2 * DIM; + + // Solve penetration. + if solve_restitution { + let mut nrm_j_id = j_id; + + for element in elements.iter_mut() { + element.normal_part.generic_solve( + cfm_factor, + nrm_j_id, + jacobians, + ndofs2, + solver_vel2, + solver_vels, + ); + nrm_j_id += j_step; + } + } + + // Solve friction. + if solve_friction { + let mut tng_j_id = j_id + ndofs2 * 2; + + for element in elements.iter_mut() { + let limit = limit * element.normal_part.impulse; + let part = &mut element.tangent_part; + part.generic_solve(tng_j_id, jacobians, ndofs2, limit, solver_vel2, solver_vels); + tng_j_id += j_step; + } + } + } +} diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs new file mode 100644 index 0000000..a4924fe --- /dev/null +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -0,0 +1,433 @@ +use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint}; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; + +use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; +use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::{ContactPointInfos, SolverVel}; +use crate::prelude::RigidBodyHandle; +#[cfg(feature = "dim2")] +use crate::utils::SimdBasis; +use na::DVector; + +#[derive(Copy, Clone)] +pub(crate) struct GenericTwoBodyConstraintBuilder { + handle1: RigidBodyHandle, + handle2: RigidBodyHandle, + ccd_thickness: Real, + inner: TwoBodyConstraintBuilder, +} + +impl GenericTwoBodyConstraintBuilder { + pub fn invalid() -> Self { + Self { + handle1: RigidBodyHandle::invalid(), + handle2: RigidBodyHandle::invalid(), + ccd_thickness: Real::MAX, + inner: TwoBodyConstraintBuilder::invalid(), + } + } + + pub fn generate( + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + out_builders: &mut [GenericTwoBodyConstraintBuilder], + out_constraints: &mut [GenericTwoBodyConstraint], + jacobians: &mut DVector, + jacobian_id: &mut usize, + ) { + let handle1 = manifold.data.rigid_body1.unwrap(); + let handle2 = manifold.data.rigid_body2.unwrap(); + + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; + + let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type); + let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type); + + let multibody1 = multibodies + .rigid_body_link(handle1) + .map(|m| (&multibodies[m.multibody], m.id)); + let multibody2 = multibodies + .rigid_body_link(handle2) + .map(|m| (&multibodies[m.multibody], m.id)); + let solver_vel1 = multibody1 + .map(|mb| mb.0.solver_id) + .unwrap_or(if type1.is_dynamic() { + rb1.ids.active_set_offset + } else { + 0 + }); + let solver_vel2 = multibody2 + .map(|mb| mb.0.solver_id) + .unwrap_or(if type2.is_dynamic() { + rb2.ids.active_set_offset + } else { + 0 + }); + let force_dir1 = -manifold.data.normal; + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); + + let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0) + + multibody2.map(|m| m.0.ndofs()).unwrap_or(0); + // For each solver contact we generate DIM constraints, and each constraints appends + // the multibodies jacobian and weighted jacobians + let required_jacobian_len = + *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; + + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } + + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let chunk_j_id = *jacobian_id; + + let builder = &mut out_builders[l]; + let constraint = &mut out_constraints[l]; + constraint.inner.dir1 = force_dir1; + constraint.inner.im1 = if type1.is_dynamic() { + mprops1.effective_inv_mass + } else { + na::zero() + }; + constraint.inner.im2 = if type2.is_dynamic() { + mprops2.effective_inv_mass + } else { + na::zero() + }; + constraint.inner.solver_vel1 = solver_vel1; + constraint.inner.solver_vel2 = solver_vel2; + constraint.inner.manifold_id = manifold_id; + constraint.inner.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] + { + constraint.inner.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let point = manifold_point.point; + let dp1 = point - mprops1.world_com; + let dp2 = point - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + constraint.inner.limit = manifold_point.friction; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + + let gcross1 = if type1.is_dynamic() { + mprops1 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir1) + } else { + na::zero() + }; + let gcross2 = if type2.is_dynamic() { + mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir2) + } else { + na::zero() + }; + + let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { + mb1.fill_jacobians( + *link_id1, + force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir1), + #[cfg(feature = "dim3")] + torque_dir1, + jacobian_id, + jacobians, + ) + .0 + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + + gcross1.gdot(gcross1) + } else { + 0.0 + }; + + let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { + mb2.fill_jacobians( + *link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0 + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2) + } else { + 0.0 + }; + + let r = crate::utils::inv(inv_r1 + inv_r2); + + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + + normal_rhs_wo_bias = + (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); + + constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart { + gcross1, + gcross2, + rhs: na::zero(), + rhs_wo_bias: na::zero(), + total_impulse: na::zero(), + impulse: na::zero(), + r, + }; + } + + // Tangent parts. + { + constraint.inner.elements[k].tangent_part.impulse = na::zero(); + + for j in 0..DIM - 1 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let gcross1 = if type1.is_dynamic() { + mprops1 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir1) + } else { + na::zero() + }; + constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1; + + let torque_dir2 = dp2.gcross(-tangents1[j]); + let gcross2 = if type2.is_dynamic() { + mprops2 + .effective_world_inv_inertia_sqrt + .transform_vector(torque_dir2) + } else { + na::zero() + }; + constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2; + + let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { + mb1.fill_jacobians( + *link_id1, + tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir1], + #[cfg(feature = "dim3")] + torque_dir1, + jacobian_id, + jacobians, + ) + .0 + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + + gcross1.gdot(gcross1) + } else { + 0.0 + }; + + let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { + mb2.fill_jacobians( + *link_id2, + -tangents1[j], + #[cfg(feature = "dim2")] + na::vector![torque_dir2], + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0 + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2) + } else { + 0.0 + }; + + let r = crate::utils::inv(inv_r1 + inv_r2); + let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); + + constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias; + + // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. + constraint.inner.elements[k].tangent_part.r[j] = r; + } + } + + // Builder. + let infos = ContactPointInfos { + local_p1: rb1 + .pos + .position + .inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_rhs_wo_bias, + }; + + builder.handle1 = handle1; + builder.handle2 = handle2; + builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; + builder.inner.infos[k] = infos; + constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + } + + let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); + let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); + + // NOTE: we use the generic constraint for non-dynamic bodies because this will + // reduce all ops to nothing because its ndofs will be zero. + let generic_constraint_mask = (multibody1.is_some() as u8) + | ((multibody2.is_some() as u8) << 1) + | (!type1.is_dynamic() as u8) + | ((!type2.is_dynamic() as u8) << 1); + + constraint.j_id = chunk_j_id; + constraint.ndofs1 = ndofs1; + constraint.ndofs2 = ndofs2; + constraint.generic_constraint_mask = generic_constraint_mask; + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &[SolverBody], + multibodies: &MultibodyJointSet, + constraint: &mut GenericTwoBodyConstraint, + ) { + // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. + let pos1 = multibodies + .rigid_body_link(self.handle1) + .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) + .unwrap_or_else(|| &bodies[constraint.inner.solver_vel1].position); + let pos2 = multibodies + .rigid_body_link(self.handle2) + .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) + .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position); + + self.inner.update_with_positions( + params, + solved_dt, + pos1, + pos2, + self.ccd_thickness, + &mut constraint.inner, + ); + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct GenericTwoBodyConstraint { + // We just build the generic constraint on top of the velocity constraint, + // adding some information we can use in the generic case. + pub inner: TwoBodyConstraint, + pub j_id: usize, + pub ndofs1: usize, + pub ndofs2: usize, + pub generic_constraint_mask: u8, +} + +impl GenericTwoBodyConstraint { + pub fn invalid() -> Self { + Self { + inner: TwoBodyConstraint::invalid(), + j_id: usize::MAX, + ndofs1: usize::MAX, + ndofs2: usize::MAX, + generic_constraint_mask: u8::MAX, + } + } + + pub fn solve( + &mut self, + jacobians: &DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + solve_restitution: bool, + solve_friction: bool, + ) { + let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1 as usize]) + } else { + GenericRhs::GenericId(self.inner.solver_vel1 as usize) + }; + + let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2 as usize]) + } else { + GenericRhs::GenericId(self.inner.solver_vel2 as usize) + }; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + TwoBodyConstraintElement::generic_solve_group( + self.inner.cfm_factor, + elements, + jacobians, + &self.inner.dir1, + #[cfg(feature = "dim3")] + &self.inner.tangent1, + &self.inner.im1, + &self.inner.im2, + self.inner.limit, + self.ndofs1, + self.ndofs2, + self.j_id, + &mut solver_vel1, + &mut solver_vel2, + generic_solver_vels, + solve_restitution, + solve_friction, + ); + + if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { + solver_vels[self.inner.solver_vel1 as usize] = solver_vel1; + } + + if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { + solver_vels[self.inner.solver_vel2 as usize] = solver_vel2; + } + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + self.inner.writeback_impulses(manifolds_all); + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.inner.remove_cfm_and_bias_from_rhs(); + } +} diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs new file mode 100644 index 0000000..f3bd3a0 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs @@ -0,0 +1,367 @@ +use crate::dynamics::solver::SolverVel; +use crate::dynamics::solver::{ + TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart, +}; +use crate::math::{AngVector, Real, Vector, DIM}; +use crate::utils::SimdDot; +use na::DVector; +#[cfg(feature = "dim2")] +use {crate::utils::SimdBasis, na::SimdPartialOrd}; + +pub(crate) enum GenericRhs { + SolverVel(SolverVel), + GenericId(usize), +} + +// Offset between the jacobians of two consecutive constraints. +#[inline(always)] +fn j_step(ndofs1: usize, ndofs2: usize) -> usize { + (ndofs1 + ndofs2) * 2 +} + +#[inline(always)] +fn j_id1(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize { + j_id +} + +#[inline(always)] +fn j_id2(j_id: usize, ndofs1: usize, _ndofs2: usize) -> usize { + j_id + ndofs1 * 2 +} + +#[inline(always)] +fn normal_j_id(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize { + j_id +} + +#[inline(always)] +fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize { + j_id + (ndofs1 + ndofs2) * 2 +} + +impl GenericRhs { + #[inline(always)] + fn dvel( + &self, + j_id: usize, + ndofs: usize, + jacobians: &DVector, + dir: &Vector, + gcross: &AngVector, + solver_vels: &DVector, + ) -> Real { + match self { + GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular), + GenericRhs::GenericId(solver_vel) => { + let j = jacobians.rows(j_id, ndofs); + let rhs = solver_vels.rows(*solver_vel, ndofs); + j.dot(&rhs) + } + } + } + + #[inline(always)] + fn apply_impulse( + &mut self, + j_id: usize, + ndofs: usize, + impulse: Real, + jacobians: &DVector, + dir: &Vector, + gcross: &AngVector, + solver_vels: &mut DVector, + inv_mass: &Vector, + ) { + match self { + GenericRhs::SolverVel(rhs) => { + rhs.linear += dir.component_mul(inv_mass) * impulse; + rhs.angular += gcross * impulse; + } + GenericRhs::GenericId(solver_vel) => { + let wj_id = j_id + ndofs; + let wj = jacobians.rows(wj_id, ndofs); + let mut rhs = solver_vels.rows_mut(*solver_vel, ndofs); + rhs.axpy(impulse, &wj, 1.0); + } + } + } +} + +impl TwoBodyConstraintTangentPart { + #[inline] + pub fn generic_solve( + &mut self, + j_id: usize, + jacobians: &DVector, + tangents1: [&Vector; DIM -