diff options
Diffstat (limited to 'src/dynamics/solver')
| -rw-r--r-- | src/dynamics/solver/categorization.rs | 8 | ||||
| -rw-r--r-- | src/dynamics/solver/generic_velocity_constraint.rs | 92 | ||||
| -rw-r--r-- | src/dynamics/solver/generic_velocity_ground_constraint.rs | 30 | ||||
| -rw-r--r-- | src/dynamics/solver/interaction_groups.rs | 55 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_constraint.rs | 129 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_island_solver.rs | 13 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_solver_constraints.rs | 3 | ||||
| -rw-r--r-- | src/dynamics/solver/parallel_velocity_solver.rs | 51 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_constraint.rs | 17 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_wide.rs | 16 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 16 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint_wide.rs | 14 | ||||
| -rw-r--r-- | src/dynamics/solver/velocity_solver.rs | 64 |
13 files changed, 212 insertions, 296 deletions
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 06ba340..d110971 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -50,18 +50,18 @@ pub(crate) fn categorize_joints( ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; - let status1 = bodies.index(joint.body1.0); - let status2 = bodies.index(joint.body2.0); + let rb1 = &bodies[joint.body1.0]; + let rb2 = &bodies[joint.body2.0]; if multibody_joints.rigid_body_link(joint.body1).is_some() || multibody_joints.rigid_body_link(joint.body2).is_some() { - if !status1.is_dynamic() || !status2.is_dynamic() { + if !rb1.is_dynamic() || !rb2.is_dynamic() { generic_ground_joints.push(*joint_i); } else { generic_nonground_joints.push(*joint_i); } - } else if !status1.is_dynamic() || !status2.is_dynamic() { + } else if !rb1.is_dynamic() || !rb2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 1be34bc..318727e 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -1,8 +1,5 @@ use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, - RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -42,18 +39,12 @@ impl GenericVelocityConstraint { let handle1 = manifold.data.rigid_body1.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap(); - let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): ( - &RigidBodyIds, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyType, - ) = bodies.index_bundle(handle1.0); - let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): ( - &RigidBodyIds, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyType, - ) = bodies.index_bundle(handle2.0); + + 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) @@ -63,15 +54,15 @@ impl GenericVelocityConstraint { .map(|m| (&multibodies[m.multibody], m.id)); let mj_lambda1 = multibody1 .map(|mb| mb.0.solver_id) - .unwrap_or(if rb_type1.is_dynamic() { - rb_ids1.active_set_offset + .unwrap_or(if type1.is_dynamic() { + rb1.ids.active_set_offset } else { 0 }); let mj_lambda2 = multibody2 .map(|mb| mb.0.solver_id) - .unwrap_or(if rb_type2.is_dynamic() { - rb_ids2.active_set_offset + .unwrap_or(if type2.is_dynamic() { + rb2.ids.active_set_offset } else { 0 }); @@ -80,11 +71,8 @@ impl GenericVelocityConstraint { #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions( - &force_dir1, - &rb_vels1.linvel, - &rb_vels2.linvel, - ); + 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); @@ -109,13 +97,13 @@ impl GenericVelocityConstraint { #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: if rb_type1.is_dynamic() { - rb_mprops1.effective_inv_mass + im1: if type1.is_dynamic() { + mprops1.effective_inv_mass } else { na::zero() }, - im2: if rb_type2.is_dynamic() { - rb_mprops2.effective_inv_mass + im2: if type2.is_dynamic() { + mprops2.effective_inv_mass } else { na::zero() }, @@ -129,11 +117,11 @@ impl GenericVelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - rb_mprops1.world_com; - let dp2 = manifold_point.point - rb_mprops2.world_com; + let dp1 = manifold_point.point - mprops1.world_com; + let dp2 = manifold_point.point - mprops2.world_com; - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -143,15 +131,15 @@ impl GenericVelocityConstraint { let torque_dir1 = dp1.gcross(force_dir1); let torque_dir2 = dp2.gcross(-force_dir1); - let gcross1 = if rb_type1.is_dynamic() { - rb_mprops1 + let gcross1 = if type1.is_dynamic() { + mprops1 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir1) } else { na::zero() }; - let gcross2 = if rb_type2.is_dynamic() { - rb_mprops2 + let gcross2 = if type2.is_dynamic() { + mprops2 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir2) } else { @@ -170,8 +158,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type1.is_dynamic() { - force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + gcross1.gdot(gcross1) } else { 0.0 @@ -189,8 +177,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type2.is_dynamic() { - force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2) } else { 0.0 @@ -224,8 +212,8 @@ impl GenericVelocityConstraint { for j in 0..DIM - 1 { let torque_dir1 = dp1.gcross(tangents1[j]); - let gcross1 = if rb_type1.is_dynamic() { - rb_mprops1 + let gcross1 = if type1.is_dynamic() { + mprops1 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir1) } else { @@ -234,8 +222,8 @@ impl GenericVelocityConstraint { constraint.elements[k].tangent_part.gcross1[j] = gcross1; let torque_dir2 = dp2.gcross(-tangents1[j]); - let gcross2 = if rb_type2.is_dynamic() { - rb_mprops2 + let gcross2 = if type2.is_dynamic() { + mprops2 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir2) } else { @@ -255,9 +243,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type1.is_dynamic() { - force_dir1 - .dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + gcross1.gdot(gcross1) } else { 0.0 @@ -275,9 +262,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type2.is_dynamic() { - force_dir1 - .dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2) } else { 0.0 @@ -303,8 +289,8 @@ impl GenericVelocityConstraint { // 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) - | (!rb_type1.is_dynamic() as u8) - | ((!rb_type2.is_dynamic() as u8) << 1); + | (!type1.is_dynamic() as u8) + | ((!type2.is_dynamic() as u8) << 1); let constraint = GenericVelocityConstraint { velocity_constraint: constraint, diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index d953e6f..16648c4 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -1,7 +1,5 @@ use crate::dynamics::solver::VelocityGroundConstraint; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::WCross; @@ -48,16 +46,15 @@ impl GenericVelocityGroundConstraint { (-manifold.data.normal, 1.0) }; - let (rb_vels1, world_com1) = if let Some(handle1) = handle1 { - let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle1.0); - (*vels1, mprops1.world_com) + 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 (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle2.unwrap().0); + let rb2 = &bodies[handle2.unwrap()]; + let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); let (mb2, link_id2) = handle2 .and_then(|h| multibodies.rigid_body_link(h)) @@ -68,11 +65,8 @@ impl GenericVelocityGroundConstraint { #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions( - &force_dir1, - &rb_vels1.linvel, - &rb_vels2.linvel, - ); + 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 @@ -96,7 +90,7 @@ impl GenericVelocityGroundConstraint { #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: rb_mprops2.effective_inv_mass, + im2: mprops2.effective_inv_mass, limit: 0.0, mj_lambda2, manifold_id, @@ -107,10 +101,10 @@ impl GenericVelocityGroundConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; let dp1 = manifold_point.point - world_com1; - let dp2 = manifold_point.point - rb_mprops2.world_com; + let dp2 = manifold_point.point - mprops2.world_com; - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 951b77f..aecd12d 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -3,7 +3,6 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { - crate::data::BundleSet, crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, vec_map::VecMap, }; @@ -90,14 +89,8 @@ impl ParallelInteractionGroups { .zip(self.interaction_colors.iter_mut()) { let mut body_pair = interactions[*interaction_id].body_pair(); - let is_fixed1 = body_pair - .0 - .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed()) - .unwrap_or(true); - let is_fixed2 = body_pair - .1 - .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed()) - .unwrap_or(true); + let is_fixed1 = body_pair.0.map(|b| bodies[b].is_fixed()).unwrap_or(true); + let is_fixed2 = body_pair.1.map(|b| bodies[b].is_fixed()).unwrap_or(true); let representative = |handle: RigidBodyHandle| { if let Some(link) = multibodies.rigid_body_link(handle).copied() { @@ -119,28 +112,28 @@ impl ParallelInteractionGroups { match (is_fixed1, is_fixed2) { (false, false) => { - let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0); - let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); + let rb1 = &bodies[body_pair.0.unwrap()]; + let rb2 = &bodies[body_pair.1.unwrap()]; let color_mask = - bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset]; + bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb_ids1.active_set_offset] |= 1 << *color; - bcolors[rb_ids2.active_set_offset] |= 1 << *color; + bcolors[rb1.ids.active_set_offset] |= 1 << *color; + bcolors[rb2.ids.active_set_offset] |= 1 << *color; } (true, false) => { - let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); - let color_mask = bcolors[rb_ids2.active_set_offset]; + let rb2 = &bodies[body_pair.1.unwrap()]; + let color_mask = bcolors[rb2.ids.active_set_offset]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb_ids2.active_set_offset] |= 1 << *color; + bcolors[rb2.ids.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]; + let rb1 = &bodies[body_pair.0.unwrap()]; + let color_mask = bcolors[rb1.ids.active_set_offset]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb_ids1.active_set_offset] |= 1 << *color; + bcolors[rb1.ids.active_set_offset] |= 1 << *color; } (true, true) => unreachable!(), } @@ -258,13 +251,11 @@ impl InteractionGroups { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i].weight; - let (status1, ids1): (&RigidBodyType, &RigidBodyIds) = - bodies.index_bundle(interaction.body1.0); - let (status2, ids2): (&RigidBodyType, &RigidBodyIds) = - bodies.index_bundle(interaction.body2.0); + let rb1 = &bodies[interaction.body1]; + let rb2 = &bodies[interaction.body2]; - let is_fixed1 = !status1.is_dynamic(); - let is_fixed2 = !status2.is_dynamic(); + let is_fixed1 = !rb1.is_dynamic(); + let is_fixed2 = !rb2.is_dynamic(); if is_fixed1 && is_fixed2 { continue; @@ -277,8 +268,8 @@ impl InteractionGroups { } let ijoint = interaction.data.locked_axes.bits() as usize; - let i1 = ids1.active_set_offset; - let i2 = ids2.active_set_offset; + let i1 = rb1.ids.active_set_offset; + let i2 = rb2.ids.active_set_offset; let conflicts = self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. @@ -421,15 +412,15 @@ impl InteractionGroups { let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 { - let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0); - (*data.0, data.1.active_set_offset) + let rb1 = &bodies[rb1]; + (rb1.body_type, rb1.ids.active_set_offset) } else { (RigidBodyType::Fixed, usize::MAX) }; let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 { - let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0); - (*data.0, data.1.active_set_offset) + let rb2 = &bodies[rb2]; + (rb2.body_type, rb2.ids.active_set_offset) } else { (RigidBodyType::Fixed, usize::MAX) }; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index a46744d..962602f 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -6,8 +6,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, RigidBodySet, RigidBodyType, RigidBodyVelocity, + ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, }; use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::MultibodyJointSet; @@ -63,39 +62,26 @@ impl AnyJointVelocityConstraint { ) { let local_frame1 = joint.data.local_frame1; let local_frame2 = joint.data.local_frame2; - let rb1: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body1.0); - let rb2: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body2.0); - - let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1; - let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2; - let frame1 = rb_pos1.position * local_frame1; - let frame2 = rb_pos2.position * local_frame2; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + let frame1 = rb1.pos.position * local_frame1; + let frame2 = rb2.pos.position * local_frame2; let body1 = SolverBody { - linvel: rb_vel1.linvel, - angvel: rb_vel1.angvel, - im: rb_mprops1.effective_inv_mass, - sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt, - world_com: rb_mprops1.world_com, - mj_lambda: [rb_ids1.active_set_offset], + linvel: rb1.vels.linvel, + angvel: rb1.vels.angvel, + im: rb1.mprops.effective_inv_mass, + sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, + world_com: rb1.mprops.world_com, + mj_lambda: [rb1.ids.active_set_offset], }; let body2 = SolverBody { - linvel: rb_vel2.linvel, - angvel: rb_vel2.angvel, - im: rb_mprops2.effective_inv_mass, - sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt, - world_com: rb_mprops2.world_com, - mj_lambda: [rb_ids2.active_set_offset], + linvel: rb2.vels.linvel, + angvel: rb2.vels.angvel, + im: rb2.mprops.effective_inv_mass, + sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, + world_com: rb2.mprops.world_com, + mj_lambda: [rb2.ids.active_set_offset], }; let mb1 = multibodies @@ -186,16 +172,20 @@ impl AnyJointVelocityConstraint { out: &mut Vec<Self>, insert_at: Option<usize>, ) { + use crate::dynamics::{ + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, + }; + let rbs1: ( [&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyVelocity; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], + gather![|ii| &bodies[impulse_joints[ii].body1].pos], + gather![|ii| &bodies[impulse_joints[ii].body1].vels], + gather![|ii| &bodies[impulse_joints[ii].body1].mprops], + gather![|ii| &bodies[impulse_joints[ii].body1].ids], ); let rbs2: ( [&RigidBodyPosition; SIMD_WIDTH], @@ -203,10 +193,10 @@ impl AnyJointVelocityConstraint { [&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], + gather![|ii| &bodies[impulse_joints[ii].body2].pos], + gather![|ii| &bodies[impulse_joints[ii].body2].vels], + gather![|ii| &bodies[impulse_joints[ii].body2].mprops], + gather![|ii| &bodies[impulse_joints[ii].body2].ids], ); let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1; @@ -276,8 +266,7 @@ impl AnyJointVelocityConstraint { ) { let mut handle1 = joint.body1; let mut handle2 = joint.body2; - let status2: &RigidBodyType = bodies.index(handle2.0); - let flipped = !status2.is_dynamic(); + let flipped = !bodies[handle2].is_dynamic(); let (local_frame1, local_frame2) = if flipped { std::mem::swap(&mut handle1, &mut handle2); @@ -286,35 +275,27 @@ impl AnyJointVelocityConstraint { (joint.data.local_frame1, joint.data.local_frame2) }; - let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle1.0); - let rb2: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ) = bodies.index_bundle(handle2.0); + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; - let (rb_pos1, rb_vel1, rb_mprops1) = rb1; - let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2; - let frame1 = rb_pos1.position * local_frame1; - let frame2 = rb_pos2.position * local_frame2; + let frame1 = rb1.pos.position * local_frame1; + let frame2 = rb2.pos.position * local_frame2; let body1 = SolverBody { - linvel: rb_vel1.linvel, - angvel: rb_vel1.angvel, - im: rb_mprops1.effective_inv_mass, - sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt, - world_com: rb_mprops1.world_com, + linvel: rb1.vels.linvel, + angvel: rb1.vels.angvel, + im: rb1.mprops.effective_inv_mass, + sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, + world_com: rb1.mprops.world_com, mj_lambda: [crate::INVALID_USIZE], }; let body2 = SolverBody { - linvel: rb_vel2.linvel, - angvel: rb_vel2.angvel, - im: rb_mprops2.effective_inv_mass, - sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt, - world_com: rb_mprops2.world_com, - mj_lambda: [rb_ids2.active_set_offset], + linvel: rb2.vels.linvel, + angvel: rb2.vels.angvel, + im: rb2.mprops.effective_inv_mass, + sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, + world_com: rb2.mprops.world_com, + mj_lambda: [rb2.ids.active_set_offset], }; if let Some(mb2) = multibodies @@ -399,9 +380,13 @@ impl AnyJointVelocityConstraint { out: &mut Vec<Self>, insert_at: Option<usize>, ) { + use crate::dynamics::{ + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + }; + 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)]; + let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -429,9 +414,9 @@ impl AnyJointVelocityConstraint { [&RigidBodyVelocity; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(handles1[ii].0)], - gather![|ii| bodies.index(handles1[ii].0)], - gather![|ii| bodies.index(handles1[ii].0)], + gather![|ii| &bodies[handles1[ii]].pos], + gather![|ii| &bodies[handles1[ii]].vels], + gather![|ii| &bodies[handles1[ii]].mprops], ); let rbs2: ( [&RigidBodyPosition; SIMD_WIDTH], @@ -439,10 +424,10 @@ impl AnyJointVelocityConstraint { [&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(handles2[ii].0)], - gather![|ii| bodies.index(handles2[ii].0)], - gather![|ii| bodies.index(handles2[ii].0)], - gather![|ii| bodies.index(handles2[ii].0)], + gather![|ii| &bodies[handles2[ii]].pos], + gather![|ii| &bodies[handles2[ii]].vels], + gather![|ii| &bodies[handles2[ii]].mprops], + gather![|ii| &bodies[handles2[ii]].ids], ); let (rb_pos1, rb_vel1, rb_mprops1) = rbs1; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 261c627..905de46 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -7,8 +7,7 @@ use crate::dynamics::solver::{ }; use crate::dynamics::{ IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, - RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, - RigidBodyType, RigidBodyVelocity, + RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use na::DVector; @@ -316,15 +315,13 @@ impl ParallelIslandSolver { mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); } } else { - let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) = - bodies.index_bundle(handle.0); - - let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset]; + let rb = &bodies[*handle]; + let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: - dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; - dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt; + dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt; + dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt; } } } diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index d883494..46f1a57 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -8,8 +8,7 @@ use crate::dynamics::solver::{ }; use crate::dynamics::{ ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex, - MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, - RigidBodyType, RigidBodyVelocity, + MultibodyJointSet, RigidBodyHandle, RigidBodySet, }; use crate::geometry::ContactManifold; use crate::math::{Real, SPATIAL_DIM}; diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 5b062e0..ab34a42 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -2,8 +2,7 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadC use crate::concurrent_loop; use crate::dynamics::{ solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, - MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, - RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + MultibodyJointSet, RigidBodySet, }; use crate::geometry::ContactManifold; use crate::math::Real; @@ -210,33 +209,22 @@ impl ParallelVelocitySolver { multibody.velocities = prev_vels; } } else { - let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) = - bodies.index_bundle(handle.0); - - let dvel = self.mj_lambdas[rb_ids.active_set_offset]; - let dangvel = rb_mprops + let rb = bodies.index_mut_internal(*handle); + let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dangvel = rb.mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); // Update positions. - let (rb_pos, rb_vels, rb_damping, rb_mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let mut new_pos = *rb_pos; - let mut new_vels = *rb_vels; + let mut new_vels = rb.vels; new_vels.linvel += dvel.linear; new_vels.angvel += dangvel; - new_vels = new_vels.apply_damping(params.dt, rb_damping); - new_pos.next_position = new_vels.integrate( + new_vels = new_vels.apply_damping(params.dt, &rb.damping); + rb.pos.next_position = new_vels.integrate( params.dt, - &rb_pos.position, - &rb_mprops.local_mprops.local_com, + &rb.pos.position, + &rb.mprops.local_mprops.local_com, ); - bodies.set_internal(handle.0, new_pos); |