diff options
Diffstat (limited to 'src/dynamics/solver')
31 files changed, 132 insertions, 99 deletions
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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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::<SimdReal>::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/ |