From 2b1374c596957ac8cabe085859be3b823a1ba0c6 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 19 Apr 2022 18:57:40 +0200 Subject: First round deleting the component sets. --- src/dynamics/solver/categorization.rs | 7 +- src/dynamics/solver/generic_velocity_constraint.rs | 16 +- .../solver/generic_velocity_ground_constraint.rs | 15 +- src/dynamics/solver/interaction_groups.rs | 31 ++-- src/dynamics/solver/island_solver.rs | 22 +-- .../solver/joint_constraint/joint_constraint.rs | 49 ++---- src/dynamics/solver/parallel_island_solver.rs | 17 +- src/dynamics/solver/parallel_solver_constraints.rs | 31 ++-- src/dynamics/solver/parallel_velocity_solver.rs | 15 +- src/dynamics/solver/solver_constraints.rs | 175 ++++++--------------- src/dynamics/solver/velocity_constraint.rs | 15 +- src/dynamics/solver/velocity_constraint_wide.rs | 11 +- src/dynamics/solver/velocity_ground_constraint.rs | 15 +- .../solver/velocity_ground_constraint_wide.rs | 11 +- src/dynamics/solver/velocity_solver.rs | 17 +- 15 files changed, 127 insertions(+), 320 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 0083388..06ba340 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,9 +1,8 @@ -use crate::data::ComponentSet; -use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyType}; +use crate::dynamics::{JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( - _bodies: &impl ComponentSet, // Unused but useful to simplify the parallel code. + _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code. multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -40,7 +39,7 @@ pub(crate) fn categorize_contacts( } pub(crate) fn categorize_joints( - bodies: &impl ComponentSet, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index de09a21..1be34bc 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -1,8 +1,7 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType, - RigidBodyVelocity, + IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, + RigidBodyType, RigidBodyVelocity, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; @@ -27,22 +26,17 @@ pub(crate) struct GenericVelocityConstraint { } impl GenericVelocityConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, out_constraints: &mut Vec, jacobians: &mut DVector, jacobian_id: &mut usize, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index 76e2e51..d953e6f 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -1,8 +1,6 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::VelocityGroundConstraint; use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType, - RigidBodyVelocity, + IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; @@ -25,22 +23,17 @@ pub(crate) struct GenericVelocityGroundConstraint { } impl GenericVelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, out_constraints: &mut Vec, jacobians: &mut DVector, jacobian_id: &mut usize, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 2bcbacc..951b77f 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,5 +1,4 @@ -use crate::data::ComponentSet; -use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds}; +use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] @@ -62,17 +61,15 @@ impl ParallelInteractionGroups { self.groups.len() - 1 } - pub fn group_interactions( + pub fn group_interactions( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, interactions: &[Interaction], interaction_indices: &[usize], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { let num_island_bodies = islands.active_island(island_id).len(); self.bodies_color.clear(); self.interaction_indices.clear(); @@ -217,7 +214,7 @@ impl InteractionGroups { &mut self, _island_id: usize, _islands: &IslandManager, - _bodies: &impl ComponentSet, + _bodies: &RigidBodySet, _interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], ) { @@ -226,16 +223,14 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_joints( + pub fn group_joints( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { // TODO: right now, we only sort based on the axes locked by the joint. // We could also take motors and limits into account in the future (most of // the SIMD constraints generation for motors and limits is already implemented). @@ -376,7 +371,7 @@ impl InteractionGroups { &mut self, _island_id: usize, _islands: &IslandManager, - _bodies: &impl ComponentSet, + _bodies: &RigidBodySet, _interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], ) { @@ -385,16 +380,14 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_manifolds( + pub fn group_manifolds( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. // TODO: currently, this is a bit overconservative because when a bucket diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 90c8d90..edd17f8 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,14 +1,10 @@ use super::VelocitySolver; use crate::counters::Counters; -use crate::data::{ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, - RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, -}; -use crate::dynamics::{IslandManager, RigidBodyVelocity}; +use crate::dynamics::IslandManager; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::prelude::MultibodyJointSet; @@ -33,27 +29,19 @@ impl IslandSolver { } } - pub fn init_and_solve( + pub fn init_and_solve( &mut self, island_id: usize, counters: &mut Counters, params: &IntegrationParameters, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], impulse_joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], multibody_joints: &mut MultibodyJointSet, - ) where - Bodies: ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { // Init the solver id for multibody_joints. // We need that for building the constraints. let mut solver_id = 0; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index e1150d5..a46744d 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -1,4 +1,3 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::joint_constraint::joint_generic_velocity_constraint::{ JointGenericVelocityConstraint, JointGenericVelocityGroundConstraint, }; @@ -8,7 +7,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds, - RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity, }; use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::MultibodyJointSet; @@ -51,22 +50,17 @@ impl AnyJointVelocityConstraint { (num_constraints, num_constraints) } - pub fn from_joint( + pub fn from_joint( params: &IntegrationParameters, joint_id: JointIndex, joint: &ImpulseJoint, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, j_id: &mut usize, jacobians: &mut DVector, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let local_frame1 = joint.data.local_frame1; let local_frame2 = joint.data.local_frame2; let rb1: ( @@ -184,19 +178,14 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint( + pub fn from_wide_joint( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], impulse_joints: [&ImpulseJoint; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let rbs1: ( [&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyVelocity; SIMD_WIDTH], @@ -274,23 +263,17 @@ impl AnyJointVelocityConstraint { } } - pub fn from_joint_ground( + pub fn from_joint_ground( params: &IntegrationParameters, joint_id: JointIndex, joint: &ImpulseJoint, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, j_id: &mut usize, jacobians: &mut DVector, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut handle1 = joint.body1; let mut handle2 = joint.body2; let status2: &RigidBodyType = bodies.index(handle2.0); @@ -408,20 +391,14 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint_ground( + pub fn from_wide_joint_ground( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], impulse_joints: [&ImpulseJoint; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut handles1 = gather![|ii| impulse_joints[ii].body1]; let mut handles2 = gather![|ii| impulse_joints[ii].body2]; let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 61b3fcb..261c627 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -2,7 +2,6 @@ use std::sync::atomic::{AtomicUsize, Ordering}; use rayon::Scope; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointVelocityConstraint, AnyVelocityConstraint, ParallelSolverConstraints, }; @@ -162,27 +161,19 @@ impl ParallelIslandSolver { } } - pub fn init_and_solve<'s, Bodies>( + pub fn init_and_solve<'s>( &'s mut self, scope: &Scope<'s>, island_id: usize, islands: &'s IslandManager, params: &'s IntegrationParameters, - bodies: &'s mut Bodies, + bodies: &'s mut RigidBodySet, manifolds: &'s mut Vec<&'s mut ContactManifold>, manifold_indices: &'s [ContactManifoldIndex], impulse_joints: &'s mut Vec, joint_indices: &[JointIndex], multibodies: &mut MultibodyJointSet, - ) where - Bodies: ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? @@ -288,7 +279,7 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let velocity_solver: &mut ParallelVelocitySolver = unsafe { std::mem::transmute(velocity_solver.load(Ordering::Relaxed)) }; - let bodies: &mut Bodies = + let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let multibodies: &mut MultibodyJointSet = unsafe { std::mem::transmute(multibodies.load(Ordering::Relaxed)) }; diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index aadee48..d883494 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -1,6 +1,5 @@ use super::ParallelInteractionGroups; use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext}; -use crate::data::ComponentSet; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint; use crate::dynamics::solver::{ @@ -88,16 +87,16 @@ macro_rules! impl_init_constraints_group { $num_active_constraints_and_jacobian_lines: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => { impl ParallelSolverConstraints<$VelocityConstraint> { - pub fn init_constraint_groups( + pub fn init_constraint_groups( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, interactions: &mut [$Interaction], interaction_groups: &ParallelInteractionGroups, j_id: &mut usize, - ) where Bodies: ComponentSet + ComponentSet { + ) { let mut total_num_constraints = 0; let num_groups = interaction_groups.num_groups(); @@ -316,20 +315,14 @@ impl_init_constraints_group!( ); impl ParallelSolverConstraints { - pub fn fill_constraints( + pub fn fill_constraints( &mut self, thread: &ThreadContext, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let descs = &self.constraint_descs; crate::concurrent_loop! { @@ -372,20 +365,14 @@ impl ParallelSolverConstraints { } impl ParallelSolverConstraints { - pub fn fill_constraints( + pub fn fill_constraints( &mut self, thread: &ThreadContext, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibodies: &MultibodyJointSet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let descs = &self.constraint_descs; crate::concurrent_loop! { diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index b913eef..5b062e0 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -1,6 +1,5 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext}; use crate::concurrent_loop; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::{ solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, @@ -26,27 +25,19 @@ impl ParallelVelocitySolver { } } - pub fn solve( + pub fn solve( &mut self, thread: &ThreadContext, params: &IntegrationParameters, island_id: usize, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], contact_constraints: &mut ParallelSolverConstraints, joint_constraints: &mut ParallelSolverConstraints, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet, - { + ) { let mut start_index = thread .solve_interaction_index .fetch_add(thread.batch_size, Ordering::SeqCst); diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 081ea9c..4006a85 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -3,15 +3,13 @@ use super::{ }; #[cfg(feature = "simd-is-enabled")] use super::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::data::ComponentSet; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint; use crate::dynamics::solver::GenericVelocityConstraint; use crate::dynamics::{ - solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, - MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + solver::AnyVelocityConstraint, IntegrationParameters, IslandManager, JointGraphEdge, + JointIndex, MultibodyJointSet, RigidBodySet, }; -use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::Real; #[cfg(feature = "simd-is-enabled")] @@ -55,17 +53,15 @@ impl SolverConstraints { } impl SolverConstraints { - pub fn init_constraint_groups( + pub fn init_constraint_groups( &mut self, island_id: usize, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - ) where - Bodies: ComponentSet + ComponentSet, - { + ) { self.not_ground_interactions.clear(); self.ground_interactions.clear(); self.generic_not_ground_interactions.clear(); @@ -109,22 +105,16 @@ impl SolverConstraints { // .append(&mut self.ground_interaction_groups.grouped_interactions); } - pub fn init( + pub fn init( &mut self, island_id: usize, params: &IntegrationParameters, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { self.velocity_constraints.clear(); self.init_constraint_groups( @@ -165,17 +155,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_constraints( + fn compute_grouped_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifolds_i in self .interaction_groups .grouped_interactions @@ -194,17 +179,12 @@ impl SolverConstraints { } } - fn compute_nongrouped_constraints( + fn compute_nongrouped_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; VelocityConstraint::generate( @@ -218,20 +198,14 @@ impl SolverConstraints { } } - fn compute_generic_constraints( + fn compute_generic_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], jacobian_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.generic_not_ground_interactions { let manifold = &manifolds_all[*manifold_i]; GenericVelocityConstraint::generate( @@ -248,20 +222,14 @@ impl SolverConstraints { } } - fn compute_generic_ground_constraints( + fn compute_generic_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, manifolds_all: &[&mut ContactManifold], jacobian_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.generic_ground_interactions { let manifold = &manifolds_all[*manifold_i]; GenericVelocityGroundConstraint::generate( @@ -279,17 +247,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_ground_constraints( + fn compute_grouped_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifolds_i in self .ground_interaction_groups .grouped_interactions @@ -308,17 +271,12 @@ impl SolverConstraints { } } - fn compute_nongrouped_ground_constraints( + fn compute_nongrouped_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, manifolds_all: &[&mut ContactManifold], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; VelocityGroundConstraint::generate( @@ -334,22 +292,16 @@ impl SolverConstraints { } impl SolverConstraints { - pub fn init( + pub fn init( &mut self, island_id: usize, params: &IntegrationParameters, islands: &IslandManager, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { // Generate constraints for impulse_joints. self.not_ground_interactions.clear(); self.ground_interactions.clear(); @@ -464,19 +416,13 @@ impl SolverConstraints { } } - fn compute_nongrouped_joint_ground_constraints( + fn compute_nongrouped_joint_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let mut j_id = 0; for joint_i in &self.ground_interaction_groups.nongrouped_interactions { let joint = &joints_all[*joint_i].weight; @@ -495,18 +441,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_ground_constraints( + fn compute_grouped_joint_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joints_i in self .ground_interaction_groups .grouped_interactions @@ -525,19 +465,14 @@ impl SolverConstraints { } } - fn compute_nongrouped_joint_constraints( + fn compute_nongrouped_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], j_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joint_i in &self.interaction_groups.nongrouped_interactions { let joint = &joints_all[*joint_i].weight; AnyJointVelocityConstraint::from_joint( @@ -554,19 +489,14 @@ impl SolverConstraints { } } - fn compute_generic_joint_constraints( + fn compute_generic_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], j_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joint_i in &self.generic_not_ground_interactions { let joint = &joints_all[*joint_i].weight; AnyJointVelocityConstraint::from_joint( @@ -583,20 +513,14 @@ impl SolverConstraints { } } - fn compute_generic_ground_joint_constraints( + fn compute_generic_ground_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, joints_all: &[JointGraphEdge], j_id: &mut usize, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joint_i in &self.generic_ground_interactions { let joint = &joints_all[*joint_i].weight; AnyJointVelocityConstraint::from_joint_ground( @@ -614,17 +538,12 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_constraints( + fn compute_grouped_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &Bodies, + bodies: &RigidBodySet, joints_all: &[JointGraphEdge], - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for joints_i in self .interaction_groups .grouped_interactions diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index dc079d4..c854136 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -1,10 +1,11 @@ -use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::{ GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint, }; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot}; @@ -144,18 +145,14 @@ impl VelocityConstraint { ) } - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { assert_eq!(manifold.data.relative_dominance, 0); let inv_dt = params.inv_dt(); diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 0c85755..81c27fd 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -1,7 +1,6 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, }; -use crate::data::ComponentSet; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ @@ -30,18 +29,14 @@ pub(crate) struct WVelocityConstraint { } impl WVelocityConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { for ii in 0..SIMD_WIDTH { assert_eq!(manifolds[ii].data.relative_dominance, 0); } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 93da6dc..0acc604 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -7,8 +7,9 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::WBasis; use crate::utils::{self, WAngularInertia, WCross, WDot}; -use crate::data::{BundleSet, ComponentSet}; -use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[derive(Copy, Clone, Debug)] @@ -27,18 +28,14 @@ pub(crate) struct VelocityGroundConstraint { } impl VelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 1889901..4b3d429 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,6 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, }; -use crate::data::ComponentSet; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ @@ -29,18 +28,14 @@ pub(crate) struct WVelocityGroundConstraint { } impl WVelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &Bodies, + bodies: &RigidBodySet, out_constraints: &mut Vec, insert_at: Option, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet, - { + ) { let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 66d7caf..a0d494e 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,9 +1,8 @@ use super::AnyJointVelocityConstraint; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping, - RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity, }; use crate::geometry::ContactManifold; @@ -24,12 +23,12 @@ impl VelocitySolver { } } - pub fn solve( + pub fn solve( &mut self, island_id: usize, params: &IntegrationParameters, islands: &IslandManager, - bodies: &mut Bodies, + bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], @@ -37,15 +36,7 @@ impl VelocitySolver { generic_contact_jacobians: &DVector, joint_constraints: &mut [AnyJointVelocityConstraint], generic_joint_jacobians: &DVector, - ) where - Bodies: ComponentSet - + ComponentSet - + ComponentSet - + ComponentSetMut - + ComponentSetMut - + ComponentSetMut - + ComponentSet, - { + ) { let cfm_factor = params.cfm_factor(); self.mj_lambdas.clear(); self.mj_lambdas -- cgit