diff options
18 files changed, 227 insertions, 154 deletions
diff --git a/.vscode/tasks.json b/.vscode/tasks.json index c445107..285a051 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -57,6 +57,21 @@ "group": "build" }, { + "label": "run 3d (simd - parallel - debug) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples3", + "--features", + "simd-stable,parallel", + "--", + "--pause" + ], + "group": "build" + }, + { "label": "run 2d (no-simd - release) ", "type": "shell", "command": "cargo", @@ -27,6 +27,9 @@ resolver = "2" #opt-level = 1 #lto = true +[profile.dev] +opt-level = 1 + #[profile.dev.package.rapier3d] #opt-level = 3 diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs index 4677a1f..f80380e 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -15,5 +15,4 @@ pub struct ImpulseJoint { // A joint needs to know its handle to simplify its removal. pub(crate) handle: ImpulseJointHandle, - pub(crate) constraint_index: usize, } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index cd11a61..448b87d 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -187,7 +187,6 @@ impl ImpulseJointSet { data, impulses: na::zero(), handle: ImpulseJointHandle(handle), - constraint_index: 0, }; let default_id = InteractionGraph::<(), ()>::invalid_graph_index(); diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index f2fa623..58d11b6 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -971,27 +971,48 @@ impl Multibody { } #[inline] + pub fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) { + let num_constraints: usize = self + .links + .iter() + .map(|l| l.joint().num_velocity_constraints()) + .sum(); + (num_constraints, num_constraints) + } + + #[inline] pub fn generate_internal_constraints( &self, params: &IntegrationParameters, j_id: &mut usize, jacobians: &mut DVector<Real>, out: &mut Vec<AnyJointVelocityConstraint>, + mut insert_at: Option<usize>, ) { - let num_constraints: usize = self - .links - .iter() - .map(|l| l.joint().num_velocity_constraints()) - .sum(); - - let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; - if jacobians.nrows() < required_jacobian_len { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + if !cfg!(feature = "parallel") { + let num_constraints: usize = self + .links + .iter() + .map(|l| l.joint().num_velocity_constraints()) + .sum(); + + let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; + if jacobians.nrows() < required_jacobian_len { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } } for link in self.links.iter() { - link.joint() - .velocity_constraints(params, self, link, 0, j_id, jacobians, out); + link.joint().velocity_constraints( + params, + self, + link, + 0, + j_id, + jacobians, + out, + &mut insert_at, + ); } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 2f7a71e..5a04070 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -255,6 +255,7 @@ impl MultibodyJoint { j_id: &mut usize, jacobians: &mut DVector<Real>, constraints: &mut Vec<AnyJointVelocityConstraint>, + insert_at: &mut Option<usize>, ) { let locked_bits = self.data.locked_axes.bits(); let limit_bits = self.data.limit_axes.bits(); @@ -281,6 +282,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); } @@ -295,6 +297,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); } curr_free_dof += 1; @@ -329,6 +332,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); Some(limits) } else { @@ -347,6 +351,7 @@ impl MultibodyJoint { j_id, jacobians, constraints, + insert_at, ); } curr_free_dof += 1; diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 42212be..a1ec483 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -20,6 +20,7 @@ pub fn unit_joint_limit_constraint( j_id: &mut usize, jacobians: &mut DVector<Real>, constraints: &mut Vec<AnyJointVelocityConstraint>, + insert_at: &mut Option<usize>, ) { let ndofs = multibody.ndofs(); let joint_velocity = multibody.joint_velocity(link); @@ -60,9 +61,14 @@ pub fn unit_joint_limit_constraint( writeback_id: WritebackId::Limit(dof_id), }; - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); + if let Some(at) = insert_at { + constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); + *at += 1; + } else { + constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( + constraint, + )); + } *j_id += 2 * ndofs; } @@ -79,6 +85,7 @@ pub fn unit_joint_motor_constraint( j_id: &mut usize, jacobians: &mut DVector<Real>, constraints: &mut Vec<AnyJointVelocityConstraint>, + insert_at: &mut Option<usize>, ) { let inv_dt = params.inv_dt(); let ndofs = multibody.ndofs(); @@ -128,8 +135,13 @@ pub fn unit_joint_motor_constraint( writeback_id: WritebackId::Limit(dof_id), }; - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); + if let Some(at) = insert_at { + constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); + *at += 1; + } else { + constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( + constraint, + )); + } *j_id += 2 * ndofs; } 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::Nongro |