From f108520b5a110cf59864abac7ac6a37e2b5a1dd9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 20 Apr 2022 12:29:57 +0200 Subject: [PATCH] Finalize refactoring --- Cargo.toml | 16 +- examples2d/one_way_platforms2.rs | 7 +- examples3d/one_way_platforms3.rs | 7 +- src/dynamics/ccd/ccd_solver.rs | 297 ++++++++---------- src/dynamics/ccd/toi_entry.rs | 104 ++---- src/dynamics/island_manager.rs | 100 +++--- .../joint/impulse_joint/impulse_joint_set.rs | 30 +- .../joint/multibody_joint/multibody.rs | 70 ++--- src/dynamics/rigid_body_components.rs | 25 +- src/dynamics/solver/categorization.rs | 8 +- .../solver/generic_velocity_constraint.rs | 92 +++--- .../generic_velocity_ground_constraint.rs | 30 +- src/dynamics/solver/interaction_groups.rs | 55 ++-- .../joint_constraint/joint_constraint.rs | 129 ++++---- src/dynamics/solver/parallel_island_solver.rs | 13 +- .../solver/parallel_solver_constraints.rs | 3 +- .../solver/parallel_velocity_solver.rs | 51 +-- src/dynamics/solver/velocity_constraint.rs | 17 +- .../solver/velocity_constraint_wide.rs | 16 +- .../solver/velocity_ground_constraint.rs | 16 +- .../solver/velocity_ground_constraint_wide.rs | 14 +- src/dynamics/solver/velocity_solver.rs | 64 ++-- .../broad_phase_multi_sap/broad_phase.rs | 11 +- src/geometry/collider_set.rs | 54 ++-- src/geometry/narrow_phase.rs | 252 ++++++--------- src/pipeline/collision_pipeline.rs | 2 +- src/pipeline/physics_pipeline.rs | 58 ++-- src/pipeline/query_pipeline.rs | 133 +++----- src/pipeline/user_changes.rs | 57 ++-- src_testbed/harness/mod.rs | 2 +- src_testbed/physics/mod.rs | 2 +- src_testbed/testbed.rs | 2 +- 32 files changed, 707 insertions(+), 1030 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index c307641..3e732d5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,18 +8,18 @@ resolver = "2" #simba = { path = "../simba" } #kiss3d = { path = "../kiss3d" } -parry2d = { path = "../parry/crates/parry2d" } -parry3d = { path = "../parry/crates/parry3d" } -parry2d-f64 = { path = "../parry/crates/parry2d-f64" } -parry3d-f64 = { path = "../parry/crates/parry3d-f64" } +#parry2d = { path = "../parry/crates/parry2d" } +#parry3d = { path = "../parry/crates/parry3d" } +#parry2d-f64 = { path = "../parry/crates/parry2d-f64" } +#parry3d-f64 = { path = "../parry/crates/parry3d-f64" } # nalgebra = { path = "../nalgebra" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } -#parry2d = { git = "https://github.com/dimforge/parry", branch = "special_cases" } -#parry3d = { git = "https://github.com/dimforge/parry", branch = "special_cases" } -#parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" } -#parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" } +parry2d = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" } +parry3d = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" } +parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" } +parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" } [profile.release] #debug = true diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 2b72c1f..dd50201 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -6,11 +6,8 @@ struct OneWayPlatformHook { platform2: ColliderHandle, } -impl PhysicsHooks for OneWayPlatformHook { - fn modify_solver_contacts( - &self, - context: &mut ContactModificationContext, - ) { +impl PhysicsHooks for OneWayPlatformHook { + fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { // The allowed normal for the first platform is its local +y axis, and the // allowed normal for the second platform is its local -y axis. // diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index 6f214b5..3b2715a 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -6,11 +6,8 @@ struct OneWayPlatformHook { platform2: ColliderHandle, } -impl PhysicsHooks for OneWayPlatformHook { - fn modify_solver_contacts( - &self, - context: &mut ContactModificationContext, - ) { +impl PhysicsHooks for OneWayPlatformHook { + fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { // The allowed normal for the first platform is its local +y axis, and the // allowed normal for the second platform is its local -y axis. // diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 8fc5a7f..77a1ff7 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,16 +1,10 @@ use super::TOIEntry; -use crate::dynamics::{ - IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle, - RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity, -}; -use crate::geometry::{ - ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent, - NarrowPhase, -}; +use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase}; use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; -use crate::prelude::{ActiveEvents, ColliderFlags}; +use crate::prelude::ActiveEvents; use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::utils::hashmap::HashMap; use std::collections::BinaryHeap; @@ -61,23 +55,18 @@ impl CCDSolver { match impacts { PredictedImpacts::Impacts(tois) => { for (handle, toi) in tois { - let (rb_poss, vels, ccd, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyCcd, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - let local_com = &mprops.local_mprops.local_com; + let rb = bodies.index_mut_internal(*handle); + let local_com = &rb.mprops.local_mprops.local_com; - let min_toi = (ccd.ccd_thickness + let min_toi = (rb.ccd.ccd_thickness * 0.15 - * crate::utils::inv(ccd.max_point_velocity(vels))) + * crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels))) .min(dt); // println!("Min toi: {}, Toi: {}", min_toi, toi); - let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com); - bodies.map_mut_internal(handle.0, |rb_poss| { - rb_poss.next_position = new_pos; - }); + let new_pos = rb + .vels + .integrate(toi.max(min_toi), &rb.pos.position, &local_com); + rb.pos.next_position = new_pos; } } _ => {} @@ -98,17 +87,16 @@ impl CCDSolver { // println!("Checking CCD activation"); for handle in islands.active_dynamic_bodies() { - let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) = - bodies.index_bundle(handle.0); - - if ccd.ccd_enabled { - let forces = if include_forces { Some(forces) } else { None }; - let moving_fast = ccd.is_moving_fast(dt, vels, forces); - - bodies.map_mut_internal(handle.0, |ccd| { - ccd.ccd_active = moving_fast; - }); + let rb = bodies.index_mut_internal(*handle); + if rb.ccd.ccd_enabled { + let forces = if include_forces { + Some(&rb.forces) + } else { + None + }; + let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces); + rb.ccd.ccd_active = moving_fast; ccd_active = ccd_active || moving_fast; } } @@ -137,36 +125,31 @@ impl CCDSolver { let mut min_toi = dt; for handle in islands.active_dynamic_bodies() { - let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + let rb1 = &bodies[*handle]; - if rb_ccd1.ccd_active { - let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyMassProps, - &RigidBodyColliders, - ) = bodies.index_bundle(handle.0); + if rb1.ccd.ccd_active { + let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( + dt, + &rb1.forces, + &rb1.vels, + &rb1.mprops, + ); - let predicted_body_pos1 = - rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1); - - for ch1 in &rb_colliders1.0 { - let co_parent1: &ColliderParent = colliders - .get(ch1.0) + for ch1 in &rb1.colliders.0 { + let co1 = &colliders[*ch1]; + let co1_parent = co1 + .parent + .as_ref() .expect("Could not find the ColliderParent component."); - let (co_shape1, co_pos1, co_type1): ( - &ColliderShape, - &ColliderPosition, - &ColliderType, - ) = colliders.index_bundle(ch1.0); - if co_type1.is_sensor() { + if co1.is_sensor() { continue; // Ignore sensors. } - let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; - let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); + let predicted_collider_pos1 = predicted_body_pos1 * co1_parent.pos_wrt_parent; + let aabb1 = co1 + .shape + .compute_swept_aabb(&co1.pos, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -182,21 +165,17 @@ impl CCDSolver { ) .is_none() { - let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); - let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); - let co_type1: &ColliderType = colliders.index(ch1.0); - let co_type2: &ColliderType = colliders.index(ch1.0); + let co1 = &colliders[*ch1]; + let co2 = &colliders[*ch2]; - let bh1 = co_parent1.map(|p| p.handle); - let bh2 = co_parent2.map(|p| p.handle); + let bh1 = co1.parent.map(|p| p.handle); + let bh2 = co2.parent.map(|p| p.handle); // Ignore self-intersection and sensors and apply collision groups filter. - if bh1 == bh2 // Ignore self-intersection. - || (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors. - || !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups. - || !c1.3.solver_groups.test(c2.3.solver_groups) + if bh1 == bh2 // Ignore self-intersection. + || (co1.is_sensor() || co2.is_sensor()) // Ignore sensors. + || !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups. + || !co1.flags.solver_groups.test(co2.flags.solver_groups) // Apply solver groups. { return true; @@ -208,16 +187,16 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b2 = bh2.map(|h| bodies.index_bundle(h.0)); + let rb2 = bh2.and_then(|h| bodies.get(h)); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, c1.3, co_parent1), - (c2.0, c2.1, c2.2, c2.3, co_parent2), - Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), - b2, + co1, + co2, + Some(rb1), + rb2, None, None, 0.0, @@ -271,29 +250,27 @@ impl CCDSolver { */ // TODO: don't iterate through all the colliders. for handle in islands.active_dynamic_bodies() { - let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); + let rb1 = &bodies[*handle]; - if rb_ccd1.ccd_active { - let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyMassProps, - &RigidBodyColliders, - ) = bodies.index_bundle(handle.0); + if rb1.ccd.ccd_active { + let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( + dt, + &rb1.forces, + &rb1.vels, + &rb1.mprops, + ); - let predicted_body_pos1 = - rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1); - - for ch1 in &rb_colliders1.0 { - let co_parent1: &ColliderParent = colliders - .get(ch1.0) + for ch1 in &rb1.colliders.0 { + let co1 = &colliders[*ch1]; + let co_parent1 = co1 + .parent + .as_ref() .expect("Could not find the ColliderParent component."); - let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = - colliders.index_bundle(ch1.0); let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; - let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); + let aabb1 = co1 + .shape + .compute_swept_aabb(&co1.pos, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -309,16 +286,15 @@ impl CCDSolver { ) .is_none() { - let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); - let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); + let co1 = &colliders[*ch1]; + let co2 = &colliders[*ch2]; - let bh1 = co_parent1.map(|p| p.handle); - let bh2 = co_parent2.map(|p| p.handle); + let bh1 = co1.parent.map(|p| p.handle); + let bh2 = co2.parent.map(|p| p.handle); // Ignore self-intersections and apply groups filter. - if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) + if bh1 == bh2 + || !co1.flags.collision_groups.test(co2.flags.collision_groups) { return true; } @@ -329,17 +305,17 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b1 = bh1.map(|h| bodies.index_bundle(h.0)); - let b2 = bh2.map(|h| bodies.index_bundle(h.0)); + let rb1 = bh1.map(|h| &bodies[h]); + let rb2 = bh2.map(|h| &bodies[h]); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, c1.3, co_parent1), - (c2.0, c2.1, c2.2, c2.3, co_parent2), - b1, - b2, + co1, + co2, + rb1, + rb2, None, None, 0.0, @@ -381,17 +357,15 @@ impl CCDSolver { while let Some(toi) = all_toi.pop() { assert!(toi.toi <= dt); - let rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> = - toi.b1.map(|b| bodies.index_bundle(b.0)); - let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> = - toi.b2.map(|b| bodies.index_bundle(b.0)); + let rb1 = toi.b1.and_then(|b| bodies.get(b)); + let rb2 = toi.b2.and_then(|b| bodies.get(b)); let mut colliders_to_check = Vec::new(); let should_freeze1 = rb1.is_some() - && rb1.unwrap().0.ccd_active + && rb1.unwrap().ccd.ccd_active && !frozen.contains_key(&toi.b1.unwrap()); let should_freeze2 = rb2.is_some() - && rb2.unwrap().0.ccd_active + && rb2.unwrap().ccd.ccd_active && !frozen.contains_key(&toi.b2.unwrap()); if !should_freeze1 && !should_freeze2 { @@ -413,12 +387,12 @@ impl CCDSolver { if should_freeze1 { let _ = frozen.insert(toi.b1.unwrap(), toi.toi); - colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0); + colliders_to_check.extend_from_slice(&rb1.unwrap().colliders.0); } if should_freeze2 { let _ = frozen.insert(toi.b2.unwrap(), toi.toi); - colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0); + colliders_to_check.extend_from_slice(&rb2.unwrap().colliders.0); } let start_time = toi.toi; @@ -426,39 +400,36 @@ impl CCDSolver { // NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the // ones we used above. for ch1 in &colliders_to_check { - let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap(); - let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = - colliders.index_bundle(ch1.0); + let co1 = &colliders[*ch1]; + let co1_parent = co1.parent.as_ref().unwrap(); + let rb1 = &bodies[co1_parent.handle]; - let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0); - let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent; - let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1); + let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent; + let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); - let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); + let co2 = &colliders[*ch2]; - let bh1 = co_parent1.map(|p| p.handle); - let bh2 = co_parent2.map(|p| p.handle); + let bh1 = co1.parent.map(|p| p.handle); + let bh2 = co2.parent.map(|p| p.handle); // Ignore self-intersection and apply groups filter. - if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) { + if bh1 == bh2 + || !co1.flags.collision_groups.test(co2.flags.collision_groups) + { return true; } let frozen1 = bh1.and_then(|h| frozen.get(&h)); let frozen2 = bh2.and_then(|h| frozen.get(&h)); - let b1: Option<(_, _, _, &RigidBodyCcd)> = - bh1.map(|h| bodies.index_bundle(h.0)); - let b2: Option<(_, _, _, &RigidBodyCcd)> = - bh2.map(|h| bodies.index_bundle(h.0)); + let rb1 = bh1.and_then(|h| bodies.get(h)); + let rb2 = bh2.and_then(|h| bodies.get(h)); - if (frozen1.is_some() || !b1.map(|b| b.3.ccd_active).unwrap_or(false)) - && (frozen2.is_some() || !b2.map(|b| b.3.ccd_active).unwrap_or(false)) + if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false)) + && (frozen2.is_some() + || !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false)) { // We already did a resweep. return true; @@ -474,10 +445,10 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, c1.3, co_parent1), - (c2.0, c2.1, c2.2, c2.3, co_parent2), - b1, - b2, + co1, + co2, + rb1, + rb2, frozen1.copied(), frozen2.copied(), start_time, @@ -500,20 +471,10 @@ impl CCDSolver { // - If the intersection isn't active anymore, and it wasn't intersecting // before, then we need to generate one interaction-start and one interaction-stop // events because it will never be detected by the narrow-phase because of tunneling. - let (co_type1, co_pos1, co_shape1, co_flags1): ( - &ColliderType, - &ColliderPosition, - &ColliderShape, - &ColliderFlags, - ) = colliders.index_bundle(toi.c1.0); - let (co_type2, co_pos2, co_shape2, co_flags2): ( - &ColliderType, - &ColliderPosition, - &ColliderShape, - &ColliderFlags, - ) = colliders.index_bundle(toi.c2.0); + let co1 = &colliders[toi.c1]; + let co2 = &colliders[toi.c2]; - if !co_type1.is_sensor() && !co_type2.is_sensor() { + if !co1.is_sensor() && !co2.is_sensor() { // TODO: this happens if we found a TOI between two non-sensor // colliders with mismatching solver_flags. It is not clear // what we should do in this case: we could report a @@ -525,56 +486,46 @@ impl CCDSolver { } let co_next_pos1 = if let Some(b1) = toi.b1 { - let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); - let (rb_pos1, rb_vels1, rb_mprops1): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - ) = bodies.index_bundle(b1.0); - - let local_com1 = &rb_mprops1.local_mprops.local_com; + let co_parent1: &ColliderParent = co1.parent.as_ref().unwrap(); + let rb1 = &bodies[b1]; + let local_com1 = &rb1.mprops.local_mprops.local_com; let frozen1 = frozen.get(&b1); let pos1 = frozen1 - .map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1)) - .unwrap_or(rb_pos1.next_position); + .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1)) + .unwrap_or(rb1.pos.next_position); pos1 * co_parent1.pos_wrt_parent } else { - co_pos1.0 + co1.pos.0 }; let co_next_pos2 = if let Some(b2) = toi.b2 { - let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap(); - let (rb_pos2, rb_vels2, rb_mprops2): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - ) = bodies.index_bundle(b2.0); - - let local_com2 = &rb_mprops2.local_mprops.local_com; + let co_parent2: &ColliderParent = co2.parent.as_ref().unwrap(); + let rb2 = &bodies[b2]; + let local_com2 = &rb2.mprops.local_mprops.local_com; let frozen2 = frozen.get(&b2); let pos2 = frozen2 - .map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2)) - .unwrap_or(rb_pos2.next_position); + .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2)) + .unwrap_or(rb2.pos.next_position); pos2 * co_parent2.pos_wrt_parent } else { - co_pos2.0 + co2.pos.0 }; - let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2); + let prev_coll_pos12 = co1.pos.inv_mul(&co2.pos); let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2); let query_dispatcher = self.query_pipeline.query_dispatcher(); let intersect_before = query_dispatcher - .intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) + .intersection_test(&prev_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref()) .unwrap_or(false); let intersect_after = query_dispatcher - .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) + .intersection_test(&next_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref()) .unwrap_or(false); if !intersect_before && !intersect_after - && (co_flags1.active_events | co_flags2.active_events) + && (co1.flags.active_events | co2.flags.active_events) .contains(ActiveEvents::COLLISION_EVENTS) { // Emit one intersection-started and one intersection-stopped event. diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index f937979..4591825 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,9 +1,5 @@ -use crate::dynamics::{ - RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, -}; -use crate::geometry::{ - ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, -}; +use crate::dynamics::{RigidBody, RigidBodyHandle}; +use crate::geometry::{Collider, ColliderHandle}; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -45,32 +41,10 @@ impl TOIEntry { query_dispatcher: &QD, ch1: ColliderHandle, ch2: ColliderHandle, - c1: ( - &ColliderType, - &ColliderShape, - &ColliderPosition, - &ColliderFlags, - Option<&ColliderParent>, - ), - c2: ( - &ColliderType, - &ColliderShape, - &ColliderPosition, - &ColliderFlags, - Option<&ColliderParent>, - ), - b1: Option<( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyCcd, - )>, - b2: Option<( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyCcd, - )>, + co1: &Collider, + co2: &Collider, + rb1: Option<&RigidBody>, + rb2: Option<&RigidBody>, frozen1: Option, frozen2: Option, start_time: Real, @@ -78,39 +52,36 @@ impl TOIEntry { smallest_contact_dist: Real, ) -> Option { assert!(start_time <= end_time); - if b1.is_none() && b2.is_none() { + if rb1.is_none() && rb2.is_none() { return None; } - let (co_type1, co_shape1, co_pos1, co_flags1, co_parent1) = c1; - let (co_type2, co_shape2, co_pos2, co_flags2, co_parent2) = c2; - let linvel1 = - frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero()); + frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero()); let linvel2 = - frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero()); + frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero()); let angvel1 = - frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero()); + frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero()); let angvel2 = - frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero()); + frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero()); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0) - + angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); + + angvel1.abs() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0) + + angvel2.abs() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0); #[cfg(feature = "dim3")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0) - + angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); + + angvel1.norm() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0) + + angvel2.norm() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0); // We may be slightly over-conservative by taking the `max(0.0)` here. // But removing the `max` doesn't really affect performances so let's // keep it since more conservatism is good at this stage. - let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness()) + let thickness = (co1.shape.0.ccd_thickness() + co2.shape.0.ccd_thickness()) + smallest_contact_dist.max(0.0); - let is_pseudo_intersection_test = co_type1.is_sensor() - || co_type2.is_sensor() - || !co_flags1.solver_groups.test(co_flags2.solver_groups); + let is_pseudo_intersection_test = co1.is_sensor() + || co2.is_sensor() + || !co1.flags.solver_groups.test(co2.flags.solver_groups); if (end_time - start_time) * vel12 < thickness { return None; @@ -118,8 +89,8 @@ impl TOIEntry { // Compute the TOI. let identity = NonlinearRigidMotion::identity(); - let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity); - let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity); + let mut motion1 = rb1.map(Self::body_motion).unwrap_or(identity); + let mut motion2 = rb2.map(Self::body_motion).unwrap_or(identity); if let Some(t) = frozen1 { motion1.freeze(t); @@ -129,8 +100,8 @@ impl TOIEntry { motion2.freeze(t); } - let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0)); - let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0)); + let motion_c1 = motion1.prepend(co1.parent.map(|p| p.pos_wrt_parent).unwrap_or(co1.pos.0)); + let motion_c2 = motion2.prepend(co2.parent.map(|p| p.pos_wrt_parent).unwrap_or(co2.pos.0)); // println!("start_time: {}", start_time); @@ -146,9 +117,9 @@ impl TOIEntry { let res_toi = query_dispatcher .nonlinear_time_of_impact( &motion_c1, - co_shape1.as_ref(), + co1.shape.as_ref(), &motion_c2, - co_shape2.as_ref(), + co2.shape.as_ref(), start_time, end_time, stop_at_penetration, @@ -160,31 +131,24 @@ impl TOIEntry { Some(Self::new( toi.toi, ch1, - co_parent1.map(|p| p.handle), + co1.parent.map(|p| p.handle), ch2, - co_parent2.map(|p| p.handle), + co2.parent.map(|p| p.handle), is_pseudo_intersection_test, 0, )) } - fn body_motion( - (poss, vels, mprops, ccd): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyCcd, - ), - ) -> NonlinearRigidMotion { - if ccd.ccd_active { + fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion { + if rb.ccd.ccd_active { NonlinearRigidMotion::new( - poss.position, - mprops.local_mprops.local_com, - vels.linvel, - vels.angvel, + rb.pos.position, + rb.mprops.local_mprops.local_com, + rb.vels.linvel, + rb.vels.angvel, ) } else { - NonlinearRigidMotion::constant_position(poss.next_position) + NonlinearRigidMotion::constant_position(rb.pos.next_position) } } } diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 0cb92e9..246b50e 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -47,12 +47,15 @@ impl IslandManager { while i < active_set.len() { let handle = active_set[i]; - if bodies.get(handle.0).is_none() { + if bodies.get(handle).is_none() { // This rigid-body no longer exists, so we need to remove it from the active set. active_set.swap_remove(i); if i < active_set.len() { - bodies.map_mut_internal(active_set[i].0, |rb_ids| rb_ids.active_set_id = i); + // Update the active_set_id for the body that has been swapped. + if let Some(swapped_rb) = bodies.get_mut_internal(active_set[i]) { + swapped_rb.ids.active_set_id = i; + } } } else { i += 1; @@ -92,15 +95,13 @@ impl IslandManager { // deleting a joint attached to an already-removed body) where we could be // attempting to wake-up a rigid-body that has already been deleted. if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { - bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { - activation.wake_up(strong) - }); - bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { - if self.active_dynamic_set.get(ids.active_set_id) != Some(&handle) { - ids.active_set_id = self.active_dynamic_set.len(); - self.active_dynamic_set.push(handle); - } - }); + let rb = bodies.index_mut_internal(handle); + rb.activation.wake_up(strong); + + if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) { + rb.ids.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } } } @@ -162,25 +163,22 @@ impl IslandManager { let can_sleep = &mut self.can_sleep; let stack = &mut self.stack; - let vels: &RigidBodyVelocity = bodies.index(h.0); - let sq_linvel = vels.linvel.norm_squared(); - let sq_angvel = vels.angvel.gdot(vels.angvel); + let rb = bodies.index_mut_internal(h); + let sq_linvel = rb.vels.linvel.norm_squared(); + let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { - update_energy(activation, sq_linvel, sq_angvel, dt); + update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt); - if activation.time_since_can_sleep - >= RigidBodyActivation::default_time_until_sleep() - { - // Mark them as sleeping for now. This will - // be set to false during the graph traversal - // if it should not be put to sleep. - activation.sleeping = true; - can_sleep.push(h); - } else { - stack.push(h); - } - }); + if rb.activation.time_since_can_sleep >= RigidBodyActivation::default_time_until_sleep() + { + // Mark them as sleeping for now. This will + // be set to false during the graph traversal + // if it should not be put to sleep. + rb.activation.sleeping = true; + can_sleep.push(h); + } else { + stack.push(h); + } } // Read all the contacts and push objects touching touching this rigid-body. @@ -199,7 +197,7 @@ impl IslandManager { (inter.collider1, inter.collider2), *collider_handle, ); - if let Some(other_body) = colliders.get(other.0) { + if let Some(other_body) = colliders[other].parent { stack.push(other_body.handle); } break; @@ -212,15 +210,15 @@ impl IslandManager { // Now iterate on all active kinematic bodies and push all the bodies // touching them to the stack so they can be woken up. for h in self.active_kinematic_set.iter() { - let (vels, rb_colliders): (&RigidBodyVelocity, _) = bodies.index_bundle(h.0); + let rb = &bodies[*h]; - if vels.is_zero() { + if rb.vels.is_zero() { // If the kinematic body does not move, it does not have // to wake up any dynamic body. continue; } - push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); + push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); } // println!("Selection: {}", instant::now() - t); @@ -235,13 +233,9 @@ impl IslandManager { let mut island_marker = self.stack.len().max(1) - 1; while let Some(handle) = self.stack.pop() { - let (rb_status, rb_ids, rb_colliders): ( - &RigidBodyType, - &RigidBodyIds, - &RigidBodyColliders, - ) = bodies.index_bundle(handle.0); + let rb = bodies.index_mut_internal(handle); - if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() { + if rb.ids.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() { // We already visited this body and its neighbors. // Also, we don't propagate awake state through fixed bodies. continue; @@ -260,7 +254,7 @@ impl IslandManager { // Transmit the active state to all the rigid-bodies with colliders // in contact or joined with this collider. - push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); + push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); for inter in impulse_joints.joints_with(handle) { let other = crate::utils::select_other((inter.0, inter.1), handle); @@ -271,16 +265,12 @@ impl IslandManager { self.stack.push(other); } - bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { - activation.wake_up(false); - }); - bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { - ids.active_island_id = self.active_islands.len() - 1; - ids.active_set_id = self.active_dynamic_set.len(); - ids.active_set_offset = - ids.active_set_id - self.active_islands[ids.active_island_id]; - ids.active_set_timestamp = self.active_set_timestamp; - }); + rb.activation.wake_up(false); + rb.ids.active_island_id = self.active_islands.len() - 1; + rb.ids.active_set_id = self.active_dynamic_set.len(); + rb.ids.active_set_offset = + rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]; + rb.ids.active_set_timestamp = self.active_set_timestamp; self.active_dynamic_set.push(handle); } @@ -293,13 +283,11 @@ impl IslandManager { // ); // Actually put to sleep bodies which have not been detected as awake. - for h in &self.can_sleep { - let activation: &RigidBodyActivation = bodies.index(h.0); - if activation.sleeping { - bodies.set_internal(h.0, RigidBodyVelocity::zero()); - bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { - activation.sleep() - }); + for handle in &self.can_sleep { + let rb = bodies.index_mut_internal(*handle); + if rb.activation.sleeping { + rb.vels = RigidBodyVelocity::zero(); + rb.activation.sleep(); } } } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 6b6d980..1cd177d 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -3,10 +3,7 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio use crate::data::arena::Arena; use crate::data::Coarena; -use crate::dynamics::{ - GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet, - RigidBodyType, -}; +use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -230,26 +227,17 @@ impl ImpulseJointSet { // FIXME: don't iterate through all the interactions. for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { let joint = &edge.weight; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; - let (status1, activation1, ids1): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body1.0); - let (status2, activation2, ids2): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body2.0); - - if (status1.is_dynamic() || status2.is_dynamic()) - && (!status1.is_dynamic() || !activation1.sleeping) - && (!status2.is_dynamic() || !activation2.sleeping) + if (rb1.is_dynamic() || rb2.is_dynamic()) + && (!rb1.is_dynamic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic() || !rb2.is_sleeping()) { - let island_index = if !status1.is_dynamic() { - ids2.active_island_id + let island_index = if !rb1.is_dynamic() { + rb2.ids.active_island_id } else { - ids1.active_island_id + rb1.ids.active_island_id }; out[island_index].push(i); diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 5bd790a..2eb90eb 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,8 +1,8 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; use crate::dynamics::{ - solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle, - RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity, + solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet, + RigidBodyType, RigidBodyVelocity, }; #[cfg(feature = "dim3")] use crate::math::Matrix; @@ -376,36 +376,32 @@ impl Multibody { for i in 0..self.links.len() { let link = &self.links[i]; - - let (rb_vels, rb_mprops, rb_forces): ( - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyForces, - ) = bodies.index_bundle(link.rigid_body.0); + let rb = &bodies[link.rigid_body]; let mut acc = RigidBodyVelocity::zero(); if i != 0 { let parent_id = link.parent_internal_id; let parent_link = &self.links[parent_id]; - let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0); + let parent_rb = &bodies[parent_link.rigid_body]; acc += self.workspace.accs[parent_id]; // The 2.0 originates from the two identical terms of Jdot (the terms become // identical once they are multiplied by the generalized velocities). - acc.linvel += 2.0 * parent_rb_vels.angvel.gcross(link.joint_velocity.linvel); + acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel); #[cfg(feature = "dim3")] { - acc.angvel += parent_rb_vels.angvel.cross(&link.joint_velocity.angvel); + acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel); } - acc.linvel += parent_rb_vels + acc.linvel += parent_rb + .vels .angvel - .gcross(parent_rb_vels.angvel.gcross(link.shift02)); + .gcross(parent_rb.vels.angvel.gcross(link.shift02)); acc.linvel += self.workspace.accs[parent_id].angvel.gcross(link.shift02); } - acc.linvel += rb_vels.angvel.gcross(rb_vels.angvel.gcross(link.shift23)); + acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23)); acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23); self.workspace.accs[i] = acc; @@ -413,12 +409,12 @@ impl Multibody { // TODO: should gyroscopic forces already be computed by the rigid-body itself // (at the same time that we add the gravity force)? let gyroscopic; - let rb_inertia = rb_mprops.effective_angular_inertia(); - let rb_mass = rb_mprops.effective_mass(); + let rb_inertia = rb.mprops.effective_angular_inertia(); + let rb_mass = rb.mprops.effective_mass(); #[cfg(feature = "dim3")] { - gyroscopic = rb_vels.angvel.cross(&(rb_inertia * rb_vels.angvel)); + gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel)); } #[cfg(feature = "dim2")] { @@ -426,8 +422,8 @@ impl Multibody { } let external_forces = Force::new( - rb_forces.force - rb_mass.component_mul(&acc.linvel), - rb_forces.torque - gyroscopic - rb_inertia * acc.angvel, + rb.forces.force - rb_mass.component_mul(&acc.linvel), + rb.forces.torque - gyroscopic - rb_inertia * acc.angvel, ); self.accelerations.gemv_tr( 1.0, @@ -456,13 +452,12 @@ impl Multibody { .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]); link.joint_velocity = joint_velocity; - bodies.set_internal(link.rigid_body.0, link.joint_velocity); + bodies.index_mut_internal(link.rigid_body).vels = link.joint_velocity; for i in 1..self.links.len() { let (link, parent_link) = self.links.get_mut_with_parent(i); - let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0); - let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(parent_link.rigid_body.0); + let rb = &bodies[link.rigid_body]; + let parent_rb = &bodies[parent_link.rigid_body]; let joint_velocity = link .joint @@ -470,12 +465,12 @@ impl Multibody { link.joint_velocity = joint_velocity.transformed( &(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation), ); - let mut new_rb_vels = *parent_rb_vels + link.joint_velocity; - let shift = rb_mprops.world_com - parent_rb_mprops.world_com; - new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift); + let mut new_rb_vels = parent_rb.vels + link.joint_velocity; + let shift = rb.mprops.world_com - parent_rb.mprops.world_com; + new_rb_vels.linvel += parent_rb.vels.angvel.gcross(shift); new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23); - bodies.set_internal(link.rigid_body.0, new_rb_vels); + bodies.index_mut_internal(link.rigid_body).vels = new_rb_vels; } /* @@ -563,10 +558,9 @@ impl Multibody { for i in 0..self.links.len() { let link = &self.links[i]; - let (rb_vels, rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(link.rigid_body.0); - let rb_mass = rb_mprops.effective_mass(); - let rb_inertia = rb_mprops.effective_angular_inertia().into_matrix(); + let rb = &bodies[link.rigid_body]; + let rb_mass = rb.mprops.effective_mass(); + let rb_inertia = rb.mprops.effective_angular_inertia().into_matrix(); let body_jacobian = &self.body_jacobians[i]; @@ -576,8 +570,8 @@ impl Multibody { #[cfg(feature = "dim3")] { // Derivative of gyroscopic forces. - let gyroscopic_matrix = rb_vels.angvel.gcross_matrix() * rb_inertia - - (rb_inertia * rb_vels.angvel).gcross_matrix(); + let gyroscopic_matrix = rb.vels.angvel.gcross_matrix() * rb_inertia + - (rb_inertia * rb.vels.angvel).gcross_matrix(); augmented_inertia += gyroscopic_matrix * dt; } @@ -604,10 +598,10 @@ impl Multibody { if i != 0 { let parent_id = link.parent_internal_id; let parent_link = &self.links[parent_id]; - let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0); + let parent_rb = &bodies[parent_link.rigid_body]; let parent_j = &self.body_jacobians[parent_id]; let parent_j_w = parent_j.fixed_rows::(DIM); - let parent_w = parent_rb_vels.angvel.gcross_matrix(); + let parent_w = parent_rb.vels.angvel.gcross_matrix(); let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id); let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id); @@ -620,7 +614,7 @@ impl Multibody { coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0); // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot) - let dvel_cross = (rb_vels.angvel.gcross(link.shift02) + let dvel_cross = (rb.vels.angvel.gcross(link.shift02) + 2.0 * link.joint_velocity.linvel) .gcross_matrix_tr(); coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0); @@ -676,13 +670,13 @@ impl Multibody { coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0); // JDot - let dvel_cross = rb_vels.angvel.gcross(link.shift23).gcross_matrix_tr(); + let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr(); coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0); // JDot/u * qdot coriolis_v.gemm( 1.0, - &(rb_vels.angvel.gcross_matrix() * shift_cross_tr), + &(rb.vels.angvel.gcross_matrix() * shift_cross_tr), &rb_j_w, 1.0, ); diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index c7655e0..b818ce2 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -231,6 +231,7 @@ pub struct RigidBodyMassProps { pub flags: LockedAxes, /// The local mass properties of the rigid-body. pub local_mprops: MassProperties, + /// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders. pub additional_local_mprops: Option>, /// The world-space center of mass of the rigid-body. pub world_com: Point, @@ -307,11 +308,11 @@ impl RigidBodyMassProps { .unwrap_or_else(MassProperties::default); for handle in &attached_colliders.0 { - if let Some(co) = colliders.get(handle) { + if let Some(co) = colliders.get(*handle) { if let Some(co_parent) = co.parent { let to_add = co .mprops - .mass_properties(&**co.shape) + .mass_properties(&*co.shape) .transform_by(&co_parent.pos_wrt_parent); self.local_mprops += to_add; } @@ -895,21 +896,17 @@ impl RigidBodyColliders { ) { for handle in &self.0 { // NOTE: the ColliderParent component must exist if we enter this method. - let co_parent: &ColliderParent = colliders - .get(handle.0) - .expect("Could not find the ColliderParent component."); - let new_pos = parent_pos * co_parent.pos_wrt_parent; + let co = colliders.index_mut_internal(*handle); + let new_pos = parent_pos * co.parent.as_ref().unwrap().pos_wrt_parent; + + if !co.changes.contains(ColliderChanges::MODIFIED) { + modified_colliders.push(*handle); + } // Set the modification flag so we can benefit from the modification-tracking // when updating the narrow-phase/broad-phase afterwards. - colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| { - if !co_changes.contains(ColliderChanges::MODIFIED) { - modified_colliders.push(*handle); - } - - *co_changes |= ColliderChanges::POSITION; - }); - colliders.set_internal(handle.0, ColliderPosition(new_pos)); + co.changes |= ColliderChanges::POSITION; + co.pos = ColliderPosition(new_pos); } } } diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 06ba340..d110971 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -50,18 +50,18 @@ pub(crate) fn categorize_joints( ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; - let status1 = bodies.index(joint.body1.0); - let status2 = bodies.index(joint.body2.0); + let rb1 = &bodies[joint.body1.0]; + let rb2 = &bodies[joint.body2.0]; if multibody_joints.rigid_body_link(joint.body1).is_some() || multibody_joints.rigid_body_link(joint.body2).is_some() { - if !status1.is_dynamic() || !status2.is_dynamic() { + if !rb1.is_dynamic() || !rb2.is_dynamic() { generic_ground_joints.push(*joint_i); } else { generic_nonground_joints.push(*joint_i); } - } else if !status1.is_dynamic() || !status2.is_dynamic() { + } else if !rb1.is_dynamic() || !rb2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 1be34bc..318727e 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -1,8 +1,5 @@ use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, - RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -42,18 +39,12 @@ impl GenericVelocityConstraint { let handle1 = manifold.data.rigid_body1.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap(); - let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): ( - &RigidBodyIds, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyType, - ) = bodies.index_bundle(handle1.0); - let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): ( - &RigidBodyIds, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyType, - ) = bodies.index_bundle(handle2.0); + + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; + + let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type); + let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type); let multibody1 = multibodies .rigid_body_link(handle1) @@ -63,15 +54,15 @@ impl GenericVelocityConstraint { .map(|m| (&multibodies[m.multibody], m.id)); let mj_lambda1 = multibody1 .map(|mb| mb.0.solver_id) - .unwrap_or(if rb_type1.is_dynamic() { - rb_ids1.active_set_offset + .unwrap_or(if type1.is_dynamic() { + rb1.ids.active_set_offset } else { 0 }); let mj_lambda2 = multibody2 .map(|mb| mb.0.solver_id) - .unwrap_or(if rb_type2.is_dynamic() { - rb_ids2.active_set_offset + .unwrap_or(if type2.is_dynamic() { + rb2.ids.active_set_offset } else { 0 }); @@ -80,11 +71,8 @@ impl GenericVelocityConstraint { #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions( - &force_dir1, - &rb_vels1.linvel, - &rb_vels2.linvel, - ); + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0) + multibody2.map(|m| m.0.ndofs()).unwrap_or(0); @@ -109,13 +97,13 @@ impl GenericVelocityConstraint { #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: if rb_type1.is_dynamic() { - rb_mprops1.effective_inv_mass + im1: if type1.is_dynamic() { + mprops1.effective_inv_mass } else { na::zero() }, - im2: if rb_type2.is_dynamic() { - rb_mprops2.effective_inv_mass + im2: if type2.is_dynamic() { + mprops2.effective_inv_mass } else { na::zero() }, @@ -129,11 +117,11 @@ impl GenericVelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - rb_mprops1.world_com; - let dp2 = manifold_point.point - rb_mprops2.world_com; + let dp1 = manifold_point.point - mprops1.world_com; + let dp2 = manifold_point.point - mprops2.world_com; - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -143,15 +131,15 @@ impl GenericVelocityConstraint { let torque_dir1 = dp1.gcross(force_dir1); let torque_dir2 = dp2.gcross(-force_dir1); - let gcross1 = if rb_type1.is_dynamic() { - rb_mprops1 + let gcross1 = if type1.is_dynamic() { + mprops1 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir1) } else { na::zero() }; - let gcross2 = if rb_type2.is_dynamic() { - rb_mprops2 + let gcross2 = if type2.is_dynamic() { + mprops2 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir2) } else { @@ -170,8 +158,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type1.is_dynamic() { - force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + gcross1.gdot(gcross1) } else { 0.0 @@ -189,8 +177,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type2.is_dynamic() { - force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2) } else { 0.0 @@ -224,8 +212,8 @@ impl GenericVelocityConstraint { for j in 0..DIM - 1 { let torque_dir1 = dp1.gcross(tangents1[j]); - let gcross1 = if rb_type1.is_dynamic() { - rb_mprops1 + let gcross1 = if type1.is_dynamic() { + mprops1 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir1) } else { @@ -234,8 +222,8 @@ impl GenericVelocityConstraint { constraint.elements[k].tangent_part.gcross1[j] = gcross1; let torque_dir2 = dp2.gcross(-tangents1[j]); - let gcross2 = if rb_type2.is_dynamic() { - rb_mprops2 + let gcross2 = if type2.is_dynamic() { + mprops2 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir2) } else { @@ -255,9 +243,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type1.is_dynamic() { - force_dir1 - .dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + gcross1.gdot(gcross1) } else { 0.0 @@ -275,9 +262,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type2.is_dynamic() { - force_dir1 - .dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2) } else { 0.0 @@ -303,8 +289,8 @@ impl GenericVelocityConstraint { // reduce all ops to nothing because its ndofs will be zero. let generic_constraint_mask = (multibody1.is_some() as u8) | ((multibody2.is_some() as u8) << 1) - | (!rb_type1.is_dynamic() as u8) - | ((!rb_type2.is_dynamic() as u8) << 1); + | (!type1.is_dynamic() as u8) + | ((!type2.is_dynamic() as u8) << 1); let constraint = GenericVelocityConstraint { velocity_constraint: constraint, diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index d953e6f..16648c4 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -1,7 +1,5 @@ use crate::dynamics::solver::VelocityGroundConstraint; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::WCross; @@ -48,16 +46,15 @@ impl GenericVelocityGroundConstraint { (-manifold.data.normal, 1.0) }; - let (rb_vels1, world_com1) = if let Some(handle1) = handle1 { - let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle1.0); - (*vels1, mprops1.world_com) + let (vels1, world_com1) = if let Some(handle1) = handle1 { + let rb1 = &bodies[handle1]; + (rb1.vels, rb1.mprops.world_com) } else { (RigidBodyVelocity::zero(), Point::origin()) }; - let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle2.unwrap().0); + let rb2 = &bodies[handle2.unwrap()]; + let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); let (mb2, link_id2) = handle2 .and_then(|h| multibodies.rigid_body_link(h)) @@ -68,11 +65,8 @@ impl GenericVelocityGroundConstraint { #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions( - &force_dir1, - &rb_vels1.linvel, - &rb_vels2.linvel, - ); + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); let multibodies_ndof = mb2.ndofs(); // For each solver contact we generate DIM constraints, and each constraints appends @@ -96,7 +90,7 @@ impl GenericVelocityGroundConstraint { #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: rb_mprops2.effective_inv_mass, + im2: mprops2.effective_inv_mass, limit: 0.0, mj_lambda2, manifold_id, @@ -107,10 +101,10 @@ impl GenericVelocityGroundConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; let dp1 = manifold_point.point - world_com1; - let dp2 = manifold_point.point - rb_mprops2.world_com; + let dp2 = manifold_point.point - mprops2.world_com; - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 951b77f..aecd12d 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -3,7 +3,6 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { - crate::data::BundleSet, crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, vec_map::VecMap, }; @@ -90,14 +89,8 @@ impl ParallelInteractionGroups { .zip(self.interaction_colors.iter_mut()) { let mut body_pair = interactions[*interaction_id].body_pair(); - let is_fixed1 = body_pair - .0 - .map(|b| ComponentSet::::index(bodies, b.0).is_fixed()) - .unwrap_or(true); - let is_fixed2 = body_pair - .1 - .map(|b| ComponentSet::::index(bodies, b.0).is_fixed()) - .unwrap_or(true); + let is_fixed1 = body_pair.0.map(|b| bodies[b].is_fixed()).unwrap_or(true); + let is_fixed2 = body_pair.1.map(|b| bodies[b].is_fixed()).unwrap_or(true); let representative = |handle: RigidBodyHandle| { if let Some(link) = multibodies.rigid_body_link(handle).copied() { @@ -119,28 +112,28 @@ impl ParallelInteractionGroups { match (is_fixed1, is_fixed2) { (false, false) => { - let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0); - let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); + let rb1 = &bodies[body_pair.0.unwrap()]; + let rb2 = &bodies[body_pair.1.unwrap()]; let color_mask = - bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset]; + bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb_ids1.active_set_offset] |= 1 << *color; - bcolors[rb_ids2.active_set_offset] |= 1 << *color; + bcolors[rb1.ids.active_set_offset] |= 1 << *color; + bcolors[rb2.ids.active_set_offset] |= 1 << *color; } (true, false) => { - let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); - let color_mask = bcolors[rb_ids2.active_set_offset]; + let rb2 = &bodies[body_pair.1.unwrap()]; + let color_mask = bcolors[rb2.ids.active_set_offset]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb_ids2.active_set_offset] |= 1 << *color; + bcolors[rb2.ids.active_set_offset] |= 1 << *color; } (false, true) => { - let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0); - let color_mask = bcolors[rb_ids1.active_set_offset]; + let rb1 = &bodies[body_pair.0.unwrap()]; + let color_mask = bcolors[rb1.ids.active_set_offset]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb_ids1.active_set_offset] |= 1 << *color; + bcolors[rb1.ids.active_set_offset] |= 1 << *color; } (true, true) => unreachable!(), } @@ -258,13 +251,11 @@ impl InteractionGroups { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i].weight; - let (status1, ids1): (&RigidBodyType, &RigidBodyIds) = - bodies.index_bundle(interaction.body1.0); - let (status2, ids2): (&RigidBodyType, &RigidBodyIds) = - bodies.index_bundle(interaction.body2.0); + let rb1 = &bodies[interaction.body1]; + let rb2 = &bodies[interaction.body2]; - let is_fixed1 = !status1.is_dynamic(); - let is_fixed2 = !status2.is_dynamic(); + let is_fixed1 = !rb1.is_dynamic(); + let is_fixed2 = !rb2.is_dynamic(); if is_fixed1 && is_fixed2 { continue; @@ -277,8 +268,8 @@ impl InteractionGroups { } let ijoint = interaction.data.locked_axes.bits() as usize; - let i1 = ids1.active_set_offset; - let i2 = ids2.active_set_offset; + let i1 = rb1.ids.active_set_offset; + let i2 = rb2.ids.active_set_offset; let conflicts = self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. @@ -421,15 +412,15 @@ impl InteractionGroups { let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 { - let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0); - (*data.0, data.1.active_set_offset) + let rb1 = &bodies[rb1]; + (rb1.body_type, rb1.ids.active_set_offset) } else { (RigidBodyType::Fixed, usize::MAX) }; let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 { - let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0); - (*data.0, data.1.active_set_offset) + let rb2 = &bodies[rb2]; + (rb2.body_type, rb2.ids.active_set_offset) } else { (RigidBodyType::Fixed, usize::MAX) }; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index a46744d..962602f 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -6,8 +6,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ }; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds, - RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity, + ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, }; use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::MultibodyJointSet; @@ -63,39 +62,26 @@ impl AnyJointVelocityConstraint { ) { let local_frame1 = joint.data.local_frame1; let local_frame2 = joint.data.local_frame2; - let rb1: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body1.0); - let rb2: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body2.0); - - let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1; - let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2; - let frame1 = rb_pos1.position * local_frame1; - let frame2 = rb_pos2.position * local_frame2; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + let frame1 = rb1.pos.position * local_frame1; + let frame2 = rb2.pos.position * local_frame2; let body1 = SolverBody { - linvel: rb_vel1.linvel, - angvel: rb_vel1.angvel, - im: rb_mprops1.effective_inv_mass, - sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt, - world_com: rb_mprops1.world_com, - mj_lambda: [rb_ids1.active_set_offset], + linvel: rb1.vels.linvel, + angvel: rb1.vels.angvel, + im: rb1.mprops.effective_inv_mass, + sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, + world_com: rb1.mprops.world_com, + mj_lambda: [rb1.ids.active_set_offset], }; let body2 = SolverBody { - linvel: rb_vel2.linvel, - angvel: rb_vel2.angvel, - im: rb_mprops2.effective_inv_mass, - sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt, - world_com: rb_mprops2.world_com, - mj_lambda: [rb_ids2.active_set_offset], + linvel: rb2.vels.linvel, + angvel: rb2.vels.angvel, + im: rb2.mprops.effective_inv_mass, + sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, + world_com: rb2.mprops.world_com, + mj_lambda: [rb2.ids.active_set_offset], }; let mb1 = multibodies @@ -186,16 +172,20 @@ impl AnyJointVelocityConstraint { out: &mut Vec, insert_at: Option, ) { + use crate::dynamics::{ + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, + }; + let rbs1: ( [&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyVelocity; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], - gather![|ii| bodies.index(impulse_joints[ii].body1.0)], + gather![|ii| &bodies[impulse_joints[ii].body1].pos], + gather![|ii| &bodies[impulse_joints[ii].body1].vels], + gather![|ii| &bodies[impulse_joints[ii].body1].mprops], + gather![|ii| &bodies[impulse_joints[ii].body1].ids], ); let rbs2: ( [&RigidBodyPosition; SIMD_WIDTH], @@ -203,10 +193,10 @@ impl AnyJointVelocityConstraint { [&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], - gather![|ii| bodies.index(impulse_joints[ii].body2.0)], + gather![|ii| &bodies[impulse_joints[ii].body2].pos], + gather![|ii| &bodies[impulse_joints[ii].body2].vels], + gather![|ii| &bodies[impulse_joints[ii].body2].mprops], + gather![|ii| &bodies[impulse_joints[ii].body2].ids], ); let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1; @@ -276,8 +266,7 @@ impl AnyJointVelocityConstraint { ) { let mut handle1 = joint.body1; let mut handle2 = joint.body2; - let status2: &RigidBodyType = bodies.index(handle2.0); - let flipped = !status2.is_dynamic(); + let flipped = !bodies[handle2].is_dynamic(); let (local_frame1, local_frame2) = if flipped { std::mem::swap(&mut handle1, &mut handle2); @@ -286,35 +275,27 @@ impl AnyJointVelocityConstraint { (joint.data.local_frame1, joint.data.local_frame2) }; - let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle1.0); - let rb2: ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyIds, - ) = bodies.index_bundle(handle2.0); + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; - let (rb_pos1, rb_vel1, rb_mprops1) = rb1; - let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2; - let frame1 = rb_pos1.position * local_frame1; - let frame2 = rb_pos2.position * local_frame2; + let frame1 = rb1.pos.position * local_frame1; + let frame2 = rb2.pos.position * local_frame2; let body1 = SolverBody { - linvel: rb_vel1.linvel, - angvel: rb_vel1.angvel, - im: rb_mprops1.effective_inv_mass, - sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt, - world_com: rb_mprops1.world_com, + linvel: rb1.vels.linvel, + angvel: rb1.vels.angvel, + im: rb1.mprops.effective_inv_mass, + sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, + world_com: rb1.mprops.world_com, mj_lambda: [crate::INVALID_USIZE], }; let body2 = SolverBody { - linvel: rb_vel2.linvel, - angvel: rb_vel2.angvel, - im: rb_mprops2.effective_inv_mass, - sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt, - world_com: rb_mprops2.world_com, - mj_lambda: [rb_ids2.active_set_offset], + linvel: rb2.vels.linvel, + angvel: rb2.vels.angvel, + im: rb2.mprops.effective_inv_mass, + sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, + world_com: rb2.mprops.world_com, + mj_lambda: [rb2.ids.active_set_offset], }; if let Some(mb2) = multibodies @@ -399,9 +380,13 @@ impl AnyJointVelocityConstraint { out: &mut Vec, insert_at: Option, ) { + use crate::dynamics::{ + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + }; + let mut handles1 = gather![|ii| impulse_joints[ii].body1]; let mut handles2 = gather![|ii| impulse_joints[ii].body2]; - let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; + let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -429,9 +414,9 @@ impl AnyJointVelocityConstraint { [&RigidBodyVelocity; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(handles1[ii].0)], - gather![|ii| bodies.index(handles1[ii].0)], - gather![|ii| bodies.index(handles1[ii].0)], + gather![|ii| &bodies[handles1[ii]].pos], + gather![|ii| &bodies[handles1[ii]].vels], + gather![|ii| &bodies[handles1[ii]].mprops], ); let rbs2: ( [&RigidBodyPosition; SIMD_WIDTH], @@ -439,10 +424,10 @@ impl AnyJointVelocityConstraint { [&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH], ) = ( - gather![|ii| bodies.index(handles2[ii].0)], - gather![|ii| bodies.index(handles2[ii].0)], - gather![|ii| bodies.index(handles2[ii].0)], - gather![|ii| bodies.index(handles2[ii].0)], + gather![|ii| &bodies[handles2[ii]].pos], + gather![|ii| &bodies[handles2[ii]].vels], + gather![|ii| &bodies[handles2[ii]].mprops], + gather![|ii| &bodies[handles2[ii]].ids], ); let (rb_pos1, rb_vel1, rb_mprops1) = rbs1; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 261c627..905de46 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -7,8 +7,7 @@ use crate::dynamics::solver::{ }; use crate::dynamics::{ IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, - RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, - RigidBodyType, RigidBodyVelocity, + RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use na::DVector; @@ -316,15 +315,13 @@ impl ParallelIslandSolver { mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); } } else { - let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) = - bodies.index_bundle(handle.0); - - let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset]; + let rb = &bodies[*handle]; + let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: - dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; - dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt; + dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt; + dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt; } } } diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index d883494..46f1a57 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -8,8 +8,7 @@ use crate::dynamics::solver::{ }; use crate::dynamics::{ ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex, - MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, - RigidBodyType, RigidBodyVelocity, + MultibodyJointSet, RigidBodyHandle, RigidBodySet, }; use crate::geometry::ContactManifold; use crate::math::{Real, SPATIAL_DIM}; diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 5b062e0..ab34a42 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -2,8 +2,7 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadC use crate::concurrent_loop; use crate::dynamics::{ solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, - MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, - RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + MultibodyJointSet, RigidBodySet, }; use crate::geometry::ContactManifold; use crate::math::Real; @@ -210,33 +209,22 @@ impl ParallelVelocitySolver { multibody.velocities = prev_vels; } } else { - let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) = - bodies.index_bundle(handle.0); - - let dvel = self.mj_lambdas[rb_ids.active_set_offset]; - let dangvel = rb_mprops + let rb = bodies.index_mut_internal(*handle); + let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dangvel = rb.mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); // Update positions. - let (rb_pos, rb_vels, rb_damping, rb_mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let mut new_pos = *rb_pos; - let mut new_vels = *rb_vels; + let mut new_vels = rb.vels; new_vels.linvel += dvel.linear; new_vels.angvel += dangvel; - new_vels = new_vels.apply_damping(params.dt, rb_damping); - new_pos.next_position = new_vels.integrate( + new_vels = new_vels.apply_damping(params.dt, &rb.damping); + rb.pos.next_position = new_vels.integrate( params.dt, - &rb_pos.position, - &rb_mprops.local_mprops.local_com, + &rb.pos.position, + &rb.mprops.local_mprops.local_com, ); - bodies.set_internal(handle.0, new_pos); } } } @@ -321,23 +309,14 @@ impl ParallelVelocitySolver { multibody.velocities += mj_lambdas; } } else { - let (rb_ids, rb_damping, rb_mprops): ( - &RigidBodyIds, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let dvel = self.mj_lambdas[rb_ids.active_set_offset]; - let dangvel = rb_mprops + let rb = bodies.index_mut_internal(*handle); + let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dangvel = rb.mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); - let damping = *rb_damping; // To avoid borrow issues. - - bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { - vels.linvel += dvel.linear; - vels.angvel += dangvel; - *vels = vels.apply_damping(params.dt, &damping); - }); + rb.vels.linvel += dvel.linear; + rb.vels.angvel += dangvel; + rb.vels = rb.vels.apply_damping(params.dt, &rb.damping); } } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index c854136..23d4b97 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -3,9 +3,7 @@ use crate::dynamics::solver::{ }; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::{ - IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot}; @@ -160,13 +158,14 @@ impl VelocityConstraint { let handle1 = manifold.data.rigid_body1.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap(); - let (ids1, vels1, mprops1): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle1.0); - let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle2.0); - let mj_lambda1 = ids1.active_set_offset; - let mj_lambda2 = ids2.active_set_offset; + let rb1 = &bodies[handle1]; + let (vels1, mprops1) = (&rb1.vels, &rb1.mprops); + let rb2 = &bodies[handle2]; + let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); + + let mj_lambda1 = rb1.ids.active_set_offset; + let mj_lambda2 = rb2.ids.active_set_offset; let force_dir1 = -manifold.data.normal; #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 81c27fd..68246da 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -1,7 +1,9 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, }; -use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, @@ -49,12 +51,12 @@ impl WVelocityConstraint { let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; - let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; - let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; - let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; - let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; - let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; - let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; + let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels]; + let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels]; + let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids]; + let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids]; + let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops]; + let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops]; let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]); diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 0acc604..39a2a19 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -7,9 +7,7 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::WBasis; use crate::utils::{self, WAngularInertia, WCross, WDot}; -use crate::dynamics::{ - IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[derive(Copy, Clone, Debug)] @@ -51,15 +49,15 @@ impl VelocityGroundConstraint { }; let (vels1, world_com1) = if let Some(handle1) = handle1 { - let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle1.0); - (*vels1, mprops1.world_com) + let rb1 = &bodies[handle1]; + (rb1.vels, rb1.mprops.world_com) } else { (RigidBodyVelocity::zero(), Point::origin()) }; - let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(handle2.unwrap().0); + let rb2 = &bodies[handle2.unwrap()]; + let vels2 = &rb2.vels; + let mprops2 = &rb2.mprops; #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); @@ -67,7 +65,7 @@ impl VelocityGroundConstraint { let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - let mj_lambda2 = ids2.active_set_offset; + let mj_lambda2 = rb2.ids.active_set_offset; for (_l, manifold_points) in manifold .data diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 4b3d429..37f4688 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,9 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, }; -use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity, +}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, @@ -54,20 +56,20 @@ impl WVelocityGroundConstraint { let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| { handles1[ii] - .map(|h| *bodies.index(h.0)) + .map(|h| bodies[h].vels) .unwrap_or_else(RigidBodyVelocity::zero) }]; let world_com1 = Point::from(gather![|ii| { handles1[ii] - .map(|h| ComponentSet::::index(bodies, h.0).world_com) + .map(|h| bodies[h].mprops.world_com) .unwrap_or_else(Point::origin) }]); let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = - gather![|ii| bodies.index(handles2[ii].unwrap().0)]; - let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)]; + gather![|ii| &bodies[handles2[ii].unwrap()].vels]; + let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids]; let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = - gather![|ii| bodies.index(handles2[ii].unwrap().0)]; + gather![|ii| &bodies[handles2[ii].unwrap()].mprops]; let flipped_sign = SimdReal::from(flipped); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index a0d494e..d5dc77d 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,9 +1,7 @@ use super::AnyJointVelocityConstraint; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping, - RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet, - RigidBodyVelocity, + IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet, }; use crate::geometry::ContactManifold; use crate::math::Real; @@ -59,15 +57,15 @@ impl VelocitySolver { mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); } } else { - let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) = - bodies.index_bundle(handle.0); - - let dvel = &mut self.mj_lambdas[ids.active_set_offset]; + let rb = &bodies[*handle]; + let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: - dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; - dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt; + dvel.angular += + rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt; + dvel.linear += + rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt; } } @@ -151,33 +149,26 @@ impl VelocitySolver { multibody.velocities = prev_vels; } } else { - let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) = - bodies.index_bundle(handle.0); + let rb = bodies.index_mut_internal(*handle); - let dvel = self.mj_lambdas[rb_ids.active_set_offset]; - let dangvel = rb_mprops + let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dangvel = rb + .mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); // Update positions. - let (rb_pos, rb_vels, rb_damping, rb_mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let mut new_pos = *rb_pos; - let mut new_vels = *rb_vels; + let mut new_pos = rb.pos; + let mut new_vels = rb.vels; new_vels.linvel += dvel.linear; new_vels.angvel += dangvel; - new_vels = new_vels.apply_damping(params.dt, rb_damping); + new_vels = new_vels.apply_damping(params.dt, &rb.damping); new_pos.next_position = new_vels.integrate( params.dt, - &rb_pos.position, - &rb_mprops.local_mprops.local_com, + &rb.pos.position, + &rb.mprops.local_mprops.local_com, ); - bodies.set_internal(handle.0, new_pos); + rb.pos = new_pos; } } @@ -234,23 +225,16 @@ impl VelocitySolver { multibody.velocities += mj_lambdas; } } else { - let (rb_ids, rb_damping, rb_mprops): ( - &RigidBodyIds, - &RigidBodyDamping, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - - let dvel = self.mj_lambdas[rb_ids.active_set_offset]; - let dangvel = rb_mprops + let rb = bodies.index_mut_internal(*handle); + let dvel = self.mj_lambdas[rb.ids.active_set_offset]; + let dangvel = rb + .mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); - let damping = *rb_damping; // To avoid borrow issues. - bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { - vels.linvel += dvel.linear; - vels.angvel += dangvel; - *vels = vels.apply_damping(params.dt, &damping); - }); + rb.vels.linvel += dvel.linear; + rb.vels.angvel += dangvel; + rb.vels = rb.vels.apply_damping(params.dt, &rb.damping); } } diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index 7afc671..2c8c5e3 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -451,7 +451,7 @@ impl BroadPhase { for handle in modified_colliders { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. - if let Some(co) = colliders.get(*handle) { + if let Some(co) = colliders.get_mut_internal(*handle) { if !co.changes.needs_broad_phase_update() { continue; } @@ -471,12 +471,9 @@ impl BroadPhase { // Make sure we have the new proxy index in case // the collider was added for the first time. - colliders.set_internal( - handle.0, - ColliderBroadPhaseData { - proxy_index: new_proxy_id, - }, - ); + co.bf_data = ColliderBroadPhaseData { + proxy_index: new_proxy_id, + }; } } } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index de182e7..b9f4ae3 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -73,7 +73,7 @@ impl ColliderSet { // Make sure the internal links are reset, they may not be // if this rigid-body was obtained by cloning another one. coll.reset_internal_references(); - coll.co_parent = None; + coll.parent = None; let handle = ColliderHandle(self.colliders.insert(coll)); self.modified_colliders.push(handle); handle @@ -91,12 +91,12 @@ impl ColliderSet { // if this collider was obtained by cloning another one. coll.reset_internal_references(); - if let Some(prev_parent) = &mut coll.co_parent { + if let Some(prev_parent) = &mut coll.parent { prev_parent.handle = parent_handle; } else { - coll.co_parent = Some(ColliderParent { + coll.parent = Some(ColliderParent { handle: parent_handle, - pos_wrt_parent: coll.co_pos.0, + pos_wrt_parent: coll.pos.0, }); } @@ -111,10 +111,10 @@ impl ColliderSet { let coll = self.colliders.get_mut(handle.0).unwrap(); parent.add_collider( handle, - coll.co_parent.as_mut().unwrap(), - &mut coll.co_pos, - &coll.co_shape, - &coll.co_mprops, + coll.parent.as_mut().unwrap(), + &mut coll.pos, + &coll.shape, + &coll.mprops, ); handle } @@ -128,12 +128,12 @@ impl ColliderSet { bodies: &mut RigidBodySet, ) { if let Some(collider) = self.get_mut(handle) { - let curr_parent = collider.co_parent.map(|p| p.handle); + let curr_parent = collider.parent.map(|p| p.handle); if new_parent_handle == curr_parent { return; // Nothing to do, this is the same parent. } - collider.co_changes |= ColliderChanges::PARENT; + collider.changes |= ColliderChanges::PARENT; if let Some(parent_handle) = curr_parent { if let Some(rb) = bodies.get_mut(parent_handle) { @@ -143,10 +143,10 @@ impl ColliderSet { match new_parent_handle { Some(new_parent_handle) => { - if let Some(co_parent) = &mut collider.co_parent { - co_parent.handle = new_parent_handle; + if let Some(parent) = &mut collider.parent { + parent.handle = new_parent_handle; } else { - collider.co_parent = Some(ColliderParent { + collider.parent = Some(ColliderParent { handle: new_parent_handle, pos_wrt_parent: Isometry::identity(), }) @@ -155,14 +155,14 @@ impl ColliderSet { if let Some(rb) = bodies.get_mut(new_parent_handle) { rb.add_collider( handle, - collider.co_parent.as_ref().unwrap(), - &mut collider.co_pos, - &collider.co_shape, - &collider.co_mprops, + collider.parent.as_ref().unwrap(), + &mut collider.pos, + &collider.shape, + &collider.mprops, ); } } - None => collider.co_parent = None, + None => collider.parent = None, } } } @@ -185,14 +185,14 @@ impl ColliderSet { */ // NOTE: we use `get_mut_internal_with_modification_tracking` instead of `get_mut_internal` so that the // modification flag is updated properly. - if let Some(co_parent) = &collider.co_parent { - if let Some(parent) = - bodies.get_mut_internal_with_modification_tracking(co_parent.handle) + if let Some(parent) = &collider.parent { + if let Some(parent_rb) = + bodies.get_mut_internal_with_modification_tracking(parent.handle) { - parent.remove_collider_internal(handle, &collider); + parent_rb.remove_collider_internal(handle, &collider); if wake_up { - islands.wake_up(bodies, co_parent.handle, true); + islands.wake_up(bodies, parent.handle, true); } } } @@ -247,8 +247,8 @@ impl ColliderSet { collider: &mut Collider, modified_colliders: &mut Vec, ) { - if !collider.co_changes.contains(ColliderChanges::MODIFIED) { - collider.co_changes = ColliderChanges::MODIFIED; + if !collider.changes.contains(ColliderChanges::MODIFIED) { + collider.changes = ColliderChanges::MODIFIED; modified_colliders.push(handle); } } @@ -261,6 +261,10 @@ impl ColliderSet { Some(result) } + pub(crate) fn index_mut_internal(&mut self, handle: ColliderHandle) -> &mut Collider { + &mut self.colliders[handle.0] + } + pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { self.colliders.get_mut(handle.0) } diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 26bddf1..f4e8c58 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -3,13 +3,11 @@ use rayon::prelude::*; use crate::data::Coarena; use crate::dynamics::{ - CoefficientCombineRule, IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, - RigidBodySet, RigidBodyType, + CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType, }; use crate::geometry::{ - BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial, - ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, - CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair, + BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair, + ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags, }; use crate::math::{Real, Vector}; @@ -17,7 +15,6 @@ use crate::pipeline::{ ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext, PhysicsHooks, }; -use crate::prelude::ColliderFlags; use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; use parry::utils::IsometryOpt; use std::collections::HashMap; @@ -311,12 +308,12 @@ impl NarrowPhase { // Wake up every body in contact with the deleted collider and generate Stopped collision events. if let Some(islands) = islands.as_deref_mut() { for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) { - if let Some(parent) = colliders.get(a.0).map(|c| c.handle) { - islands.wake_up(bodies, parent, true) + if let Some(parent) = colliders.get(a).and_then(|c| c.parent.as_ref()) { + islands.wake_up(bodies, parent.handle, true) } - if let Some(parent) = colliders.get(b.0).map(|c| c.handle) { - islands.wake_up(bodies, parent, true) + if let Some(parent) = colliders.get(b).and_then(|c| c.parent.as_ref()) { + islands.wake_up(bodies, parent.handle, true) } if pair.start_event_emited { @@ -379,10 +376,8 @@ impl NarrowPhase { for handle in modified_colliders { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. - let co_changes: Option<&ColliderChanges> = colliders.get(handle.0); - - if let Some(co_changes) = co_changes { - if co_changes.needs_narrow_phase_update() { + if let Some(co) = colliders.get(*handle) { + if co.changes.needs_narrow_phase_update() { // No flag relevant to the narrow-phase is enabled for this collider. continue; } @@ -392,13 +387,8 @@ impl NarrowPhase { // so that the narrow-phase properly takes into account the change in, e.g., // collision groups. Waking up the modified collider's parent isn't enough because // it could be a fixed or kinematic body which don't propagate the wake-up state. - - let co_parent: Option<&ColliderParent> = colliders.get(handle.0); - let (co_changes, co_type): (&ColliderChanges, &ColliderType) = - colliders.index_bundle(handle.0); - if let Some(islands) = islands.as_deref_mut() { - if let Some(co_parent) = co_parent { + if let Some(co_parent) = &co.parent { islands.wake_up(bodies, co_parent.handle, true); } @@ -407,8 +397,9 @@ impl NarrowPhase { .interactions_with(gid.contact_graph_index) { let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 }; - let other_parent: Option<&ColliderParent> = - colliders.get(other_handle.0); + let other_parent = colliders + .get(other_handle) + .and_then(|co| co.parent.as_ref()); if let Some(other_parent) = other_parent { islands.wake_up(bodies, other_parent.handle, true); @@ -420,8 +411,8 @@ impl NarrowPhase { // to transfer their contact/intersection graph edges to the intersection/contact graph. // To achieve this we will remove the relevant contact/intersection pairs form the // contact/intersection graphs, and then add them into the other graph. - if co_changes.contains(ColliderChanges::TYPE) { - if co_type.is_sensor() { + if co.changes.contains(ColliderChanges::TYPE) { + if co.is_sensor() { // Find the contact pairs for this collider and // push them to `pairs_to_remove`. for inter in self @@ -441,9 +432,7 @@ impl NarrowPhase { .intersection_graph .interactions_with(gid.intersection_graph_index) .filter(|(h1, h2, _)| { - let co_type1: &ColliderType = colliders.index(h1.0); - let co_type2: &ColliderType = colliders.index(h2.0); - !co_type1.is_sensor() && !co_type2.is_sensor() + !colliders[*h1].is_sensor() && !colliders[*h2].is_sensor() }) { pairs_to_remove.push(( @@ -484,10 +473,9 @@ impl NarrowPhase { events: &dyn EventHandler, mode: PairRemovalMode, ) { - let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0); - let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0); - - if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) { + if let (Some(co1), Some(co2)) = + (colliders.get(pair.collider1), colliders.get(pair.collider2)) + { // TODO: could we just unwrap here? // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`? if let (Some(gid1), Some(gid2)) = ( @@ -495,8 +483,7 @@ impl NarrowPhase { self.graph_indices.get(pair.collider2.0), ) { if mode == PairRemovalMode::FromIntersectionGraph - || (mode == PairRemovalMode::Auto - && (co_type1.is_sensor() || co_type2.is_sensor())) + || (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor())) { let intersection = self .intersection_graph @@ -505,10 +492,7 @@ impl NarrowPhase { // Emit an intersection lost event if we had an intersection before removing the edge. if let Some(mut intersection) = intersection { if intersection.intersecting { - let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0); - let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0); - - if (co_flag1.active_events | co_flag2.active_events) + if (co1.flags.active_events | co2.flags.active_events) .contains(ActiveEvents::COLLISION_EVENTS) { intersection.emit_stop_event(pair.collider1, pair.collider2, events) @@ -524,25 +508,17 @@ impl NarrowPhase { // Also wake up the dynamic bodies that were in contact. if let Some(mut ctct) = contact_pair { if ctct.has_any_active_contact { - let co_parent1: Option<&ColliderParent> = - colliders.get(pair.collider1.0); - let co_parent2: Option<&ColliderParent> = - colliders.get(pair.collider2.0); - if let Some(islands) = islands { - if let Some(co_parent1) = co_parent1 { + if let Some(co_parent1) = &co1.parent { islands.wake_up(bodies, co_parent1.handle, true); } - if let Some(co_parent2) = co_parent2 { + if let Some(co_parent2) = co2.parent { islands.wake_up(bodies, co_parent2.handle, true); } } - let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0); - let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0); - - if (co_flag1.active_events | co_flag2.active_events) + if (co1.flags.active_events | co2.flags.active_events) .contains(ActiveEvents::COLLISION_EVENTS) { ctct.emit_stop_event(events); @@ -555,15 +531,11 @@ impl NarrowPhase { } fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) { - let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0); - let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0); - - if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) { - let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0); - - if co_parent1.map(|p| p.handle) == co_parent2.map(|p| p.handle) { - if co_parent1.is_some() { + if let (Some(co1), Some(co2)) = + (colliders.get(pair.collider1), colliders.get(pair.collider2)) + { + if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) { + if co1.parent.is_some() { // Same parents. Ignore collisions. return; } @@ -577,7 +549,7 @@ impl NarrowPhase { ColliderGraphIndices::invalid(), ); - if co_type1.is_sensor() || co_type2.is_sensor() { + if co1.is_sensor() || co2.is_sensor() { // NOTE: the collider won't have a graph index as long // as it does not interact with anything. if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index) @@ -682,27 +654,13 @@ impl NarrowPhase { let handle1 = nodes[edge.source().index()].weight; let handle2 = nodes[edge.target().index()].weight; let had_intersection = edge.weight.intersecting; + let co1 = &colliders[handle1]; + let co2 = &colliders[handle2]; // TODO: remove the `loop` once labels on blocks is stabilized. 'emit_events: loop { - let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0); - let (co_changes1, co_shape1, co_pos1, co_flags1): ( - &ColliderChanges, - &ColliderShape, - &ColliderPosition, - &ColliderFlags, - ) = colliders.index_bundle(handle1.0); - - let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0); - let (co_changes2, co_shape2, co_pos2, co_flags2): ( - &ColliderChanges, - &ColliderShape, - &ColliderPosition, - &ColliderFlags, - ) = colliders.index_bundle(handle2.0); - - if !co_changes1.needs_narrow_phase_update() - && !co_changes2.needs_narrow_phase_update() + if !co1.changes.needs_narrow_phase_update() + && !co2.changes.needs_narrow_phase_update() { // No update needed for these colliders. return; @@ -712,36 +670,36 @@ impl NarrowPhase { let mut rb_type1 = RigidBodyType::Fixed; let mut rb_type2 = RigidBodyType::Fixed; - if let Some(co_parent1) = co_parent1 { - rb_type1 = *bodies.index(co_parent1.handle.0); + if let Some(co_parent1) = &co1.parent { + rb_type1 = bodies[co_parent1.handle].body_type; } - if let Some(co_parent2) = co_parent2 { - rb_type2 = *bodies.index(co_parent2.handle.0); + if let Some(co_parent2) = &co2.parent { + rb_type2 = bodies[co_parent2.handle].body_type; } // Filter based on the rigid-body types. - if !co_flags1.active_collision_types.test(rb_type1, rb_type2) - && !co_flags2.active_collision_types.test(rb_type1, rb_type2) + if !co1.flags.active_collision_types.test(rb_type1, rb_type2) + && !co2.flags.active_collision_types.test(rb_type1, rb_type2) { edge.weight.intersecting = false; break 'emit_events; } // Filter based on collision groups. - if !co_flags1.collision_groups.test(co_flags2.collision_groups) { + if !co1.flags.collision_groups.test(co2.flags.collision_groups) { edge.weight.intersecting = false; break 'emit_events; } - let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks; + let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks; if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) { let context = PairFilterContext { bodies, colliders, - rigid_body1: co_parent1.map(|p| p.handle), - rigid_body2: co_parent2.map(|p| p.handle), + rigid_body1: co1.parent.map(|p| p.handle), + rigid_body2: co2.parent.map(|p| p.handle), collider1: handle1, collider2: handle2, }; @@ -753,16 +711,14 @@ impl NarrowPhase { } } - let pos12 = co_pos1.inv_mul(co_pos2); + let pos12 = co1.pos.inv_mul(&co2.pos); edge.weight.intersecting = query_dispatcher - .intersection_test(&pos12, &**co_shape1, &**co_shape2) + .intersection_test(&pos12, &*co1.shape, &*co2.shape) .unwrap_or(false); break 'emit_events; } - let co_flags1: &ColliderFlags = colliders.index(handle1.0); - let co_flags2: &ColliderFlags = colliders.index(handle2.0); - let active_events = co_flags1.active_events | co_flags2.active_events; + let active_events = co1.flags.active_events | co2.flags.active_events; if active_events.contains(ActiveEvents::COLLISION_EVENTS) && had_intersection != edge.weight.intersecting @@ -795,29 +751,13 @@ impl NarrowPhase { par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { let pair = &mut edge.weight; let had_any_active_contact = pair.has_any_active_contact; + let co1 = &colliders[pair.collider1]; + let co2 = &colliders[pair.collider2]; // TODO: remove the `loop` once labels on blocks are supported. 'emit_events: loop { - let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0); - let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): ( - &ColliderChanges, - &ColliderShape, - &ColliderPosition, - &ColliderMaterial, - &ColliderFlags, - ) = colliders.index_bundle(pair.collider1.0); - - let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0); - let (co_changes2, co_shape2, co_pos2, co_material2, co_flags2): ( - &ColliderChanges, - &ColliderShape, - &ColliderPosition, - &ColliderMaterial, - &ColliderFlags, - ) = colliders.index_bundle(pair.collider2.0); - - if !co_changes1.needs_narrow_phase_update() - && !co_changes2.needs_narrow_phase_update() + if !co1.changes.needs_narrow_phase_update() + && !co2.changes.needs_narrow_phase_update() { // No update needed for these colliders. return; @@ -827,36 +767,36 @@ impl NarrowPhase { let mut rb_type1 = RigidBodyType::Fixed; let mut rb_type2 = RigidBodyType::Fixed; - if let Some(co_parent1) = co_parent1 { - rb_type1 = *bodies.index(co_parent1.handle.0); + if let Some(co_parent1) = &co1.parent { + rb_type1 = bodies[co_parent1.handle].body_type; } - if let Some(co_parent2) = co_parent2 { - rb_type2 = *bodies.index(co_parent2.handle.0); + if let Some(co_parent2) = &co2.parent { + rb_type2 = bodies[co_parent2.handle].body_type; } // Filter based on the rigid-body types. - if !co_flags1.active_collision_types.test(rb_type1, rb_type2) - && !co_flags2.active_collision_types.test(rb_type1, rb_type2) + if !co1.flags.active_collision_types.test(rb_type1, rb_type2) + && !co2.flags.active_collision_types.test(rb_type1, rb_type2) { pair.clear(); break 'emit_events; } // Filter based on collision groups. - if !co_flags1.collision_groups.test(co_flags2.collision_groups) { + if !co1.flags.collision_groups.test(co2.flags.collision_groups) { pair.clear(); break 'emit_events; } - let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks; + let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks; let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) { let context = PairFilterContext { bodies, colliders, - rigid_body1: co_parent1.map(|p| p.handle), - rigid_body2: co_parent2.map(|p| p.handle), + rigid_body1: co1.parent.map(|p| p.handle), + rigid_body2: co2.parent.map(|p| p.handle), collider1: pair.collider1, collider2: pair.collider2, }; @@ -872,53 +812,55 @@ impl NarrowPhase { SolverFlags::default() }; - if !co_flags1.solver_groups.test(co_flags2.solver_groups) { + if !co1.flags.solver_groups.test(co2.flags.solver_groups) { solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); } - if co_changes1.contains(ColliderChanges::SHAPE) - || co_changes2.contains(ColliderChanges::SHAPE) + if co1.changes.contains(ColliderChanges::SHAPE) + || co2.changes.contains(ColliderChanges::SHAPE) { // The shape changed so the workspace is no longer valid. pair.workspace = None; } - let pos12 = co_pos1.inv_mul(co_pos2); + let pos12 = co1.pos.inv_mul(&co2.pos); let _ = query_dispatcher.contact_manifolds( &pos12, - &**co_shape1, - &**co_shape2, + &*co1.shape, + &*co2.shape, prediction_distance, &mut pair.manifolds, &mut pair.workspace, ); let friction = CoefficientCombineRule::combine( - co_material1.friction, - co_material2.friction, - co_material1.friction_combine_rule as u8, - co_material2.friction_combine_rule as u8, + co1.material.friction, + co2.material.friction, + co1.material.friction_combine_rule as u8, + co2.material.friction_combine_rule as u8, ); let restitution = CoefficientCombineRule::combine( - co_material1.restitution, - co_material2.restitution, - co_material1.restitution_combine_rule as u8, - co_material2.restitution_combine_rule as u8, + co1.material.restitution, + co2.material.restitution, + co1.material.restitution_combine_rule as u8, + co2.material.restitution_combine_rule as u8, ); let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups. - let dominance1 = co_parent1 - .map(|p1| *bodies.index(p1.handle.0)) + let dominance1 = co1 + .parent + .map(|p1| bodies[p1.handle].dominance) .unwrap_or(zero); - let dominance2 = co_parent2 - .map(|p2| *bodies.index(p2.handle.0)) + let dominance2 = co2 + .parent + .map(|p2| bodies[p2.handle].dominance) .unwrap_or(zero); for manifold in &mut pair.manifolds { - let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1); + let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos); manifold.data.solver_contacts.clear(); - manifold.data.rigid_body1 = co_parent1.map(|p| p.handle); - manifold.data.rigid_body2 = co_parent2.map(|p| p.handle); + manifold.data.rigid_body1 = co1.parent.map(|p| p.handle); + manifold.data.rigid_body2 = co2.parent.map(|p| p.handle); manifold.data.solver_flags = solver_flags; manifold.data.relative_dominance = dominance1.effective_group(&rb_type1) - dominance2.effective_group(&rb_type2); @@ -960,8 +902,8 @@ impl NarrowPhase { let mut context = ContactModificationContext { bodies, colliders, - rigid_body1: co_parent1.map(|p| p.handle), - rigid_body2: co_parent2.map(|p| p.handle), + rigid_body1: co1.parent.map(|p| p.handle), + rigid_body2: co2.parent.map(|p| p.handle), collider1: pair.collider1, collider2: pair.collider2, manifold, @@ -981,9 +923,7 @@ impl NarrowPhase { break 'emit_events; } - let co_flags1: &ColliderFlags = colliders.index(pair.collider1.0); - let co_flags2: &ColliderFlags = colliders.index(pair.collider2.0); - let active_events = co_flags1.active_events | co_flags2.active_events; + let active_events = co1.flags.active_events | co2.flags.active_events; if pair.has_any_active_contact != had_any_active_contact { if active_events.contains(ActiveEvents::COLLISION_EVENTS) { @@ -1021,18 +961,24 @@ impl NarrowPhase { { let (active_island_id1, rb_type1, sleeping1) = if let Some(handle1) = manifold.data.rigid_body1 { - let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) = - bodies.index_bundle(handle1.0); - (data.0.active_island_id, *data.1, data.2.sleeping) + let rb1 = &bodies[handle1]; + ( + rb1.ids.active_island_id, + rb1.body_type, + rb1.activation.sleeping, + ) } else { (0, RigidBodyType::Fixed, true) }; let (active_island_id2, rb_type2, sleeping2) = if let Some(handle2) = manifold.data.rigid_body2 { - let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) = - bodies.index_bundle(handle2.0); - (data.0.active_island_id, *data.1, data.2.sleeping) + let rb2 = &bodies[handle2]; + ( + rb2.ids.active_island_id, + rb2.body_type, + rb2.activation.sleeping, + ) } else { (0, RigidBodyType::Fixed, true) }; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 48a11e7..872206b 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -97,7 +97,7 @@ impl CollisionPipeline { modified_colliders: &mut Vec, ) { for handle in modified_colliders.drain(..) { - colliders.set_internal(handle.0, ColliderChanges::empty()) + colliders.index_mut_internal(handle).changes = ColliderChanges::empty(); } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index ffcb25f..71c559a 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -5,8 +5,7 @@ use crate::counters::Counters; use crate::dynamics::IslandSolver; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyColliders, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, - RigidBodyType, RigidBodyVelocity, + RigidBodyHandle, RigidBodyPosition, RigidBodyType, }; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; @@ -72,7 +71,9 @@ impl PhysicsPipeline { modified_colliders: &mut Vec, ) { for handle in modified_colliders.drain(..) { - colliders.set_internal(handle.0, ColliderChanges::empty()) + if let Some(co) = colliders.get_mut_internal(handle) { + co.changes = ColliderChanges::empty(); + } } } @@ -187,18 +188,11 @@ impl PhysicsPipeline { self.counters.stages.update_time.resume(); for handle in islands.active_dynamic_bodies() { - let poss: &RigidBodyPosition = bodies.index(handle.0); - let position = poss.position; - - let effective_inv_mass = bodies - .map_mut_internal(handle.0, |mprops: &mut RigidBodyMassProps| { - mprops.update_world_mass_properties(&position); - mprops.effective_mass() - }) - .unwrap(); - bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| { - forces.compute_effective_force_and_torque(&gravity, &effective_inv_mass) - }); + let rb = bodies.index_mut_internal(*handle); + rb.mprops.update_world_mass_properties(&rb.pos.position); + let effective_mass = rb.mprops.effective_mass(); + rb.forces + .compute_effective_force_and_torque(&gravity, &effective_mass); } for multibody in &mut multibody_joints.multibodies { @@ -319,13 +313,10 @@ impl PhysicsPipeline { ) { // Set the rigid-bodies and kinematic bodies to their final position. for handle in islands.iter_active_bodies() { - bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| { - poss.position = poss.next_position - }); - - let (rb_poss, rb_colls): (&RigidBodyPosition, &RigidBodyColliders) = - bodies.index_bundle(handle.0); - rb_colls.update_positions(colliders, modified_colliders, &rb_poss.position); + let rb = bodies.index_mut_internal(handle); + rb.pos.position = rb.pos.next_position; + rb.colliders + .update_positions(colliders, modified_colliders, &rb.pos.position); } } @@ -341,29 +332,22 @@ impl PhysicsPipeline { // there to determine if this kinematic body should wake-up dynamic // bodies it is touching. for handle in islands.active_kinematic_bodies() { - let (rb_type, rb_pos, rb_vel, rb_mprops): ( - &RigidBodyType, - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); + let rb = bodies.index_mut_internal(*handle); - match rb_type { + match rb.body_type { RigidBodyType::KinematicPositionBased => { - let rb_pos: &RigidBodyPosition = bodies.index(handle.0); - let new_vel = rb_pos.interpolate_velocity( + rb.vels = rb.pos.interpolate_velocity( integration_parameters.inv_dt(), - &rb_mprops.local_mprops.local_com, + &rb.mprops.local_mprops.local_com, ); - bodies.set_internal(handle.0, new_vel); } RigidBodyType::KinematicVelocityBased => { - let new_pos = rb_vel.integrate( + let new_pos = rb.vels.integrate( integration_parameters.dt, - &rb_pos.position, - &rb_mprops.local_mprops.local_com, + &rb.pos.position, + &rb.mprops.local_mprops.local_com, ); - bodies.set_internal(handle.0, RigidBodyPosition::from(new_pos)); + rb.pos = RigidBodyPosition::from(new_pos); } _ => {} } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 584989e..3735d12 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,10 +1,6 @@ -use crate::dynamics::{ - IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition, - RigidBodyVelocity, -}; +use crate::dynamics::IslandManager; use crate::geometry::{ - ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, - InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH, + ColliderHandle, InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH, }; use crate::math::{Isometry, Point, Real, Vector}; use parry::partitioning::QBVHDataGenerator; @@ -75,7 +71,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { if co.flags.collision_groups.test(self.query_groups) && self.filter.map(|f| f(shape_id)).unwrap_or(true) { - f(Some(co.pos), &**co.shape) + f(Some(&co.pos), &*co.shape) } } } @@ -189,54 +185,35 @@ impl QueryPipeline { fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) { match self.mode { QueryPipelineMode::CurrentPosition => { - self.colliders.for_each(|h, co_shape: &ColliderShape| { - let co_pos: &ColliderPosition = self.colliders.index(h); - f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) - }) + for (h, co) in self.colliders.iter() { + f(h, co.shape.compute_aabb(&co.pos)) + } } QueryPipelineMode::SweepTestWithNextPosition => { - self.colliders.for_each(|h, co_shape: &ColliderShape| { - let co_parent: Option<&ColliderParent> = self.colliders.get(h); - let co_pos: &ColliderPosition = self.colliders.index(h); - - if let Some(co_parent) = co_parent { - let rb_pos: &RigidBodyPosition = - self.bodies.index(co_parent.handle.0); - let next_position = rb_pos.next_position * co_parent.pos_wrt_parent; - - f( - ColliderHandle(h), - co_shape.compute_swept_aabb(&co_pos, &next_position), - ) + for (h, co) in self.colliders.iter() { + if let Some(co_parent) = co.parent { + let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position; + let next_position = rb_next_pos * co_parent.pos_wrt_parent; + f(h, co.shape.compute_swept_aabb(&co.pos, &next_position)) } else { - f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) + f(h, co.shape.compute_aabb(&co.pos)) } - }) + } } QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - self.colliders.for_each(|h, co_shape: &ColliderShape| { - let co_parent: Option<&ColliderParent> = self.colliders.get(h); - let co_pos: &ColliderPosition = self.colliders.index(h); - - if let Some(co_parent) = co_parent { - let (rb_pos, vels, forces, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyMassProps, - ) = self.bodies.index_bundle(co_parent.handle.0); - let predicted_pos = rb_pos - .integrate_forces_and_velocities(dt, forces, vels, mprops); + for (h, co) in self.colliders.iter() { + if let Some(co_parent) = co.parent { + let rb = &self.bodies[co_parent.handle]; + let predicted_pos = rb.pos.integrate_forces_and_velocities( + dt, &rb.forces, &rb.vels, &rb.mprops, + ); let next_position = predicted_pos * co_parent.pos_wrt_parent; - f( - ColliderHandle(h), - co_shape.compute_swept_aabb(&co_pos, &next_position), - ) + f(h, co.shape.compute_swept_aabb(&co.pos, &next_position)) } else { - f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) + f(h, co.shape.compute_aabb(&co.pos)) } - }) + } } } } @@ -256,8 +233,8 @@ impl QueryPipeline { } for handle in islands.iter_active_bodies() { - let body_colliders: &RigidBodyColliders = bodies.index(handle.0); - for handle in &body_colliders.0 { + let rb = &bodies[handle]; + for handle in &rb.colliders.0 { self.qbvh.pre_update(*handle) } } @@ -266,9 +243,8 @@ impl QueryPipeline { QueryPipelineMode::CurrentPosition => { self.qbvh.update( |handle| { - let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = - colliders.index_bundle(handle.0); - co_shape.compute_aabb(&co_pos) + let co = &colliders[*handle]; + co.shape.compute_aabb(&co.pos) }, self.dilation_factor, ); @@ -276,10 +252,10 @@ impl QueryPipeline { QueryPipelineMode::SweepTestWithNextPosition => { self.qbvh.update( |handle| { - let co = &colliders[handle]; - if let Some(parent) = co.parent { - let rb_pos: &RigidBodyPosition = bodies.index(co.parent.handle.0); - let next_position = rb_pos.next_position * co.parent.pos_wrt_parent; + let co = &colliders[*handle]; + if let Some(parent) = &co.parent { + let rb_next_pos = &bodies[parent.handle].pos.next_position; + let next_position = rb_next_pos * parent.pos_wrt_parent; co.shape.compute_swept_aabb(&co.pos, &next_position) } else { co.shape.compute_aabb(&co.pos) @@ -291,12 +267,12 @@ impl QueryPipeline { QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { self.qbvh.update( |handle| { - let co = &colliders[handle]; + let co = &colliders[*handle]; if let Some(parent) = co.parent { let rb = &bodies[parent.handle]; - let predicted_pos = rb - .pos - .integrate_forces_and_velocities(dt, rb.forces, rb.vels, rb.mprops); + let predicted_pos = rb.pos.integrate_forces_and_velocities( + dt, &rb.forces, &rb.vels, &rb.mprops, + ); let next_position = predicted_pos * parent.pos_wrt_parent; co.shape.compute_swept_aabb(&co.pos, &next_position) @@ -394,7 +370,7 @@ impl QueryPipeline { /// * `callback`: function executed on each collider for which a ray intersection has been found. /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, /// this method will exit early, ignore any further raycast. - pub fn intersections_with_ray<'a, ColliderSet>( + pub fn intersections_with_ray<'a>( &self, colliders: &'a ColliderSet, ray: &Ray, @@ -405,14 +381,13 @@ impl QueryPipeline { mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool, ) { let mut leaf_callback = &mut |handle: &ColliderHandle| { - let co_shape: Option<&ColliderShape> = colliders.get(handle.0); - if let Some(co_shape) = co_shape { - let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) = - colliders.index_bundle(handle.0); - if co_flags.collision_groups.test(query_groups) + if let Some(co) = colliders.get(*handle) { + if co.flags.collision_groups.test(query_groups) && filter.map(|f| f(*handle)).unwrap_or(true) { - if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid) + if let Some(hit) = co + .shape + .cast_ray_and_get_normal(&co.pos, ray, max_toi, solid) { return callback(*handle, hit); } @@ -502,7 +477,7 @@ impl QueryPipeline { /// is either `None` or returns `true`. /// * `callback` - A function called with each collider with a shape /// containing the `point`. - pub fn intersections_with_point<'a, ColliderSet>( + pub fn intersections_with_point<'a>( &self, colliders: &'a ColliderSet, point: &Point, @@ -511,15 +486,10 @@ impl QueryPipeline { mut callback: impl FnMut(ColliderHandle) -> bool, ) { let mut leaf_callback = &mut |handle: &ColliderHandle| { - let co_shape: Option<&ColliderShape> = colliders.get(handle.0); - - if let Some(co_shape) = co_shape { - let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) = - colliders.index_bundle(handle.0); - - if co_flags.collision_groups.test(query_groups) + if let Some(co) = colliders.get(*handle) { + if co.flags.collision_groups.test(query_groups) && filter.map(|f| f(*handle)).unwrap_or(true) - && co_shape.contains_point(co_pos, point) + && co.shape.contains_point(&co.pos, point) { return callback(*handle); } @@ -677,7 +647,7 @@ impl QueryPipeline { /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. /// * `callback` - A function called with the handles of each collider intersecting the `shape`. - pub fn intersections_with_shape<'a, ColliderSet>( + pub fn intersections_with_shape<'a>( &self, colliders: &'a ColliderSet, shape_pos: &Isometry, @@ -690,18 +660,13 @@ impl QueryPipeline { let inv_shape_pos = shape_pos.inverse(); let mut leaf_callback = &mut |handle: &ColliderHandle| { - let co_shape: Option<&ColliderShape> = colliders.get(handle.0); - - if let Some(co_shape) = co_shape { - let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) = - colliders.index_bundle(handle.0); - - if co_flags.collision_groups.test(query_groups) + if let Some(co) = colliders.get(*handle) { + if co.flags.collision_groups.test(query_groups) && filter.map(|f| f(*handle)).unwrap_or(true) { - let pos12 = inv_shape_pos * co_pos.as_ref(); + let pos12 = inv_shape_pos * co.pos.as_ref(); - if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) { + if dispatcher.intersection_test(&pos12, shape, &*co.shape) == Ok(true) { return callback(*handle); } } diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index b999f0f..69f29d9 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -1,6 +1,6 @@ use crate::dynamics::{ IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, - RigidBodyPosition, RigidBodySet, RigidBodyType, + RigidBodySet, RigidBodyType, }; use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet}; use parry::utils::hashmap::HashMap; @@ -17,15 +17,13 @@ pub(crate) fn handle_user_changes_to_colliders( for handle in modified_colliders { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. - if let Some(co) = colliders.get(*handle) { + if let Some(co) = colliders.get_mut_internal(*handle) { if co.changes.contains(ColliderChanges::PARENT) { if let Some(co_parent) = co.parent { - let parent_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0); + let parent_rb = &bodies[co_parent.handle]; - let new_pos = parent_pos.position * co_parent.pos_wrt_parent; - let new_changes = co.changes | ColliderChanges::POSITION; - colliders.set_internal(handle.0, ColliderPosition(new_pos)); - colliders.set_internal(handle.0, new_changes); + co.pos = ColliderPosition(parent_rb.pos.position * co_parent.pos_wrt_parent); + co.changes |= ColliderChanges::POSITION; } } @@ -38,18 +36,12 @@ pub(crate) fn handle_user_changes_to_colliders( } for (to_update, _) in mprops_to_update { - let rb = &bodies[to_update]; - let position = rb.position(); - // FIXME: remove the clone once we remove the ComponentSets. - let attached_colliders = rb.colliders().clone(); - - bodies.map_mut_internal(to_update.0, |rb_mprops| { - rb_mprops.recompute_mass_properties_from_colliders( - colliders, - &attached_colliders, - &position, - ) - }); + let rb = bodies.index_mut_internal(to_update); + rb.mprops.recompute_mass_properties_from_colliders( + colliders, + &rb.colliders, + &rb.pos.position, + ); } } @@ -73,7 +65,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( continue; } - let rb = &bodies[handle]; + let rb = bodies.index_mut_internal(*handle); let mut changes = rb.changes; let mut ids: RigidBodyIds = rb.ids; let mut activation: RigidBodyActivation = rb.activation; @@ -83,7 +75,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // it is on the correct active set. if let Some(islands) = islands.as_deref_mut() { if changes.contains(RigidBodyChanges::TYPE) { - match rb.status { + match rb.body_type { RigidBodyType::Dynamic => { // Remove from the active kinematic set if it was there. if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { @@ -162,20 +154,19 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( || changes.contains(RigidBodyChanges::TYPE) { for handle in rb.colliders.0.iter() { - colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| { - if !co_changes.contains(ColliderChanges::MODIFIED) { - modified_colliders.push(*handle); - } + let co = colliders.index_mut_internal(*handle); + if !co.changes.contains(ColliderChanges::MODIFIED) { + modified_colliders.push(*handle); + } - *co_changes |= - ColliderChanges::MODIFIED | ColliderChanges::PARENT_EFFECTIVE_DOMINANCE; - }); + co.changes |= + ColliderChanges::MODIFIED | ColliderChanges::PARENT_EFFECTIVE_DOMINANCE; } } - bodies.set_internal(handle.0, RigidBodyChanges::empty()); - bodies.set_internal(handle.0, ids); - bodies.set_internal(handle.0, activation); + rb.changes = RigidBodyChanges::empty(); + rb.ids = ids; + rb.activation = activation; } // Adjust some ids, if needed. @@ -187,9 +178,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( }; if id < active_set.len() { - bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| { - ids2.active_set_id = id; - }); + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; } } } diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index a09e0ac..5b71aa5 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -156,7 +156,7 @@ impl Harness { impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, gravity: Vector, - hooks: impl PhysicsHooks + 'static, + hooks: impl PhysicsHooks + 'static, ) { // println!("Num bodies: {}", bodies.len()); // println!("Num impulse_joints: {}", impulse_joints.len()); diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs index c8168d9..a848ea8 100644 --- a/src_testbed/physics/mod.rs +++ b/src_testbed/physics/mod.rs @@ -83,7 +83,7 @@ pub struct PhysicsState { pub query_pipeline: QueryPipeline, pub integration_parameters: IntegrationParameters, pub gravity: Vector, - pub hooks: Box>, + pub hooks: Box, } impl PhysicsState { diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index cd0ff3a..1bb30eb 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -493,7 +493,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, gravity: Vector, - hooks: impl PhysicsHooks + 'static, + hooks: impl PhysicsHooks + 'static, ) { self.harness.set_world_with_params( bodies,