From cf77d17d9e6c425b1899c03db8e07f265259791b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 3 Feb 2021 16:33:08 +0100 Subject: Experiment with incremental island computation. --- src/dynamics/solver/interaction_groups.rs | 31 +++++++++++----------- src/dynamics/solver/island_solver.rs | 23 ++++++++++++---- .../joint_constraint/ball_position_constraint.rs | 8 +++--- .../ball_position_constraint_wide.rs | 6 ++--- .../joint_constraint/ball_velocity_constraint.rs | 6 ++--- .../ball_velocity_constraint_wide.rs | 6 ++--- .../joint_constraint/fixed_position_constraint.rs | 6 ++--- .../joint_constraint/fixed_velocity_constraint.rs | 6 ++--- .../fixed_velocity_constraint_wide.rs | 6 ++--- .../prismatic_position_constraint.rs | 6 ++--- .../prismatic_velocity_constraint.rs | 6 ++--- .../prismatic_velocity_constraint_wide.rs | 6 ++--- .../revolute_position_constraint.rs | 6 ++--- .../revolute_velocity_constraint.rs | 6 ++--- .../revolute_velocity_constraint_wide.rs | 6 ++--- src/dynamics/solver/parallel_island_solver.rs | 6 ++--- src/dynamics/solver/position_constraint.rs | 4 +-- src/dynamics/solver/position_constraint_wide.rs | 4 +-- src/dynamics/solver/position_ground_constraint.rs | 2 +- .../solver/position_ground_constraint_wide.rs | 2 +- src/dynamics/solver/position_solver.rs | 22 ++++++++++----- src/dynamics/solver/solver_constraints.rs | 12 +++++++-- src/dynamics/solver/velocity_constraint.rs | 4 +-- src/dynamics/solver/velocity_constraint_wide.rs | 4 +-- ...ocity_constraint_wide_with_manifold_friction.rs | 4 +-- .../velocity_constraint_with_manifold_friction.rs | 4 +-- src/dynamics/solver/velocity_ground_constraint.rs | 2 +- .../solver/velocity_ground_constraint_wide.rs | 2 +- ...round_constraint_wide_with_manifold_friction.rs | 2 +- ...ity_ground_constraint_with_manifold_friction.rs | 2 +- src/dynamics/solver/velocity_solver.rs | 21 ++++++++------- 31 files changed, 132 insertions(+), 99 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 21cc642..fef7565 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{BodyPair, IslandSet, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { @@ -83,24 +83,23 @@ impl ParallelInteractionGroups { match (rb1.is_static(), rb2.is_static()) { (false, false) => { - let color_mask = - bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset]; + let color_mask = bcolors[rb1.island_offset] | bcolors[rb2.island_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.active_set_offset] |= 1 << *color; - bcolors[rb2.active_set_offset] |= 1 << *color; + bcolors[rb1.island_offset] |= 1 << *color; + bcolors[rb2.island_offset] |= 1 << *color; } (true, false) => { - let color_mask = bcolors[rb2.active_set_offset]; + let color_mask = bcolors[rb2.island_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb2.active_set_offset] |= 1 << *color; + bcolors[rb2.island_offset] |= 1 << *color; } (false, true) => { - let color_mask = bcolors[rb1.active_set_offset]; + let color_mask = bcolors[rb1.island_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.active_set_offset] |= 1 << *color; + bcolors[rb1.island_offset] |= 1 << *color; } (true, true) => unreachable!(), } @@ -176,6 +175,7 @@ impl InteractionGroups { pub fn group_joints( &mut self, island_id: usize, + islands: &IslandSet, bodies: &RigidBodySet, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], @@ -197,7 +197,7 @@ impl InteractionGroups { // 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. @@ -215,8 +215,8 @@ impl InteractionGroups { } let ijoint = interaction.params.type_id(); - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = body1.island_offset; + let i2 = body2.island_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. @@ -320,6 +320,7 @@ impl InteractionGroups { pub fn group_manifolds( &mut self, island_id: usize, + islands: &IslandSet, bodies: &RigidBodySet, interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], @@ -331,7 +332,7 @@ impl InteractionGroups { // 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. @@ -365,8 +366,8 @@ impl InteractionGroups { continue; } - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = body1.island_offset; + let i2 = body2.island_offset; 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 8b7e587..81fc5d0 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -4,7 +4,7 @@ use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, IslandSet, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub struct IslandSolver { @@ -29,6 +29,7 @@ impl IslandSolver { island_id: usize, counters: &mut Counters, params: &IntegrationParameters, + islands: &IslandSet, bodies: &mut RigidBodySet, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -37,15 +38,22 @@ impl IslandSolver { ) { if manifold_indices.len() != 0 || joint_indices.len() != 0 { counters.solver.velocity_assembly_time.resume(); - self.contact_constraints - .init(island_id, params, bodies, manifolds, manifold_indices); + self.contact_constraints.init( + island_id, + islands, + params, + bodies, + manifolds, + manifold_indices, + ); self.joint_constraints - .init(island_id, params, bodies, joints, joint_indices); + .init(island_id, islands, params, bodies, joints, joint_indices); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); self.velocity_solver.solve( island_id, + islands, params, bodies, manifolds, @@ -57,7 +65,11 @@ impl IslandSolver { } counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt)); + for handle in islands.active_island(island_id).bodies() { + if let Some(rb) = bodies.get_mut_internal(*handle) { + rb.integrate(params.dt) + } + } counters.solver.velocity_update_time.pause(); if params.position_erp != 0.0 { @@ -66,6 +78,7 @@ impl IslandSolver { self.position_solver.solve( island_id, params, + islands, bodies, &self.contact_constraints.position_constraints, &self.joint_constraints.position_constraints, diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 744c00d..6708d44 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -33,8 +33,8 @@ impl BallPositionConstraint { ii2: rb2.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: rb1.island_offset, + position2: rb2.island_offset, } } @@ -118,7 +118,7 @@ impl BallPositionGroundConstraint { im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, - position2: rb2.active_set_offset, + position2: rb2.island_offset, local_com2: rb2.mass_properties.local_com, } } else { @@ -127,7 +127,7 @@ impl BallPositionGroundConstraint { im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, local_com2: rb2.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 043eea7..08fb96c 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -43,8 +43,8 @@ impl WBallPositionConstraint { .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 position1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; Self { local_com1, @@ -151,7 +151,7 @@ impl WBallPositionGroundConstraint { } else { cparams[ii].local_anchor2 }; SIMD_WIDTH]); - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); Self { diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index e75f978..2791747 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -79,8 +79,8 @@ impl BallVelocityConstraint { BallVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, im2, impulse: cparams.impulse * params.warmstart_coeff, @@ -201,7 +201,7 @@ impl BallVelocityGroundConstraint { BallVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, impulse: cparams.impulse * params.warmstart_coeff, r2: anchor2, 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 95b0bb5..a18bccb 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -46,7 +46,7 @@ impl WBallVelocityConstraint { 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 mj_lambda1 = array![|ii| rbs1[ii].island_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]); @@ -56,7 +56,7 @@ impl WBallVelocityConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_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]); @@ -232,7 +232,7 @@ impl WBallVelocityGroundConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor2 = Point::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 7e8fc97..5e0b485 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -31,8 +31,8 @@ impl FixedPositionConstraint { Self { local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, im1, im2, ii1, @@ -110,7 +110,7 @@ impl FixedPositionGroundConstraint { Self { anchor1, local_anchor2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_com2: rb2.mass_properties.local_com, diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index 2868d4b..2f284a7 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -112,8 +112,8 @@ impl FixedVelocityConstraint { FixedVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, im2, ii1, @@ -301,7 +301,7 @@ impl FixedVelocityGroundConstraint { FixedVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, ii2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index c423272..0090a8b 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -65,7 +65,7 @@ impl WFixedVelocityConstraint { 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 mj_lambda1 = array![|ii| rbs1[ii].island_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]); @@ -75,7 +75,7 @@ impl WFixedVelocityConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); @@ -319,7 +319,7 @@ impl WFixedVelocityGroundConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let local_anchor1 = Isometry::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index ea7e927..f03804a 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -46,8 +46,8 @@ impl PrismaticPositionConstraint { local_frame2: cparams.local_frame2(), local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, limits: cparams.limits, } } @@ -135,7 +135,7 @@ impl PrismaticPositionGroundConstraint { local_frame2, axis1, local_axis2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, limits: cparams.limits, } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index d1943fc..6c8e9b6 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -173,8 +173,8 @@ impl PrismaticVelocityConstraint { PrismaticVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, im2, @@ -463,7 +463,7 @@ impl PrismaticVelocityGroundConstraint { PrismaticVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index f24cfa5..f084d60 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -77,7 +77,7 @@ impl WPrismaticVelocityConstraint { 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 mj_lambda1 = array![|ii| rbs1[ii].island_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]); @@ -87,7 +87,7 @@ impl WPrismaticVelocityConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_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]); @@ -435,7 +435,7 @@ impl WPrismaticVelocityGroundConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; #[cfg(feature = "dim2")] let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 19dd451..d9f6cff 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -44,8 +44,8 @@ impl RevolutePositionConstraint { local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: rb1.island_offset, + position2: rb2.island_offset, } } @@ -118,7 +118,7 @@ impl RevolutePositionGroundConstraint { local_anchor2, axis1, local_axis2, - position2: rb2.active_set_offset, + position2: rb2.island_offset, } } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 38f56d9..22fd916 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -84,8 +84,8 @@ impl RevoluteVelocityConstraint { RevoluteVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: rb1.island_offset, + mj_lambda2: rb2.island_offset, im1, ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, basis1, @@ -238,7 +238,7 @@ impl RevoluteVelocityGroundConstraint { RevoluteVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: rb2.island_offset, im2, ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 822c2ac..fbfec3b 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -47,7 +47,7 @@ impl WRevoluteVelocityConstraint { 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 mj_lambda1 = array![|ii| rbs1[ii].island_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]); @@ -57,7 +57,7 @@ impl WRevoluteVelocityConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_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]); @@ -266,7 +266,7 @@ impl WRevoluteVelocityGroundConstraint { 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 mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); let local_anchor1 = Point::from( diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index af8e9c0..5f7073e 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -248,11 +248,11 @@ impl ParallelIslandSolver { let batch_size = thread.batch_size; for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { let rb = &mut bodies[handle.0]; - let dvel = mj_lambdas[rb.active_set_offset]; + let dvel = mj_lambdas[rb.island_offset]; rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.integrate(params.dt); - positions[rb.active_set_offset] = rb.position; + positions[rb.island_offset] = rb.position; } } @@ -273,7 +273,7 @@ impl ParallelIslandSolver { let batch_size = thread.batch_size; for handle in active_bodies[thread.position_writeback_index] { let rb = &mut bodies[handle.0]; - rb.set_position_internal(positions[rb.active_set_offset]); + rb.set_position_internal(positions[rb.island_offset]); } } }) diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index ecf8286..4c68305 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -82,8 +82,8 @@ impl PositionConstraint { } let constraint = PositionConstraint { - rb1: rb1.active_set_offset, - rb2: rb2.active_set_offset, + rb1: rb1.island_offset, + rb2: rb2.island_offset, local_p1, local_p2, local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal), diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index b7918c5..ae0fc62 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -54,8 +54,8 @@ impl WPositionConstraint { array![|ii| manifolds[ii].data.normal; SIMD_WIDTH], )); - let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let rb1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; + let rb2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let num_active_contacts = manifolds[0].data.num_active_contacts(); diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index 1e5a2ff..24a717a 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -58,7 +58,7 @@ impl PositionGroundConstraint { } let constraint = PositionGroundConstraint { - rb2: rb2.active_set_offset, + rb2: rb2.island_offset, p1, local_p2, n1, diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 531eead..9397325 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -55,7 +55,7 @@ impl WPositionGroundConstraint { ); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let rb2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let num_active_contacts = manifolds[0].data.num_active_contacts(); diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index df0e3fc..d979233 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,5 +1,7 @@ use super::AnyJointPositionConstraint; -use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; +use crate::dynamics::{ + solver::AnyPositionConstraint, IntegrationParameters, IslandSet, RigidBodySet, +}; use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { @@ -17,15 +19,19 @@ impl PositionSolver { &mut self, island_id: usize, params: &IntegrationParameters, + islands: &IslandSet, bodies: &mut RigidBodySet, contact_constraints: &[AnyPositionConstraint], joint_constraints: &[AnyJointPositionConstraint], ) { self.positions.clear(); self.positions.extend( - bodies - .iter_active_island(island_id) - .map(|(_, b)| b.position), + islands + .active_island(island_id) + .bodies() + .iter() + .filter_map(|h| bodies.get(*h)) + .map(|b| b.position), ); for _ in 0..params.max_position_iterations { @@ -38,8 +44,10 @@ impl PositionSolver { } } - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.set_position_internal(self.positions[rb.active_set_offset]) - }); + for handle in islands.active_island(island_id).bodies() { + if let Some(rb) = bodies.get_mut(*handle) { + rb.set_position_internal(self.positions[rb.island_offset]) + } + } } } diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 1f4cec5..b4ac54d 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -14,7 +14,8 @@ use crate::dynamics::solver::{ VelocityGroundConstraintWithManifoldFriction, }; use crate::dynamics::{ - solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + solver::AnyVelocityConstraint, IntegrationParameters, IslandSet, JointGraphEdge, JointIndex, + RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] @@ -48,6 +49,7 @@ impl SolverConstraints { pub fn init_constraint_groups( &mut self, island_id: usize, + islands: &IslandSet, bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -65,6 +67,7 @@ impl SolverConstraints { self.interaction_groups.clear_groups(); self.interaction_groups.group_manifolds( island_id, + islands, bodies, manifolds, &self.not_ground_interactions, @@ -73,6 +76,7 @@ impl SolverConstraints { self.ground_interaction_groups.clear_groups(); self.ground_interaction_groups.group_manifolds( island_id, + islands, bodies, manifolds, &self.ground_interactions, @@ -90,6 +94,7 @@ impl SolverConstraints { pub fn init( &mut self, island_id: usize, + islands: &IslandSet, params: &IntegrationParameters, bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], @@ -98,7 +103,7 @@ impl SolverConstraints { self.velocity_constraints.clear(); self.position_constraints.clear(); - self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); + self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices); #[cfg(feature = "simd-is-enabled")] { @@ -245,6 +250,7 @@ impl SolverConstraints { pub fn init( &mut self, island_id: usize, + islands: &IslandSet, params: &IntegrationParameters, bodies: &RigidBodySet, joints: &[JointGraphEdge], @@ -267,6 +273,7 @@ impl SolverConstraints { self.interaction_groups.clear_groups(); self.interaction_groups.group_joints( island_id, + islands, bodies, joints, &self.not_ground_interactions, @@ -275,6 +282,7 @@ impl SolverConstraints { self.ground_interaction_groups.clear_groups(); self.ground_interaction_groups.group_joints( island_id, + islands, bodies, joints, &self.ground_interactions, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index aa3ea62..4454a9a 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -177,8 +177,8 @@ impl VelocityConstraint { let inv_dt = params.inv_dt(); let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; - let mj_lambda1 = rb1.active_set_offset; - let mj_lambda2 = rb2.active_set_offset; + let mj_lambda1 = rb1.island_offset; + let mj_lambda2 = rb2.island_offset; let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 97aa0eb..03e4ed8 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -93,8 +93,8 @@ impl WVelocityConstraint { let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); diff --git a/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs index 4a1ad82..69e44e9 100644 --- a/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs +++ b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs @@ -85,8 +85,8 @@ impl WVelocityConstraintWithManifoldFriction { let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = array![|ii| rbs1[ii].island_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); diff --git a/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs index de88f09..396a987 100644 --- a/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs +++ b/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs @@ -67,8 +67,8 @@ impl VelocityConstraintWithManifoldFriction { let inv_dt = params.inv_dt(); let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; - let mj_lambda1 = rb1.active_set_offset; - let mj_lambda2 = rb2.active_set_offset; + let mj_lambda1 = rb1.island_offset; + let mj_lambda2 = rb2.island_offset; let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index e773d5e..e3e3d77 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -75,7 +75,7 @@ impl VelocityGroundConstraint { -manifold.data.normal }; - let mj_lambda2 = rb2.active_set_offset; + let mj_lambda2 = rb2.island_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index d7221fc..035f897 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -93,7 +93,7 @@ impl WVelocityGroundConstraint { let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs index 39f8d72..a0f2339 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs @@ -86,7 +86,7 @@ impl WVelocityGroundConstraintWithManifoldFriction { let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].island_offset; SIMD_WIDTH]; let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); diff --git a/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs b/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs index 30ad214..5596a84 100644 --- a/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs +++ b/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs @@ -72,7 +72,7 @@ impl VelocityGroundConstraintWithManifoldFriction { -manifold.data.normal }; - let mj_lambda2 = rb2.active_set_offset; + let mj_lambda2 = rb2.island_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 89cf34e..68669e9 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,7 +1,7 @@ use super::AnyJointVelocityConstraint; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, JointGraphEdge, RigidBodySet, + IntegrationParameters, IslandSet, JointGraphEdge, RigidBodySet, }; use crate::geometry::ContactManifold; use crate::math::Real; @@ -21,6 +21,7 @@ impl VelocitySolver { pub fn solve( &mut self, island_id: usize, + islands: &IslandSet, params: &IntegrationParameters, bodies: &mut RigidBodySet, manifolds_all: &mut [&mut ContactManifold], @@ -30,7 +31,7 @@ impl VelocitySolver { ) { self.mj_lambdas.clear(); self.mj_lambdas - .resize(bodies.active_island(island_id).len(), DeltaVel::zero()); + .resize(islands.active_island(island_id).len(), DeltaVel::zero()); /* * Warmstart constraints. @@ -57,13 +58,15 @@ impl VelocitySolver { } // Update velocities. - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - let dvel = self.mj_lambdas[rb.active_set_offset]; - rb.linvel += dvel.linear; - rb.angvel += rb - .effective_world_inv_inertia_sqrt - .transform_vector(dvel.angular); - }); + for handle in islands.active_island(island_id).bodies() { + if let Some(rb) = bodies.get_mut(*handle) { + let dvel = self.mj_lambdas[rb.island_offset]; + rb.linvel += dvel.linear; + rb.angvel += rb + .effective_world_inv_inertia_sqrt + .transform_vector(dvel.angular); + } + } // Write impulses back into the manifold structures. for constraint in &*joint_constraints { -- cgit