From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- src/dynamics/solver/categorization.rs | 13 +- src/dynamics/solver/interaction_groups.rs | 96 ++++--- src/dynamics/solver/island_solver.rs | 92 +++++-- .../joint_constraint/ball_position_constraint.rs | 56 ++-- .../ball_position_constraint_wide.rs | 83 +++--- .../joint_constraint/ball_velocity_constraint.rs | 114 ++++---- .../ball_velocity_constraint_wide.rs | 190 +++++++------ .../joint_constraint/fixed_position_constraint.rs | 48 ++-- .../fixed_position_constraint_wide.rs | 34 ++- .../joint_constraint/fixed_velocity_constraint.rs | 86 +++--- .../fixed_velocity_constraint_wide.rs | 196 +++++++------- .../generic_position_constraint.rs | 2 +- .../generic_position_constraint_wide.rs | 13 +- .../generic_velocity_constraint.rs | 16 +- .../generic_velocity_constraint_wide.rs | 154 +++++------ .../solver/joint_constraint/joint_constraint.rs | 139 +++++++--- .../joint_constraint/joint_position_constraint.rs | 99 ++++--- .../prismatic_position_constraint.rs | 42 +-- .../prismatic_position_constraint_wide.rs | 34 ++- .../prismatic_velocity_constraint.rs | 138 ++++++---- .../prismatic_velocity_constraint_wide.rs | 296 +++++++++++---------- .../revolute_position_constraint.rs | 60 +++-- .../revolute_position_constraint_wide.rs | 34 ++- .../revolute_velocity_constraint.rs | 140 ++++++---- .../revolute_velocity_constraint_wide.rs | 258 ++++++++++-------- src/dynamics/solver/parallel_island_solver.rs | 39 +-- src/dynamics/solver/parallel_solver_constraints.rs | 26 +- src/dynamics/solver/position_constraint.rs | 44 +-- src/dynamics/solver/position_constraint_wide.rs | 69 ++--- src/dynamics/solver/position_ground_constraint.rs | 33 ++- .../solver/position_ground_constraint_wide.rs | 54 ++-- src/dynamics/solver/position_solver.rs | 31 ++- src/dynamics/solver/solver_constraints.rs | 152 ++++++++--- src/dynamics/solver/velocity_constraint.rs | 60 +++-- src/dynamics/solver/velocity_constraint_wide.rs | 144 +++++----- src/dynamics/solver/velocity_ground_constraint.rs | 56 ++-- .../solver/velocity_ground_constraint_wide.rs | 123 +++++---- src/dynamics/solver/velocity_solver.rs | 51 ++-- 38 files changed, 1984 insertions(+), 1331 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index e3327c6..366a5b3 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,8 +1,9 @@ -use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; +use crate::data::ComponentSet; +use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( - _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code. + _bodies: &impl ComponentSet, // Unused but useful to simplify the parallel code. manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec, @@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts( } pub(crate) fn categorize_joints( - bodies: &RigidBodySet, + bodies: &impl ComponentSet, joints: &[JointGraphEdge], joint_indices: &[JointIndex], ground_joints: &mut Vec, @@ -28,10 +29,10 @@ pub(crate) fn categorize_joints( ) { for joint_i in joint_indices { let joint = &joints[*joint_i].weight; - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; + let status1 = bodies.index(joint.body1.0); + let status2 = bodies.index(joint.body2.0); - if !rb1.is_dynamic() || !rb2.is_dynamic() { + if !status1.is_dynamic() || !status2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 6b8de5a..86f9cff 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,4 +1,8 @@ -use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::data::{BundleSet, ComponentSet}; +#[cfg(feature = "parallel")] +use crate::dynamics::BodyPair; +use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyType}; +use crate::dynamics::{JointGraphEdge, JointIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { @@ -6,16 +10,19 @@ use { vec_map::VecMap, }; +#[cfg(feature = "parallel")] pub(crate) trait PairInteraction { fn body_pair(&self) -> BodyPair; } +#[cfg(feature = "parallel")] impl<'a> PairInteraction for &'a mut ContactManifold { fn body_pair(&self) -> BodyPair { self.data.body_pair } } +#[cfg(feature = "parallel")] impl<'a> PairInteraction for JointGraphEdge { fn body_pair(&self) -> BodyPair { BodyPair::new(self.weight.body1, self.weight.body2) @@ -54,7 +61,7 @@ impl ParallelInteractionGroups { pub fn group_interactions( &mut self, island_id: usize, - bodies: &RigidBodySet, + bodies: &impl ComponentSet, interactions: &[Interaction], interaction_indices: &[usize], ) { @@ -78,8 +85,8 @@ impl ParallelInteractionGroups { .zip(self.interaction_colors.iter_mut()) { let body_pair = interactions[*interaction_id].body_pair(); - let rb1 = &bodies[body_pair.body1]; - let rb2 = &bodies[body_pair.body2]; + let rb1 = bodies.index(body_pair.body1); + let rb2 = bodies.index(body_pair.body2); match (rb1.is_static(), rb2.is_static()) { (false, false) => { @@ -168,14 +175,15 @@ impl InteractionGroups { self.nongrouped_interactions.clear(); } - // FIXME: there is a lot of duplicated code with group_manifolds here. + // TODO: there is a lot of duplicated code with group_manifolds here. // But we don't refactor just now because we may end up with distinct // grouping strategies in the future. #[cfg(not(feature = "simd-is-enabled"))] pub fn group_joints( &mut self, _island_id: usize, - _bodies: &RigidBodySet, + _islands: &IslandManager, + _bodies: &impl ComponentSet, _interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], ) { @@ -184,13 +192,16 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_joints( + pub fn group_joints( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet + ComponentSet, + { // NOTE: in 3D we have up to 10 different joint types. // In 2D we only have 5 joint types. #[cfg(feature = "dim3")] @@ -204,11 +215,11 @@ impl InteractionGroups { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. - // FIXME: currently, this is a bit overconservative because when a bucket + // TODO: currently, this is a bit overconservative because when a bucket // is full, we don't clear the corresponding body mask bit. This may result // in less grouped constraints. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -216,10 +227,14 @@ impl InteractionGroups { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i].weight; - let body1 = &bodies[interaction.body1]; - let body2 = &bodies[interaction.body2]; - let is_static1 = !body1.is_dynamic(); - let is_static2 = !body2.is_dynamic(); + + let (status1, ids1): (&RigidBodyType, &RigidBodyIds) = + bodies.index_bundle(interaction.body1.0); + let (status2, ids2): (&RigidBodyType, &RigidBodyIds) = + bodies.index_bundle(interaction.body2.0); + + let is_static1 = !status1.is_dynamic(); + let is_static2 = !status2.is_dynamic(); if is_static1 && is_static2 { continue; @@ -232,8 +247,8 @@ impl InteractionGroups { } let ijoint = interaction.params.type_id(); - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = ids1.active_set_offset; + let i2 = ids2.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. @@ -325,7 +340,8 @@ impl InteractionGroups { pub fn group_manifolds( &mut self, _island_id: usize, - _bodies: &RigidBodySet, + _islands: &IslandManager, + _bodies: &impl ComponentSet, _interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], ) { @@ -334,21 +350,24 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_manifolds( + pub fn group_manifolds( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, 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. - // FIXME: currently, this is a bit overconservative because when a bucket + // TODO: currently, this is a bit overconservative because when a bucket // is full, we don't clear the corresponding body mask bit. This may result // in less grouped contacts. // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -359,31 +378,44 @@ impl InteractionGroups { .max() .unwrap_or(1); - // FIXME: find a way to reduce the number of iteration. + // TODO: find a way to reduce the number of iteration. // There must be a way to iterate just once on every interaction indices // instead of MAX_MANIFOLD_POINTS times. for k in 1..=max_interaction_points { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i]; - // FIXME: how could we avoid iterating + // TODO: how could we avoid iterating // on each interaction at every iteration on k? if interaction.data.num_active_contacts() != k { continue; } - let body1 = &bodies[interaction.data.body_pair.body1]; - let body2 = &bodies[interaction.data.body_pair.body2]; - let is_static1 = !body1.is_dynamic(); - let is_static2 = !body2.is_dynamic(); + 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) + } else { + (RigidBodyType::Static, 0) + }; + 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) + } else { + (RigidBodyType::Static, 0) + }; + + let is_static1 = !status1.is_dynamic(); + let is_static2 = !status2.is_dynamic(); - // FIXME: don't generate interactions between static bodies in the first place. + // TODO: don't generate interactions between static bodies in the first place. if is_static1 && is_static2 { continue; } - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = active_set_offset1; + let i2 = active_set_offset2; let conflicts = self.body_masks[i1] | self.body_masks[i2]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index c684cc5..1e65341 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,10 +1,15 @@ use super::{PositionSolver, VelocitySolver}; use crate::counters::Counters; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, +}; +use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub struct IslandSolver { @@ -24,17 +29,21 @@ impl IslandSolver { } } - pub fn solve_position_constraints( + pub fn solve_position_constraints( &mut self, island_id: usize, + islands: &IslandManager, counters: &mut Counters, params: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + bodies: &mut Bodies, + ) where + Bodies: ComponentSet + ComponentSetMut, + { counters.solver.position_resolution_time.resume(); self.position_solver.solve( island_id, params, + islands, bodies, &self.contact_constraints.position_constraints, &self.joint_constraints.position_constraints, @@ -42,31 +51,47 @@ impl IslandSolver { counters.solver.position_resolution_time.pause(); } - pub fn init_constraints_and_solve_velocity_constraints( + pub fn init_constraints_and_solve_velocity_constraints( &mut self, island_id: usize, counters: &mut Counters, params: &IntegrationParameters, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; if has_constraints { counters.solver.velocity_assembly_time.resume(); - self.contact_constraints - .init(island_id, params, bodies, manifolds, manifold_indices); + self.contact_constraints.init( + island_id, + params, + islands, + bodies, + manifolds, + manifold_indices, + ); self.joint_constraints - .init(island_id, params, bodies, joints, joint_indices); + .init(island_id, params, islands, bodies, joints, joint_indices); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); self.velocity_solver.solve( island_id, params, + islands, bodies, manifolds, joints, @@ -76,21 +101,50 @@ impl IslandSolver { counters.solver.velocity_resolution_time.pause(); counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); - }); + + for handle in islands.active_island(island_id) { + let (poss, vels, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_poss = *poss; + let new_vels = vels.apply_damping(params.dt, damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } + counters.solver.velocity_update_time.pause(); } else { self.contact_constraints.clear(); self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { + + for handle in islands.active_island(island_id) { // Since we didn't run the velocity solver we need to integrate the accelerations here - rb.integrate_accelerations(params.dt); - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); - }); + let (poss, vels, forces, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_poss = *poss; + let new_vels = forces + .integrate(params.dt, vels, mprops) + .apply_damping(params.dt, &damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } counters.solver.velocity_update_time.pause(); } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 93996f4..159cc77 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; @@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint { } impl BallPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &BallJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + Self { - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, - im1: rb1.effective_inv_mass, - im2: rb2.effective_inv_mass, - ii1: rb1.effective_world_inv_inertia_sqrt.squared(), - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, + im1: mprops1.effective_inv_mass, + im2: mprops2.effective_inv_mass, + ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, } } @@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint { impl BallPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &BallJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + if flipped { // Note the only thing that is flipped here // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.next_position * cparams.local_anchor2, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor2, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } else { Self { - anchor1: rb1.next_position * cparams.local_anchor1, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor1, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index e9162a4..6b00639 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; @@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint { impl WBallPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], ) -> Self { - let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); - let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + let (mprops1, ids1) = rbs1; + let (mprops2, ids2) = rbs2; + + let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii1 = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + let ii2 = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let position1 = gather![|ii| ids1[ii].active_set_offset]; + let position2 = gather![|ii| ids2[ii].active_set_offset]; Self { local_com1, @@ -61,8 +72,8 @@ impl WBallPositionConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); - let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]); + let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); let anchor1 = position1 * self.local_anchor1; let anchor2 = position2 * self.local_anchor2; @@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint { impl WBallPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]); + let poss1 = rbs1; + let (mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]); let anchor1 = position1 - * Point::from(array![|ii| if flipped[ii] { + * Point::from(gather![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 - }; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + }]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2 = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let local_anchor2 = Point::from(array![|ii| if flipped[ii] { + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 - }; SIMD_WIDTH]); - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + }]); + let position2 = gather![|ii| ids2[ii].active_set_offset]; + let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); Self { anchor1, @@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); let anchor2 = position2 * self.local_anchor2; let com2 = position2 * self.local_com2; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 508ac65..9e5227e 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -36,19 +37,32 @@ impl BallVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &BallJoint, ) -> Self { - let anchor_world1 = rb1.position * joint.local_anchor1; - let anchor_world2 = rb2.position * joint.local_anchor2; - let anchor1 = anchor_world1 - rb1.world_com; - let anchor2 = anchor_world2 - rb2.world_com; + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; - let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); - let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + let anchor_world1 = poss1.position * joint.local_anchor1; + let anchor_world2 = poss2.position * joint.local_anchor2; + let anchor1 = anchor_world1 - mprops1.world_com; + let anchor2 = anchor_world2 - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); + let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -59,12 +73,12 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] { - lhs = rb2 + lhs = mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2) - + rb1 + + mprops1 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat1) @@ -75,8 +89,8 @@ impl BallVelocityConstraint { // it's just easier that way. #[cfg(feature = "dim2")] { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; @@ -100,8 +114,8 @@ impl BallVelocityConstraint { ); if stiffness != 0.0 { - let dpos = rb2.position.rotation - * (rb1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = poss2.position.rotation + * (poss1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -113,15 +127,15 @@ impl BallVelocityConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; + let curr_vel = vels2.angvel - vels1.angvel; motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } #[cfg(feature = "dim2")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / (ii1 + ii2)) } else { Some(gamma) @@ -132,8 +146,8 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some((ii1 + ii2).inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -151,8 +165,8 @@ impl BallVelocityConstraint { BallVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, im2, impulse: joint.impulse * params.warmstart_coeff, @@ -164,8 +178,8 @@ impl BallVelocityConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, } } @@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &BallJoint, flipped: bool, ) -> Self { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let (anchor_world1, anchor_world2) = if flipped { ( - rb1.position * joint.local_anchor2, - rb2.position * joint.local_anchor1, + poss1.position * joint.local_anchor2, + poss2.position * joint.local_anchor1, ) } else { ( - rb1.position * joint.local_anchor1, - rb2.position * joint.local_anchor2, + poss1.position * joint.local_anchor1, + poss2.position * joint.local_anchor2, ) }; - let anchor1 = anchor_world1 - rb1.world_com; - let anchor2 = anchor_world2 - rb2.world_com; + let anchor1 = anchor_world1 - mprops1.world_com; + let anchor2 = anchor_world2 - mprops2.world_com; - let im2 = rb2.effective_inv_mass; - let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); - let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let im2 = mprops2.effective_inv_mass; + let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); + let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] { - lhs = rb2 + lhs = mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) @@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim2")] { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im2 + cmat2.x * cmat2.x * ii2; let m12 = cmat2.x * cmat2.y * ii2; let m22 = im2 + cmat2.y * cmat2.y * ii2; @@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint { ); if stiffness != 0.0 { - let dpos = rb2.position.rotation - * (rb1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = poss2.position.rotation + * (poss1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; + let curr_vel = vels2.angvel - vels1.angvel; motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } #[cfg(feature = "dim2")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / ii2) } else { Some(gamma) @@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(ii2.inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint { BallVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, impulse: joint.impulse * params.warmstart_coeff, r2: anchor2, @@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, } } diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index ff5e001..ee465cd 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, @@ -34,33 +35,46 @@ impl WBallVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); let anchor_world1 = position1 * local_anchor1; let anchor_world2 = position2 * local_anchor2; @@ -114,20 +128,16 @@ impl WBallVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; mj_lambda1.linear += self.impulse * self.im1; @@ -147,20 +157,16 @@ impl WBallVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let local_anchor1 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], - ); - - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - - let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let local_anchor1 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); let anchor_world1 = position1 * local_anchor1; let anchor_world2 = position2 * local_anchor2; @@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; mj_lambda2.linear -= self.impulse * self.im2; @@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index c98660f..239138f 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::WAngularInertia; @@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint { } impl FixedPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &FixedJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); Self { local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, im1, im2, ii1, ii2, - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, lin_inv_lhs, ang_inv_lhs, } @@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint { impl FixedPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &FixedJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + let anchor1; let local_anchor2; if flipped { - anchor1 = rb1.next_position * cparams.local_anchor2; + anchor1 = poss1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; } else { - anchor1 = rb1.next_position * cparams.local_anchor1; + anchor1 = poss1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; }; Self { anchor1, local_anchor2, - position2: rb2.active_set_offset, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), + local_com2: mprops2.mass_properties.local_com, impulse: 0.0, } } diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs index 2648816..0c0a6fd 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs @@ -1,5 +1,7 @@ use super::{FixedPositionConstraint, FixedPositionGroundConstraint}; -use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{Isometry, Real, SIMD_WIDTH}; // TODO: this does not uses SIMD optimizations yet. @@ -10,12 +12,22 @@ pub(crate) struct WFixedPositionConstraint { impl WFixedPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| FixedPositionConstraint::from_params( + (rbs1.0[ii], rbs1.1[ii]), + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii] + )], } } @@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint { impl WFixedPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + constraints: gather![|ii| FixedPositionGroundConstraint::from_params( + rbs1[ii], + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii], + flipped[ii] + )], } } diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index 11ade23..a0c0739 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -45,18 +46,31 @@ impl FixedVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), cparams: &FixedJoint, ) -> Self { - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1.translation.vector - rb1.world_com.coords; - let r2 = anchor2.translation.vector - rb2.world_com.coords; + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + + let anchor1 = poss1.position * cparams.local_anchor1; + let anchor2 = poss2.position * cparams.local_anchor2; + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - mprops1.world_com.coords; + let r2 = anchor2.translation.vector - mprops2.world_com.coords; let rmat1 = r1.gcross_matrix(); let rmat2 = r2.gcross_matrix(); @@ -99,8 +113,9 @@ impl FixedVelocityConstraint { #[cfg(feature = "dim3")] let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); - let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); - let ang_dvel = -rb1.angvel + rb2.angvel; + let lin_dvel = + -vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2); + let ang_dvel = -vels1.angvel + vels2.angvel; #[cfg(feature = "dim2")] let mut rhs = @@ -133,14 +148,14 @@ impl FixedVelocityConstraint { FixedVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, im2, ii1, ii2, -