diff options
Diffstat (limited to 'src/dynamics/solver')
| -rw-r--r-- | src/dynamics/solver/generic_velocity_constraint.rs | 11 | ||||
| -rw-r--r-- | src/dynamics/solver/generic_velocity_ground_constraint.rs | 12 | ||||
| -rw-r--r-- | src/dynamics/solver/interaction_groups.rs | 4 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_constraint.rs | 85 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_solver_constraints.rs | 99 | ||||
| -rw-r--r-- | src/dynamics/solver/solver_constraints.rs | 25 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_constraint.rs | 18 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_wide.rs | 10 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 11 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint_wide.rs | 10 |
10 files changed, 154 insertions, 131 deletions
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 99dc40e..261c4c8 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -36,7 +36,7 @@ impl GenericVelocityConstraint { out_constraints: &mut Vec<AnyVelocityConstraint>, jacobians: &mut DVector<Real>, jacobian_id: &mut usize, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyVelocity> @@ -99,7 +99,7 @@ impl GenericVelocityConstraint { let required_jacobian_len = *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; - if jacobians.nrows() < required_jacobian_len { + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } @@ -320,11 +320,10 @@ impl GenericVelocityConstraint { generic_constraint_mask, }; - if push { - out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint)); + if let Some(at) = insert_at { + out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGeneric(constraint); } else { - out_constraints[manifold.data.constraint_index + _l] = - AnyVelocityConstraint::NongroupedGeneric(constraint); + out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint)); } } } diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index b2e5878..044bd48 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -34,7 +34,7 @@ impl GenericVelocityGroundConstraint { out_constraints: &mut Vec<AnyVelocityConstraint>, jacobians: &mut DVector<Real>, jacobian_id: &mut usize, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyVelocity> @@ -87,7 +87,7 @@ impl GenericVelocityGroundConstraint { let required_jacobian_len = *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; - if jacobians.nrows() < required_jacobian_len { + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } @@ -200,11 +200,11 @@ impl GenericVelocityGroundConstraint { ndofs2: mb2.ndofs(), }; - if push { - out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint)); - } else { - out_constraints[manifold.data.constraint_index + _l] = + if let Some(at) = insert_at { + out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGenericGround(constraint); + } else { + out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint)); } } } diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 4adbc18..7f49ec3 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -131,14 +131,14 @@ impl ParallelInteractionGroups { (true, false) => { let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); let color_mask = bcolors[rb_ids2.active_set_offset]; - *color = (!color_mask).leading_zeros() as usize; + *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; bcolors[rb_ids2.active_set_offset] |= 1 << *color; } (false, true) => { let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0); let color_mask = bcolors[rb_ids1.active_set_offset]; - *color = (!color_mask).leading_zeros() as usize; + *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; bcolors[rb_ids1.active_set_offset] |= 1 << *color; } diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index c006417..988e6a2 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -30,20 +30,21 @@ pub enum AnyJointVelocityConstraint { impl AnyJointVelocityConstraint { #[cfg(feature = "parallel")] - pub fn num_active_constraints(joint: &ImpulseJoint) -> usize { + pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, usize) { 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(); - (motor_axes & !coupled_axes).count_ones() as usize + 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 + + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize; + (num_constraints, num_constraints) } pub fn from_joint<Bodies>( @@ -55,7 +56,7 @@ impl AnyJointVelocityConstraint { j_id: &mut usize, jacobians: &mut DVector<Real>, out: &mut Vec<Self>, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyPosition> + ComponentSet<RigidBodyVelocity> @@ -122,7 +123,7 @@ impl AnyJointVelocityConstraint { // TODO: is this count correct when we take both motors and limits into account? let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - if jacobians.nrows() < required_jacobian_len { + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } @@ -143,14 +144,13 @@ impl AnyJointVelocityConstraint { &mut out_tmp, ); - if push { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGenericConstraint(c)); + if let Some(at) = insert_at { + for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { + out[at + i] = AnyJointVelocityConstraint::JointGenericConstraint(c); } } else { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[joint.constraint_index + i] = - AnyJointVelocityConstraint::JointGenericConstraint(c); + for c in out_tmp.into_iter().take(out_tmp_len) { + out.push(AnyJointVelocityConstraint::JointGenericConstraint(c)); } } } else { @@ -167,14 +167,13 @@ impl AnyJointVelocityConstraint { &mut out_tmp, ); - if push { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointConstraint(c)); + if let Some(at) = insert_at { + for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { + out[at + i] = AnyJointVelocityConstraint::JointConstraint(c); } } else { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[joint.constraint_index + i] = - AnyJointVelocityConstraint::JointConstraint(c); + for c in out_tmp.into_iter().take(out_tmp_len) { + out.push(AnyJointVelocityConstraint::JointConstraint(c)); } } } @@ -187,7 +186,7 @@ impl AnyJointVelocityConstraint { impulse_joints: [&ImpulseJoint; SIMD_WIDTH], bodies: &Bodies, out: &mut Vec<Self>, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyPosition> + ComponentSet<RigidBodyVelocity> @@ -260,14 +259,13 @@ impl AnyJointVelocityConstraint { &mut out_tmp, ); - if push { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointConstraintSimd(c)); + if let Some(at) = insert_at { + for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { + out[at + i] = AnyJointVelocityConstraint::JointConstraintSimd(c); } } else { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[impulse_joints[0].constraint_index + i] = - AnyJointVelocityConstraint::JointConstraintSimd(c); + for c in out_tmp.into_iter().take(out_tmp_len) { + out.push(AnyJointVelocityConstraint::JointConstraintSimd(c)); } } } @@ -281,7 +279,7 @@ impl AnyJointVelocityConstraint { j_id: &mut usize, jacobians: &mut DVector<Real>, out: &mut Vec<Self>, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyPosition> + ComponentSet<RigidBodyType> @@ -350,7 +348,7 @@ impl AnyJointVelocityConstraint { // TODO: is this count correct when we take both motors and limits into account? let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - if jacobians.nrows() < required_jacobian_len { + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } @@ -370,14 +368,13 @@ impl AnyJointVelocityConstraint { &mut out_tmp, ); - if push { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c)); + if let Some(at) = insert_at { + for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { + out[at + i] = AnyJointVelocityConstraint::JointGenericGroundConstraint(c); } } else { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[joint.constraint_index + i] = - AnyJointVelocityConstraint::JointGenericGroundConstraint(c); + for c in out_tmp.into_iter().take(out_tmp_len) { + out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c)); } } } else { @@ -394,14 +391,13 @@ impl AnyJointVelocityConstraint { &mut out_tmp, ); - if push { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGroundConstraint(c)); + if let Some(at) = insert_at { + for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { + out[at + i] = AnyJointVelocityConstraint::JointGroundConstraint(c); } } else { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[joint.constraint_index + i] = - AnyJointVelocityConstraint::JointGroundConstraint(c); + for c in out_tmp.into_iter().take(out_tmp_len) { + out.push(AnyJointVelocityConstraint::JointGroundConstraint(c)); } } } @@ -414,7 +410,7 @@ impl AnyJointVelocityConstraint { impulse_joints: [&ImpulseJoint; SIMD_WIDTH], bodies: &Bodies, out: &mut Vec<Self>, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyPosition> + ComponentSet<RigidBodyType> @@ -506,14 +502,13 @@ impl AnyJointVelocityConstraint { &mut out_tmp, ); - if push { - for c in out_tmp.into_iter().take(out_tmp_len) { - out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c)); + if let Some(at) = insert_at { + for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { + out[at + i] = AnyJointVelocityConstraint::JointGroundConstraintSimd(c); } } else { - for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() { - out[impulse_joints[0].constraint_index + i] = - AnyJointVelocityConstraint::JointGroundConstraintSimd(c); + for c in out_tmp.into_iter().take(out_tmp_len) { + out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c)); } } } diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index 3a8f9c8..4c43f46 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -8,12 +8,12 @@ use crate::dynamics::solver::{ VelocityGroundConstraint, }; use crate::dynamics::{ - ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, - RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, - RigidBodyVelocity, + ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex, + MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, + RigidBodyType, RigidBodyVelocity, }; use crate::geometry::ContactManifold; -use crate::math::{Real, SPATIAL_DIM}; +use crate::math::{Real, DIM, SPATIAL_DIM}; #[cfg(feature = "simd-is-enabled")] use crate::{ dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}, @@ -46,6 +46,7 @@ pub(crate) enum ConstraintDesc { GroundGrouped([usize; SIMD_WIDTH]), GenericNongroundNongrouped(usize, usize), GenericGroundNongrouped(usize, usize), + GenericMultibodyInternal(MultibodyIndex, usize), } pub(crate) struct ParallelSolverConstraints<VelocityConstraint> { @@ -81,10 +82,10 @@ impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> { macro_rules! impl_init_constraints_group { ($VelocityConstraint: ty, $Interaction: ty, $categorize: ident, $group: ident, - $data: ident$(.$constraint_index: ident)*, $body1: ident, $body2: ident, - $num_active_constraints: path, + $generate_internal_constraints: expr, + $num_active_constraints_and_jacobian_lines: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => { impl ParallelSolverConstraints<$VelocityConstraint> { pub fn init_constraint_groups<Bodies>( @@ -151,12 +152,11 @@ macro_rules! impl_init_constraints_group { // Compute constraint indices. for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; - interaction.$data$(.$constraint_index)* = total_num_constraints; self.constraint_descs.push(( total_num_constraints, ConstraintDesc::NongroundNongrouped(*interaction_i), )); - total_num_constraints += $num_active_constraints(interaction); + total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; } #[cfg(feature = "simd-is-enabled")] @@ -164,26 +164,24 @@ macro_rules! impl_init_constraints_group { self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) { let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; - interaction.$data$(.$constraint_index)* = total_num_constraints; self.constraint_descs.push(( total_num_constraints, ConstraintDesc::NongroundGrouped( gather![|ii| interaction_i[ii]], ), )); - total_num_constraints += $num_active_constraints(interaction); + total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; } for interaction_i in &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; - interaction.$data$(.$constraint_index)* = total_num_constraints; self.constraint_descs.push(( total_num_constraints, ConstraintDesc::GroundNongrouped(*interaction_i), )); - total_num_constraints += $num_active_constraints(interaction); + total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; } #[cfg(feature = "simd-is-enabled")] @@ -192,14 +190,13 @@ macro_rules! impl_init_constraints_group { .chunks(SIMD_WIDTH) { let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; - interaction.$data$(.$constraint_index)* = total_num_constraints; self.constraint_descs.push(( total_num_constraints, ConstraintDesc::GroundGrouped( gather![|ii| interaction_i[ii]], ), )); - total_num_constraints += $num_active_constraints(interaction); + total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; } let multibody_ndofs = |handle| { @@ -215,38 +212,62 @@ macro_rules! impl_init_constraints_group { for interaction_i in &self.generic_not_ground_interactions[..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; - interaction.$data$(.$constraint_index)* = total_num_constraints; self.constraint_descs.push(( total_num_constraints, ConstraintDesc::GenericNongroundNongrouped(*interaction_i, *j_id), )); - let num_constraints = $num_active_constraints(interaction); + let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction); let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0); let ndofs2 = $body2(interaction).map(multibody_ndofs).unwrap_or(0); - *j_id += num_constraints * (ndofs1 + ndofs2) * 2; + *j_id += (ndofs1 + ndofs2) * 2 * num_jac_lines; total_num_constraints += num_constraints; } for interaction_i in &self.generic_ground_interactions[..] { let interaction = &mut interactions[*interaction_i]$(.$weight)*; - interaction.$data$(.$constraint_index)* = total_num_constraints; self.constraint_descs.push(( total_num_constraints, ConstraintDesc::GenericGroundNongrouped(*interaction_i, *j_id), )); - let num_constraints = $num_active_constraints(interaction); + let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction); let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0); let ndofs2 = $body2(interaction).map(multibody_ndofs).unwrap_or(0); - *j_id += num_constraints * (ndofs1 + ndofs2) * 2; + *j_id += (ndofs1 + ndofs2) * 2 * num_jac_lines; total_num_constraints += num_constraints; } self.parallel_desc_groups.push(self.constraint_descs.len()); } + if $generate_internal_constraints { + let mut had_any_internal_constraint = false; + for handle in islands.active_island(island_id) { + if let Some(link) = multibodies.rigid_body_link(*handle) { + let multibody = multibodies.get_multibody(link.multibody).unwrap(); + if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { + let (num_constraints, num_jac_lines) = multibody.num_active_internal_constraints_and_jacobian_lines(); + let ndofs = multibody.ndofs(); + + self.constraint_descs.push(( + total_num_constraints, + ConstraintDesc::GenericMultibodyInternal(link.multibody, *j_id) + )); + + *j_id += ndofs * 2 * num_jac_lines; + total_num_constraints += num_constraints; + had_any_internal_constraint = true; + } + } + } + + if had_any_internal_constraint { + self.parallel_desc_groups.push(self.constraint_descs.len()); + } + } + // Resize the constraint sets. self.velocity_constraints.clear(); self.velocity_constraints @@ -274,10 +295,10 @@ impl_init_constraints_group!( &mut ContactManifold, categorize_contacts, group_manifolds, - data.constraint_index, manifold_body1, manifold_body2, - VelocityConstraint::num_active_constraints, + false, + VelocityConstraint::num_active_constraints_and_jacobian_lines, AnyVelocityConstraint::Empty ); @@ -286,10 +307,10 @@ impl_init_constraints_group!( JointGraphEdge, categorize_joints, group_joints, - constraint_index, joint_body1, joint_body2, - AnyJointVelocityConstraint::num_active_constraints, + true, + AnyJointVelocityConstraint::num_active_constraints_and_jacobian_lines, AnyJointVelocityConstraint::Empty, weight ); @@ -317,32 +338,33 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> { match &desc.1 { ConstraintDesc::NongroundNongrouped(manifold_id) => { let manifold = &*manifolds_all[*manifold_id]; - VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false); + VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GroundNongrouped(manifold_id) => { let manifold = &*manifolds_all[*manifold_id]; - VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false); + VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::NongroundGrouped(manifold_id) => { let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; - WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false); + WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::GroundGrouped(manifold_id) => { let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; - WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false); + WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GenericNongroundNongrouped(manifold_id, j_id) => { let mut j_id = *j_id; let manifold = &*manifolds_all[*manifold_id]; - GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, false); + GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0)); } ConstraintDesc::GenericGroundNongrouped(manifold_id, j_id) => { let mut j_id = *j_id; let manifold = &*manifolds_all[*manifold_id]; - GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, false); + GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0)); } + ConstraintDesc::GenericMultibodyInternal(..) => unreachable!() } } } @@ -372,31 +394,36 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint> { match &desc.1 { ConstraintDesc::NongroundNongrouped(joint_id) => { let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, false); + AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GroundNongrouped(joint_id) => { let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, false); + AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::NongroundGrouped(joint_id) => { let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight]; - AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false); + AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::GroundGrouped(joint_id) => { let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight]; - AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false); + AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GenericNongroundNongrouped(joint_id, j_id) => { let mut j_id = *j_id; let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, false); + AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GenericGroundNongrouped(joint_id, j_id) => { let mut j_id = *j_id; let joint = &joints_all[*joint_id].weight; - AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, false); + AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); + } + ConstraintDesc::GenericMultibodyInternal(multibody_id, j_id) => { + let mut j_id = *j_id; + let multibody = multibodies.get_multibody(*multibody_id).unwrap(); + multibody.generate_internal_constraints(params, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0)); } } } diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index e780e58..c03789a 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -189,7 +189,7 @@ impl SolverConstraints<AnyVelocityConstraint> { manifolds, bodies, &mut self.velocity_constraints, - true, + None, ); } } @@ -213,7 +213,7 @@ impl SolverConstraints<AnyVelocityConstraint> { manifold, bodies, &mut self.velocity_constraints, - true, + None, ); } } @@ -243,7 +243,7 @@ impl SolverConstraints<AnyVelocityConstraint> { &mut self.velocity_constraints, &mut self.generic_jacobians, jacobian_id, - true, + None, ); } } @@ -273,7 +273,7 @@ impl SolverConstraints<AnyVelocityConstraint> { &mut self.velocity_constraints, &mut self.generic_jacobians, jacobian_id, - true, + None, ); } } @@ -303,7 +303,7 @@ impl SolverConstraints<AnyVelocityConstraint> { manifolds, bodies, &mut self.velocity_constraints, - true, + None, ); } } @@ -327,7 +327,7 @@ impl SolverConstraints<AnyVelocityConstraint> { manifold, bodies, &mut self.velocity_constraints, - true, + None, ); } } @@ -457,6 +457,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, + None, ) } } @@ -488,7 +489,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, - true, + None, ); } } @@ -519,7 +520,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { impulse_joints, bodies, &mut self.velocity_constraints, - true, + None, ); } } @@ -548,7 +549,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, - true, + None, ); } } @@ -577,7 +578,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, - true, + None, ) } } @@ -607,7 +608,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, - true, + None, ) } } @@ -637,7 +638,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> { impulse_joints, bodies, &mut self.velocity_constraints, - true, + None, ); } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 186498a..f1c213d 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -136,9 +136,12 @@ pub(crate) struct VelocityConstraint { impl VelocityConstraint { #[cfg(feature = "parallel")] - pub fn num_active_constraints(manifold: &ContactManifold) -> usize { + pub fn num_active_constraints_and_jacobian_lines(manifold: &ContactManifold) -> (usize, usize) { let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; - manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize + ( + manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize, + manifold.data.solver_contacts.len() * DIM, + ) } pub fn generate<Bodies>( @@ -147,7 +150,7 @@ impl VelocityConstraint { manifold: &ContactManifold, bodies: &Bodies, out_constraints: &mut Vec<AnyVelocityConstraint>, - push: bool, + insert_at: Option<usize>, ) where Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyVelocity> @@ -209,7 +212,7 @@ impl VelocityConstraint { // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way // for the moment. #[cfg(target_arch = "wasm32")] - |