diff --git a/CHANGELOG.md b/CHANGELOG.md index 61bce74..2d3dcf9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,8 @@ +## v0.9.0 + +### Modified +- Renamed `BodyStatus` to `RigidBodyType`. + ## v0.8.0 ### Modified - Switch to nalgebra 0.26. diff --git a/benchmarks2d/balls2.rs b/benchmarks2d/balls2.rs index 6791505..9a28195 100644 --- a/benchmarks2d/balls2.rs +++ b/benchmarks2d/balls2.rs @@ -1,5 +1,5 @@ use na::Point2; -use rapier2d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -42,9 +42,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shifty + centery; let status = if j == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; // Build the rigid body. diff --git a/benchmarks2d/joint_ball2.rs b/benchmarks2d/joint_ball2.rs index ecfdc47..35423e7 100644 --- a/benchmarks2d/joint_ball2.rs +++ b/benchmarks2d/joint_ball2.rs @@ -1,5 +1,5 @@ use na::Point2; -use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -28,9 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/benchmarks2d/joint_fixed2.rs b/benchmarks2d/joint_fixed2.rs index 32a219e..861912c 100644 --- a/benchmarks2d/joint_fixed2.rs +++ b/benchmarks2d/joint_fixed2.rs @@ -1,5 +1,5 @@ use na::{Isometry2, Point2}; -use rapier2d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -33,9 +33,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if k == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/benchmarks3d/balls3.rs b/benchmarks3d/balls3.rs index bdbe75e..1f0f1df 100644 --- a/benchmarks3d/balls3.rs +++ b/benchmarks3d/balls3.rs @@ -1,5 +1,5 @@ use na::Point3; -use rapier3d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -30,9 +30,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; let status = if j == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let density = 0.477; diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs index fee32a2..12b62a9 100644 --- a/benchmarks3d/joint_ball3.rs +++ b/benchmarks3d/joint_ball3.rs @@ -1,5 +1,5 @@ use na::Point3; -use rapier3d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -23,9 +23,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if i == 0 && (k % 4 == 0 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs index c8912e7..9839e38 100644 --- a/benchmarks3d/joint_fixed3.rs +++ b/benchmarks3d/joint_fixed3.rs @@ -1,5 +1,5 @@ use na::{Isometry3, Point3}; -use rapier3d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::dynamics::{FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) { // a joint between these. let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/build/rapier2d-f64/Cargo.toml b/build/rapier2d-f64/Cargo.toml index 3314825..d244d9b 100644 --- a/build/rapier2d-f64/Cargo.toml +++ b/build/rapier2d-f64/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim2", "f64" ] +default = [ "dim2", "f64", "default-sets" ] dim2 = [ ] f64 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 790bb94..a149bd9 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim2", "f32" ] +default = [ "dim2", "f32", "default-sets" ] dim2 = [ ] f32 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "simba/wide", "simd-is-enabled" ] simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] diff --git a/build/rapier3d-f64/Cargo.toml b/build/rapier3d-f64/Cargo.toml index cb4f03c..4b6519c 100644 --- a/build/rapier3d-f64/Cargo.toml +++ b/build/rapier3d-f64/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim3", "f64" ] +default = [ "dim3", "f64", "default-sets" ] dim3 = [ ] f64 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ] simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index e797ea5..8a3877b 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -16,9 +16,10 @@ edition = "2018" maintenance = { status = "actively-developed" } [features] -default = [ "dim3", "f32" ] +default = [ "dim3", "f32", "default-sets" ] dim3 = [ ] f32 = [ ] +default-sets = [ ] parallel = [ "rayon" ] simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ] simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs index 0aeffbe..826d5c9 100644 --- a/examples2d/add_remove2.rs +++ b/examples2d/add_remove2.rs @@ -31,9 +31,12 @@ pub fn init_world(testbed: &mut Testbed) { .map(|e| e.0) .collect(); for handle in to_remove { - physics - .bodies - .remove(handle, &mut physics.colliders, &mut physics.joints); + physics.bodies.remove( + handle, + &mut physics.islands, + &mut physics.colliders, + &mut physics.joints, + ); if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { graphics.remove_body_nodes(*window, handle); diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index d1a1942..4d7d060 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -1,5 +1,5 @@ use na::Point2; -use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::dynamics::{BallJoint, JointSet, RigidBodyBuilder, RigidBodySet, RigidBodyType}; use rapier2d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed2d::Testbed; @@ -31,9 +31,9 @@ pub fn init_world(testbed: &mut Testbed) { let fi = i as f32; let status = if i == 0 && k == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index fc3acb1..90607f7 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -9,12 +9,15 @@ struct OneWayPlatformHook { platform2: ColliderHandle, } -impl PhysicsHooks for OneWayPlatformHook { +impl PhysicsHooks for OneWayPlatformHook { fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS } - fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + 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. // @@ -29,16 +32,16 @@ impl PhysicsHooks for OneWayPlatformHook { // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y. let mut allowed_local_n1 = Vector2::zeros(); - if context.collider_handle1 == self.platform1 { + if context.collider1 == self.platform1 { allowed_local_n1 = Vector2::y(); - } else if context.collider_handle2 == self.platform1 { + } else if context.collider2 == self.platform1 { // Flip the allowed direction. allowed_local_n1 = -Vector2::y(); } - if context.collider_handle1 == self.platform2 { + if context.collider1 == self.platform2 { allowed_local_n1 = -Vector2::y(); - } else if context.collider_handle2 == self.platform2 { + } else if context.collider2 == self.platform2 { // Flip the allowed direction. allowed_local_n1 = Vector2::y(); } @@ -47,13 +50,12 @@ impl PhysicsHooks for OneWayPlatformHook { context.update_as_oneway_platform(&allowed_local_n1, 0.1); // Set the surface velocity of the accepted contacts. - let tangent_velocity = if context.collider_handle1 == self.platform1 - || context.collider_handle2 == self.platform2 - { - -12.0 - } else { - 12.0 - }; + let tangent_velocity = + if context.collider1 == self.platform1 || context.collider2 == self.platform2 { + -12.0 + } else { + 12.0 + }; for contact in context.solver_contacts.iter_mut() { contact.tangent_velocity.x = tangent_velocity; @@ -115,13 +117,14 @@ pub fn init_world(testbed: &mut Testbed) { } } - physics.bodies.foreach_active_dynamic_body_mut(|_, body| { + for handle in physics.islands.active_dynamic_bodies() { + let body = &mut physics.bodies[*handle]; if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { body.set_gravity_scale(-1.0, false); } - }); + } }); /* diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs index f353162..9aab754 100644 --- a/examples3d/debug_add_remove_collider3.rs +++ b/examples3d/debug_add_remove_collider3.rs @@ -38,7 +38,12 @@ pub fn init_world(testbed: &mut Testbed) { // Remove then re-add the ground collider. let coll = physics .colliders - .remove(ground_collider_handle, &mut physics.bodies, true) + .remove( + ground_collider_handle, + &mut physics.islands, + &mut physics.bodies, + true, + ) .unwrap(); ground_collider_handle = physics .colliders diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs index bd7205e..61c1482 100644 --- a/examples3d/debug_dynamic_collider_add3.rs +++ b/examples3d/debug_dynamic_collider_add3.rs @@ -79,7 +79,9 @@ pub fn init_world(testbed: &mut Testbed) { step = snapped_frame; for handle in &extra_colliders { - physics.colliders.remove(*handle, &mut physics.bodies, true); + physics + .colliders + .remove(*handle, &mut physics.islands, &mut physics.bodies, true); } extra_colliders.clear(); diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index 6c80562..08e1dfe 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -61,9 +61,12 @@ pub fn init_world(testbed: &mut Testbed) { let num_to_remove = to_remove.len() - MAX_NUMBER_OF_BODIES; for (handle, _) in &to_remove[..num_to_remove] { - physics - .bodies - .remove(*handle, &mut physics.colliders, &mut physics.joints); + physics.bodies.remove( + *handle, + &mut physics.islands, + &mut physics.colliders, + &mut physics.joints, + ); if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { graphics.remove_body_nodes(window, *handle); diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 6ba9461..5b082fb 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -1,7 +1,7 @@ use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3}; use rapier3d::dynamics::{ - BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, - RigidBodyHandle, RigidBodySet, + BallJoint, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, + RigidBodyHandle, RigidBodySet, RigidBodyType, }; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -203,9 +203,9 @@ fn create_fixed_joints( // fixed bodies. Because physx will crash if we add // a joint between these. let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) @@ -258,9 +258,9 @@ fn create_ball_joints( let fi = i as f32; let status = if i == 0 && (k % 4 == 0 || k == num - 1) { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) @@ -317,9 +317,9 @@ fn create_actuated_revolute_joints( // fixed bodies. Because physx will crash if we add // a joint between these. let status = if i == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let shifty = (i >= 1) as u32 as f32 * -2.0; @@ -378,9 +378,9 @@ fn create_actuated_ball_joints( // fixed bodies. Because physx will crash if we add // a joint between these. let status = if i == 0 { - BodyStatus::Static + RigidBodyType::Static } else { - BodyStatus::Dynamic + RigidBodyType::Dynamic }; let rigid_body = RigidBodyBuilder::new(status) diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index d117a5b..f5a2f1b 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -9,36 +9,39 @@ struct OneWayPlatformHook { platform2: ColliderHandle, } -impl PhysicsHooks for OneWayPlatformHook { +impl PhysicsHooks for OneWayPlatformHook { fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS } - fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + 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. // // Now we have to be careful because the `manifold.local_n1` normal points // toward the outside of the shape of `context.co1`. So we need to flip the - // allowed normal direction if the platform is in `context.collider_handle2`. + // allowed normal direction if the platform is in `context.collider2`. // // Therefore: - // - If context.collider_handle1 == self.platform1 then the allowed normal is +y. - // - If context.collider_handle2 == self.platform1 then the allowed normal is -y. - // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y. - // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y. + // - If context.collider1 == self.platform1 then the allowed normal is +y. + // - If context.collider2 == self.platform1 then the allowed normal is -y. + // - If context.collider1 == self.platform2 then its allowed normal +y needs to be flipped to -y. + // - If context.collider2 == self.platform2 then the allowed normal -y needs to be flipped to +y. let mut allowed_local_n1 = Vector3::zeros(); - if context.collider_handle1 == self.platform1 { + if context.collider1 == self.platform1 { allowed_local_n1 = Vector3::y(); - } else if context.collider_handle2 == self.platform1 { + } else if context.collider2 == self.platform1 { // Flip the allowed direction. allowed_local_n1 = -Vector3::y(); } - if context.collider_handle1 == self.platform2 { + if context.collider1 == self.platform2 { allowed_local_n1 = -Vector3::y(); - } else if context.collider_handle2 == self.platform2 { + } else if context.collider2 == self.platform2 { // Flip the allowed direction. allowed_local_n1 = Vector3::y(); } @@ -47,13 +50,12 @@ impl PhysicsHooks for OneWayPlatformHook { context.update_as_oneway_platform(&allowed_local_n1, 0.1); // Set the surface velocity of the accepted contacts. - let tangent_velocity = if context.collider_handle1 == self.platform1 - || context.collider_handle2 == self.platform2 - { - -12.0 - } else { - 12.0 - }; + let tangent_velocity = + if context.collider1 == self.platform1 || context.collider2 == self.platform2 { + -12.0 + } else { + 12.0 + }; for contact in context.solver_contacts.iter_mut() { contact.tangent_velocity.z = tangent_velocity; @@ -115,13 +117,14 @@ pub fn init_world(testbed: &mut Testbed) { } } - physics.bodies.foreach_active_dynamic_body_mut(|_, body| { + for handle in physics.islands.active_dynamic_bodies() { + let body = physics.bodies.get_mut(*handle).unwrap(); if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { body.set_gravity_scale(-1.0, false); } - }); + } }); /* diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index 7b5c986..cbe5b15 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -1,4 +1,4 @@ -use na::Point3; +use na::{Isometry3, Point3, UnitQuaternion, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -14,7 +14,7 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground. */ - let ground_size = 10.0; + let ground_size = 20.0; let ground_height = 0.1; let rigid_body = RigidBodyBuilder::new_static() @@ -27,25 +27,28 @@ pub fn init_world(testbed: &mut Testbed) { /* * Create the boxes */ - let num = 6; + let num = 1; let rad = 0.2; let shift = rad * 2.0; let centerx = shift * num as f32 / 2.0; - let centery = shift / 2.0 + 3.04; + let centery = shift / 2.0; let centerz = shift * num as f32 / 2.0; - for i in 0usize..num { + for i in 0usize..20 { for j in 0usize..num { for k in 0usize..num { - let x = i as f32 * shift - centerx; + let x = i as f32 * (shift + rad / 4.0) - centerx; let y = j as f32 * shift + centery; let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(x, y + rad, z) + .ccd_enabled(true) + .build(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad * 2.0, rad).build(); colliders.insert(collider, handle, &mut bodies); } } @@ -55,11 +58,16 @@ pub fn init_world(testbed: &mut Testbed) { * Setup a kinematic rigid body. */ let platform_body = RigidBodyBuilder::new_kinematic() - .translation(0.0, 1.5 + 0.8, -10.0 * rad) + .translation(0.2, 0.4, -40.0 * rad) .build(); let platform_handle = bodies.insert(platform_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0).build(); - colliders.insert(collider, platform_handle, &mut bodies); + let collider1 = ColliderBuilder::cuboid(rad * 5.0, rad * 2.0, rad * 10.0).build(); + let collider2 = ColliderBuilder::cuboid(rad * 5.0, rad * 2.0, rad * 10.0) + .position_wrt_parent(Isometry3::translation(0.0, rad * 2.1, 0.0)) + .build(); + colliders.insert(collider1, platform_handle, &mut bodies); + colliders.insert(collider2, platform_handle, &mut bodies); + testbed.set_body_color(platform_handle, Point3::new(1.0, 1.0, 0.0)); /* * Setup a callback to control the platform. @@ -67,23 +75,27 @@ pub fn init_world(testbed: &mut Testbed) { let mut count = 0; testbed.add_callback(move |_, _, physics, _, run_state| { count += 1; - if count % 100 > 50 { - return; - } + // if count % 100 > 50 { + // return; + // } if let Some(platform) = physics.bodies.get_mut(platform_handle) { let mut next_pos = *platform.position(); let dt = 0.016; - next_pos.translation.vector.y += (run_state.time * 5.0).sin() * dt; - next_pos.translation.vector.z += run_state.time.sin() * 5.0 * dt; + // next_pos.translation.vector.y += (run_state.time * 5.0).sin() * dt; + // next_pos.translation.vector.z += run_state.time.sin() * 5.0 * dt; - if next_pos.translation.vector.z >= rad * 10.0 { - next_pos.translation.vector.z -= dt; - } - if next_pos.translation.vector.z <= -rad * 10.0 { - next_pos.translation.vector.z += dt; - } + let drot = UnitQuaternion::new(Vector3::y() * 0.01); + next_pos.rotation = drot * next_pos.rotation; + next_pos.translation.vector += next_pos.rotation * Vector3::z() * 0.1; + + // if next_pos.translation.vector.z >= rad * 10.0 { + // next_pos.translation.vector.z -= dt; + // } + // if next_pos.translation.vector.z <= -rad * 10.0 { + // next_pos.translation.vector.z += dt; + // } platform.set_next_kinematic_position(next_pos); } diff --git a/src/data/arena.rs b/src/data/arena.rs index 9d057b8..bc7176d 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -19,16 +19,16 @@ use std::vec; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct Arena { items: Vec>, - generation: u64, - free_list_head: Option, + generation: u32, + free_list_head: Option, len: usize, } #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] enum Entry { - Free { next_free: Option }, - Occupied { generation: u64, value: T }, + Free { next_free: Option }, + Occupied { generation: u32, value: T }, } /// An index (and generation) into an `Arena`. @@ -48,17 +48,17 @@ enum Entry { #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct Index { - index: usize, - generation: u64, + index: u32, + generation: u32, } impl IndexedData for Index { fn default() -> Self { - Self::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + Self::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32) } fn index(&self) -> usize { - self.into_raw_parts().0 + self.into_raw_parts().0 as usize } } @@ -70,7 +70,7 @@ impl Index { /// /// Providing arbitrary values will lead to malformed indices and ultimately /// panics. - pub fn from_raw_parts(a: usize, b: u64) -> Index { + pub fn from_raw_parts(a: u32, b: u32) -> Index { Index { index: a, generation: b, @@ -84,7 +84,7 @@ impl Index { /// `Index` like `pub struct MyIdentifier(Index);`. However, for external /// types whose definition you can't customize, but which you can construct /// instances of, this method can be useful. - pub fn into_raw_parts(self) -> (usize, u64) { + pub fn into_raw_parts(self) -> (u32, u32) { (self.index, self.generation) } } @@ -161,7 +161,7 @@ impl Arena { pub fn clear(&mut self) { self.items.clear(); - let end = self.items.capacity(); + let end = self.items.capacity() as u32; self.items.extend((0..end).map(|i| { if i == end - 1 { Entry::Free { next_free: None } @@ -206,7 +206,7 @@ impl Arena { match self.try_alloc_next_index() { None => Err(value), Some(index) => { - self.items[index.index] = Entry::Occupied { + self.items[index.index as usize] = Entry::Occupied { generation: self.generation, value, }; @@ -247,7 +247,7 @@ impl Arena { match self.try_alloc_next_index() { None => Err(create), Some(index) => { - self.items[index.index] = Entry::Occupied { + self.items[index.index as usize] = Entry::Occupied { generation: self.generation, value: create(index), }; @@ -260,13 +260,13 @@ impl Arena { fn try_alloc_next_index(&mut self) -> Option { match self.free_list_head { None => None, - Some(i) => match self.items[i] { + Some(i) => match self.items[i as usize] { Entry::Occupied { .. } => panic!("corrupt free list"), Entry::Free { next_free } => { self.free_list_head = next_free; self.len += 1; Some(Index { - index: i, + index: i as u32, generation: self.generation, }) } @@ -355,14 +355,14 @@ impl Arena { /// assert_eq!(arena.remove(idx), None); /// ``` pub fn remove(&mut self, i: Index) -> Option { - if i.index >= self.items.len() { + if i.index >= self.items.len() as u32 { return None; } - match self.items[i.index] { + match self.items[i.index as usize] { Entry::Occupied { generation, .. } if i.generation == generation => { let entry = mem::replace( - &mut self.items[i.index], + &mut self.items[i.index as usize], Entry::Free { next_free: self.free_list_head, }, @@ -402,8 +402,8 @@ impl Arena { /// assert!(crew_members.next().is_none()); /// ``` pub fn retain(&mut self, mut predicate: impl FnMut(Index, &mut T) -> bool) { - for i in 0..self.capacity() { - let remove = match &mut self.items[i] { + for i in 0..self.capacity() as u32 { + let remove = match &mut self.items[i as usize] { Entry::Occupied { generation, value } => { let index = Index { index: i, @@ -462,7 +462,7 @@ impl Arena { /// assert!(arena.get(idx).is_none()); /// ``` pub fn get(&self, i: Index) -> Option<&T> { - match self.items.get(i.index) { + match self.items.get(i.index as usize) { Some(Entry::Occupied { generation, value }) if *generation == i.generation => { Some(value) } @@ -488,7 +488,7 @@ impl Arena { /// assert!(arena.get_mut(idx).is_none()); /// ``` pub fn get_mut(&mut self, i: Index) -> Option<&mut T> { - match self.items.get_mut(i.index) { + match self.items.get_mut(i.index as usize) { Some(Entry::Occupied { generation, value }) if *generation == i.generation => { Some(value) } @@ -526,7 +526,7 @@ impl Arena { /// assert_eq!(arena[idx2], 4); /// ``` pub fn get2_mut(&mut self, i1: Index, i2: Index) -> (Option<&mut T>, Option<&mut T>) { - let len = self.items.len(); + let len = self.items.len() as u32; if i1.index == i2.index { assert!(i1.generation != i2.generation); @@ -544,11 +544,13 @@ impl Arena { } let (raw_item1, raw_item2) = { - let (xs, ys) = self.items.split_at_mut(cmp::max(i1.index, i2.index)); + let (xs, ys) = self + .items + .split_at_mut(cmp::max(i1.index, i2.index) as usize); if i1.index < i2.index { - (&mut xs[i1.index], &mut ys[0]) + (&mut xs[i1.index as usize], &mut ys[0]) } else { - (&mut ys[0], &mut xs[i2.index]) + (&mut ys[0], &mut xs[i2.index as usize]) } }; @@ -666,11 +668,11 @@ impl Arena { } } else { Entry::Free { - next_free: Some(i + 1), + next_free: Some(i as u32 + 1), } } })); - self.free_list_head = Some(start); + self.free_list_head = Some(start as u32); } /// Iterate over shared references to the elements in this arena. @@ -774,7 +776,7 @@ impl Arena { value, Index { generation: *generation, - index: i, + index: i as u32, }, )), _ => None, @@ -797,7 +799,7 @@ impl Arena { value, Index { generation: *generation, - index: i, + index: i as u32, }, )), _ => None, @@ -941,7 +943,10 @@ impl<'a, T> Iterator for Iter<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -970,7 +975,10 @@ impl<'a, T> DoubleEndedIterator for Iter<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1039,7 +1047,10 @@ impl<'a, T> Iterator for IterMut<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1068,7 +1079,10 @@ impl<'a, T> DoubleEndedIterator for IterMut<'a, T> { }, )) => { self.len -= 1; - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => { @@ -1126,7 +1140,10 @@ impl<'a, T> Iterator for Drain<'a, T> { match self.inner.next() { Some((_, Entry::Free { .. })) => continue, Some((index, Entry::Occupied { generation, value })) => { - let idx = Index { index, generation }; + let idx = Index { + index: index as u32, + generation, + }; return Some((idx, value)); } None => return None, diff --git a/src/data/coarena.rs b/src/data/coarena.rs index c25cc55..cd64910 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -4,7 +4,7 @@ use crate::data::arena::Index; #[derive(Clone, Debug)] /// A container for data associated to item existing into another Arena. pub struct Coarena { - data: Vec<(u64, T)>, + data: Vec<(u32, T)>, } impl Coarena { @@ -17,7 +17,7 @@ impl Coarena { pub fn get(&self, index: Index) -> Option<&T> { let (i, g) = index.into_raw_parts(); self.data - .get(i) + .get(i as usize) .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } @@ -25,7 +25,7 @@ impl Coarena { pub fn get_mut(&mut self, index: Index) -> Option<&mut T> { let (i, g) = index.into_raw_parts(); self.data - .get_mut(i) + .get_mut(i as usize) .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } @@ -36,11 +36,11 @@ impl Coarena { { let (i1, g1) = a.into_raw_parts(); - if self.data.len() <= i1 { - self.data.resize(i1 + 1, (u32::MAX as u64, T::default())); + if self.data.len() <= i1 as usize { + self.data.resize(i1 as usize + 1, (u32::MAX, T::default())); } - self.data[i1] = (g1, value); + self.data[i1 as usize] = (g1, value); } /// Ensure that elements at the two given indices exist in this coarena, and return their reference. @@ -56,20 +56,22 @@ impl Coarena { assert_ne!(i1, i2, "Cannot index the same object twice."); let (elt1, elt2) = if i1 > i2 { - if self.data.len() <= i1 { - self.data.resize(i1 + 1, (u32::MAX as u64, default.clone())); + if self.data.len() <= i1 as usize { + self.data + .resize(i1 as usize + 1, (u32::MAX, default.clone())); } - let (left, right) = self.data.split_at_mut(i1); - (&mut right[0], &mut left[i2]) + let (left, right) = self.data.split_at_mut(i1 as usize); + (&mut right[0], &mut left[i2 as usize]) } else { // i2 > i1 - if self.data.len() <= i2 { - self.data.resize(i2 + 1, (u32::MAX as u64, default.clone())); + if self.data.len() <= i2 as usize { + self.data + .resize(i2 as usize + 1, (u32::MAX, default.clone())); } - let (left, right) = self.data.split_at_mut(i2); - (&mut left[i1], &mut right[0]) + let (left, right) = self.data.split_at_mut(i2 as usize); + (&mut left[i1 as usize], &mut right[0]) }; if elt1.0 != g1 { diff --git a/src/data/component_set.rs b/src/data/component_set.rs new file mode 100644 index 0000000..76e076a --- /dev/null +++ b/src/data/component_set.rs @@ -0,0 +1,106 @@ +use crate::data::Index; + +// TODO ECS: use this to handle optional components properly. +// pub trait OptionalComponentSet { +// fn get(&self, handle: Index) -> Option<&T>; +// } + +pub trait ComponentSetOption { + fn get(&self, handle: Index) -> Option<&T>; +} + +pub trait ComponentSet: ComponentSetOption { + fn size_hint(&self) -> usize; + // TODO ECS: remove this, its only needed by the query pipeline update + // which should only take the modified colliders into account. + fn for_each(&self, f: impl FnMut(Index, &T)); + fn index(&self, handle: Index) -> &T { + self.get(handle).unwrap() + } +} + +pub trait ComponentSetMut: ComponentSet { + fn map_mut_internal( + &mut self, + handle: crate::data::Index, + f: impl FnOnce(&mut T) -> Result, + ) -> Option; + fn set_internal(&mut self, handle: crate::data::Index, val: T); +} + +pub trait BundleSet<'a, T> { + fn index_bundle(&'a self, handle: Index) -> T; +} + +impl<'a, T, A, B> BundleSet<'a, (&'a A, &'a B)> for T +where + T: ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B) { + (self.index(handle), self.index(handle)) + } +} + +impl<'a, T, A, B, C> BundleSet<'a, (&'a A, &'a B, &'a C)> for T +where + T: ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C) { + (self.index(handle), self.index(handle), self.index(handle)) + } +} + +impl<'a, T, A, B, C, D> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D)> for T +where + T: ComponentSet + ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} + +impl<'a, T, A, B, C, D, E> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E)> for T +where + T: ComponentSet + ComponentSet + ComponentSet + ComponentSet + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} + +impl<'a, T, A, B, C, D, E, F> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F)> for T +where + T: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, +{ + #[inline(always)] + fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F) { + ( + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + self.index(handle), + ) + } +} diff --git a/src/data/mod.rs b/src/data/mod.rs index 7c49314..f20e433 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -1,8 +1,11 @@ //! Data structures modified with guaranteed deterministic behavior after deserialization. +pub use self::arena::{Arena, Index}; pub use self::coarena::Coarena; +pub use self::component_set::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; pub mod arena; mod coarena; +mod component_set; pub(crate) mod graph; pub mod pubsub; diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index ff463c7..6afd860 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,6 +1,12 @@ use super::TOIEntry; -use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ColliderSet, IntersectionEvent, NarrowPhase}; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces}; +use crate::dynamics::{ + RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, +}; +use crate::geometry::{ + ColliderParent, ColliderPosition, ColliderShape, ColliderType, IntersectionEvent, NarrowPhase, +}; use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; @@ -44,19 +50,34 @@ impl CCDSolver { /// Apply motion-clamping to the bodies affected by the given `impacts`. /// /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`. - pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) { + pub fn clamp_motions(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts) + where + Bodies: ComponentSet + + ComponentSetMut + + ComponentSet + + ComponentSet, + { match impacts { PredictedImpacts::Impacts(tois) => { // println!("Num to clamp: {}", tois.len()); for (handle, toi) in tois { - if let Some(body) = bodies.get_mut_internal(*handle) { - let min_toi = (body.ccd_thickness - * 0.15 - * crate::utils::inv(body.max_point_velocity())) - .min(dt); - // println!("Min toi: {}, Toi: {}", min_toi, toi); - body.integrate_next_position(toi.max(min_toi)); - } + let (rb_poss, vels, ccd, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyCcd, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + let local_com = &mprops.mass_properties.local_com; + + let min_toi = (ccd.ccd_thickness + * 0.15 + * crate::utils::inv(ccd.max_point_velocity(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; + }); } } _ => {} @@ -66,34 +87,64 @@ impl CCDSolver { /// Updates the set of bodies that needs CCD to be resolved. /// /// Returns `true` if any rigid-body must have CCD resolved. - pub fn update_ccd_active_flags( + pub fn update_ccd_active_flags( &self, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, dt: Real, include_forces: bool, - ) -> bool { + ) -> bool + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSet, + { let mut ccd_active = false; // println!("Checking CCD activation"); - bodies.foreach_active_dynamic_body_mut_internal(|_, body| { - body.update_ccd_active_flag(dt, include_forces); - // println!("CCD is active: {}, for {:?}", ccd_active, handle); - ccd_active = ccd_active || body.is_ccd_active(); - }); + 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; + }); + + ccd_active = ccd_active || moving_fast; + } + } ccd_active } /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider. - pub fn find_first_impact( + pub fn find_first_impact( &mut self, dt: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, narrow_phase: &NarrowPhase, - ) -> Option { + ) -> Option + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithPredictedPosition { dt }, @@ -102,19 +153,37 @@ impl CCDSolver { let mut pairs_seen = HashMap::default(); let mut min_toi = dt; - for (_, rb1) in bodies.iter_active_dynamic() { - if rb1.is_ccd_active() { - let predicted_body_pos1 = rb1.predict_position_using_velocity_and_forces(dt); + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); - for ch1 in &rb1.colliders { - let co1 = &colliders[*ch1]; + 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 co1.is_sensor() { + let predicted_body_pos1 = + rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1); + + for ch1 in &rb_colliders1.0 { + let co_parent1: &ColliderParent = colliders + .get(ch1.0) + .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() { continue; // Ignore sensors. } - let aabb1 = - co1.compute_swept_aabb(&(predicted_body_pos1 * co1.position_wrt_parent())); + let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; + let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -130,12 +199,17 @@ impl CCDSolver { ) .is_none() { - let c1 = colliders.get(*ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); + let co_type1: &ColliderType = colliders.index(ch1.0); + let co_type2: &ColliderType = colliders.index(ch1.0); - if bh1 == bh2 || (c1.is_sensor() || c2.is_sensor()) { + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); + + if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) { // Ignore self-intersection and sensors. return true; } @@ -146,16 +220,15 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); + let b2 = bh2.map(|h| bodies.index_bundle(h.0)); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - c1, - c2, - b1, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), + Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), b2, None, None, @@ -181,14 +254,27 @@ impl CCDSolver { } /// Outputs the set of bodies as well as their first time-of-impact event. - pub fn predict_impacts_at_next_positions( + pub fn predict_impacts_at_next_positions( &mut self, dt: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, narrow_phase: &NarrowPhase, events: &dyn EventHandler, - ) -> PredictedImpacts { + ) -> PredictedImpacts + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { let mut frozen = HashMap::<_, Real>::default(); let mut all_toi = BinaryHeap::new(); let mut pairs_seen = HashMap::default(); @@ -196,6 +282,7 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( + islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, @@ -207,71 +294,94 @@ impl CCDSolver { * */ // TODO: don't iterate through all the colliders. - for (ch1, co1) in colliders.iter() { - let rb1 = &bodies[co1.parent()]; - if rb1.is_ccd_active() { - let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + for handle in islands.active_dynamic_bodies() { + let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); - self.query_pipeline - .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - if ch1 == *ch2 { - // Ignore self-intersection. - return true; - } + 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 pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) - .is_none() - { - let c1 = colliders.get(ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + let predicted_body_pos1 = + rb_pos1.integrate_force_and_velocity(dt, forces1, rb_vels1, rb_mprops1); - if bh1 == bh2 { + for ch1 in &rb_colliders1.0 { + let co_parent1: &ColliderParent = colliders + .get(ch1.0) + .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); + + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { + if *ch1 == *ch2 { // Ignore self-intersection. return true; } - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); + if pairs_seen + .insert( + SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), + (), + ) + .is_none() + { + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); - let smallest_dist = narrow_phase - .contact_pair(ch1, *ch2) - .and_then(|p| p.find_deepest_contact()) - .map(|c| c.1.dist) - .unwrap_or(0.0); + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); - if let Some(toi) = TOIEntry::try_from_colliders( - self.query_pipeline.query_dispatcher(), - ch1, - *ch2, - c1, - c2, - b1, - b2, - None, - None, - 0.0, - // NOTE: we use dt here only once we know that - // there is at least one TOI before dt. - min_overstep, - smallest_dist, - ) { - if toi.toi > dt { - min_overstep = min_overstep.min(toi.toi); - } else { - min_overstep = dt; - all_toi.push(toi); + if bh1 == bh2 { + // Ignore self-intersection. + return true; + } + + let smallest_dist = narrow_phase + .contact_pair(*ch1, *ch2) + .and_then(|p| p.find_deepest_contact()) + .map(|c| c.1.dist) + .unwrap_or(0.0); + + let b2 = bh2.map(|h| bodies.index_bundle(h.0)); + + if let Some(toi) = TOIEntry::try_from_colliders( + self.query_pipeline.query_dispatcher(), + *ch1, + *ch2, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), + Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), + b2, + None, + None, + 0.0, + // NOTE: we use dt here only once we know that + // there is at least one TOI before dt. + min_overstep, + smallest_dist, + ) { + if toi.toi > dt { + min_overstep = min_overstep.min(toi.toi); + } else { + min_overstep = dt; + all_toi.push(toi); + } } } - } - true - }); + true + }); + } } } @@ -293,19 +403,25 @@ impl CCDSolver { while let Some(toi) = all_toi.pop() { assert!(toi.toi <= dt); - let body1 = bodies.get(toi.b1).unwrap(); - let body2 = bodies.get(toi.b2).unwrap(); + 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 mut colliders_to_check = Vec::new(); - let should_freeze1 = body1.is_ccd_active() && !frozen.contains_key(&toi.b1); - let should_freeze2 = body2.is_ccd_active() && !frozen.contains_key(&toi.b2); + let should_freeze1 = rb1.is_some() + && rb1.unwrap().0.ccd_active + && !frozen.contains_key(&toi.b1.unwrap()); + let should_freeze2 = rb2.is_some() + && rb2.unwrap().0.ccd_active + && !frozen.contains_key(&toi.b2.unwrap()); if !should_freeze1 && !should_freeze2 { continue; } if toi.is_intersection_test { - // NOTE: this test is rendundant with the previous `if !should_freeze && ...` + // NOTE: this test is redundant with the previous `if !should_freeze && ...` // but let's keep it to avoid tricky regressions if we end up swapping both // `if` for some reasons in the future. if should_freeze1 || should_freeze2 { @@ -318,42 +434,51 @@ impl CCDSolver { } if should_freeze1 { - let _ = frozen.insert(toi.b1, toi.toi); - colliders_to_check.extend_from_slice(&body1.colliders); + let _ = frozen.insert(toi.b1.unwrap(), toi.toi); + colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0); } if should_freeze2 { - let _ = frozen.insert(toi.b2, toi.toi); - colliders_to_check.extend_from_slice(&body2.colliders); + let _ = frozen.insert(toi.b2.unwrap(), toi.toi); + colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0); } let start_time = toi.toi; for ch1 in &colliders_to_check { - let co1 = &colliders[*ch1]; - let rb1 = &bodies[co1.parent]; - let aabb = co1.compute_swept_aabb(&(rb1.next_position * co1.position_wrt_parent())); + let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap(); + let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = + colliders.index_bundle(ch1.0); + + 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); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - let c1 = colliders.get(*ch1).unwrap(); - let c2 = colliders.get(*ch2).unwrap(); - let bh1 = c1.parent(); - let bh2 = c2.parent(); + let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); + let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); + let c1: (_, _, _) = colliders.index_bundle(ch1.0); + let c2: (_, _, _) = colliders.index_bundle(ch2.0); + + let bh1 = co_parent1.map(|p| p.handle); + let bh2 = co_parent2.map(|p| p.handle); if bh1 == bh2 { // Ignore self-intersection. return true; } - let frozen1 = frozen.get(&bh1); - let frozen2 = frozen.get(&bh2); + let frozen1 = bh1.and_then(|h| frozen.get(&h)); + let frozen2 = bh2.and_then(|h| frozen.get(&h)); - let b1 = bodies.get(bh1).unwrap(); - let b2 = bodies.get(bh2).unwrap(); + let b1: Option<(_, _, _, &RigidBodyCcd)> = + bh1.map(|h| bodies.index_bundle(h.0)); + let b2: Option<(_, _, _, &RigidBodyCcd)> = + bh1.map(|h| bodies.index_bundle(h.0)); - if (frozen1.is_some() || !b1.is_ccd_active()) - && (frozen2.is_some() || !b2.is_ccd_active()) + 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)) { // We already did a resweep. return true; @@ -369,8 +494,8 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - c1, - c2, + (c1.0, c1.1, c1.2, co_parent1), + (c2.0, c2.1, c2.2, co_parent2), b1, b2, frozen1.copied(), @@ -395,30 +520,57 @@ 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 body1 = &bodies[toi.b1]; - let body2 = &bodies[toi.b2]; - let co1 = &colliders[toi.c1]; - let co2 = &colliders[toi.c2]; - let frozen1 = frozen.get(&toi.b1); - let frozen2 = frozen.get(&toi.b2); - let pos1 = frozen1 - .map(|t| body1.integrate_velocity(*t)) - .unwrap_or(body1.next_position); - let pos2 = frozen2 - .map(|t| body2.integrate_velocity(*t)) - .unwrap_or(body2.next_position); + let (co_pos1, co_shape1): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(toi.c1.0); + let (co_pos2, co_shape2): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(toi.c2.0); - let prev_coll_pos12 = co1.position.inv_mul(&co2.position); - let next_coll_pos12 = - (pos1 * co1.position_wrt_parent()).inverse() * (pos2 * co2.position_wrt_parent()); + 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.mass_properties.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); + pos1 * co_parent1.pos_wrt_parent + } else { + co_pos1.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.mass_properties.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); + pos2 * co_parent2.pos_wrt_parent + } else { + co_pos2.0 + }; + + let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2); + 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, co1.shape(), co2.shape()) + .intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) .unwrap_or(false); let intersect_after = query_dispatcher - .intersection_test(&next_coll_pos12, co1.shape(), co2.shape()) + .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) .unwrap_or(false); if !intersect_before && !intersect_after { diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index f1066e0..4637940 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,5 +1,9 @@ -use crate::dynamics::{RigidBody, RigidBodyHandle}; -use crate::geometry::{Collider, ColliderHandle}; +use crate::dynamics::{ + RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, +}; +use crate::geometry::{ + ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, +}; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -7,9 +11,9 @@ use parry::query::{NonlinearRigidMotion, QueryDispatcher}; pub struct TOIEntry { pub toi: Real, pub c1: ColliderHandle, - pub b1: RigidBodyHandle, + pub b1: Option, pub c2: ColliderHandle, - pub b2: RigidBodyHandle, + pub b2: Option, pub is_intersection_test: bool, pub timestamp: usize, } @@ -18,9 +22,9 @@ impl TOIEntry { fn new( toi: Real, c1: ColliderHandle, - b1: RigidBodyHandle, + b1: Option, c2: ColliderHandle, - b2: RigidBodyHandle, + b2: Option, is_intersection_test: bool, timestamp: usize, ) -> Self { @@ -39,10 +43,30 @@ impl TOIEntry { query_dispatcher: &QD, ch1: ColliderHandle, ch2: ColliderHandle, - c1: &Collider, - c2: &Collider, - b1: &RigidBody, - b2: &RigidBody, + c1: ( + &ColliderType, + &ColliderShape, + &ColliderPosition, + Option<&ColliderParent>, + ), + c2: ( + &ColliderType, + &ColliderShape, + &ColliderPosition, + Option<&ColliderParent>, + ), + b1: Option<( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyCcd, + )>, + b2: Option<( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyCcd, + )>, frozen1: Option, frozen2: Option, start_time: Real, @@ -50,35 +74,46 @@ impl TOIEntry { smallest_contact_dist: Real, ) -> Option { assert!(start_time <= end_time); + if b1.is_none() && b2.is_none() { + return None; + } - let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel(); - let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel(); - let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel(); - let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel(); + let (co_type1, co_shape1, co_pos1, co_parent1) = c1; + let (co_type2, co_shape2, co_pos2, co_parent2) = c2; + + let linvel1 = + frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero()); + let linvel2 = + frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero()); + let angvel1 = + frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero()); + let angvel2 = + frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero()); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.abs() * b1.ccd_max_dist - + angvel2.abs() * b2.ccd_max_dist; + + 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); #[cfg(feature = "dim3")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.norm() * b1.ccd_max_dist - + angvel2.norm() * b2.ccd_max_dist; + + 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); // 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 = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()) + let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness()) + smallest_contact_dist.max(0.0); - let is_intersection_test = c1.is_sensor() || c2.is_sensor(); + let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor(); if (end_time - start_time) * vel12 < thickness { return None; } // Compute the TOI. - let mut motion1 = Self::body_motion(b1); - let mut motion2 = Self::body_motion(b2); + 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); if let Some(t) = frozen1 { motion1.freeze(t); @@ -88,8 +123,8 @@ impl TOIEntry { motion2.freeze(t); } - let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); - let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); + 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)); // println!("start_time: {}", start_time); @@ -105,9 +140,9 @@ impl TOIEntry { let res_toi = query_dispatcher .nonlinear_time_of_impact( &motion_c1, - c1.shape(), + co_shape1.as_ref(), &motion_c2, - c2.shape(), + co_shape2.as_ref(), start_time, end_time, stop_at_penetration, @@ -119,24 +154,31 @@ impl TOIEntry { Some(Self::new( toi.toi, ch1, - c1.parent(), + co_parent1.map(|p| p.handle), ch2, - c2.parent(), + co_parent2.map(|p| p.handle), is_intersection_test, 0, )) } - fn body_motion(body: &RigidBody) -> NonlinearRigidMotion { - if body.is_ccd_active() { + fn body_motion( + (poss, vels, mprops, ccd): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyCcd, + ), + ) -> NonlinearRigidMotion { + if ccd.ccd_active { NonlinearRigidMotion::new( - body.position, - body.mass_properties.local_com, - body.linvel, - body.angvel, + poss.position, + mprops.mass_properties.local_com, + vels.linvel, + vels.angvel, ) } else { - NonlinearRigidMotion::constant_position(body.next_position) + NonlinearRigidMotion::constant_position(poss.next_position) } } } diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 9b3b9ee..1016d09 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -20,17 +20,13 @@ pub enum CoefficientCombineRule { Max, } -impl CoefficientCombineRule { - pub(crate) fn from_value(val: u8) -> Self { - match val { - 0 => CoefficientCombineRule::Average, - 1 => CoefficientCombineRule::Min, - 2 => CoefficientCombineRule::Multiply, - 3 => CoefficientCombineRule::Max, - _ => panic!("Invalid coefficient combine rule."), - } +impl Default for CoefficientCombineRule { + fn default() -> Self { + CoefficientCombineRule::Average } +} +impl CoefficientCombineRule { pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real { let effective_rule = rule_value1.max(rule_value2); diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs new file mode 100644 index 0000000..c29609f --- /dev/null +++ b/src/dynamics/island_manager.rs @@ -0,0 +1,344 @@ +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{ + Joint, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, RigidBodyIds, RigidBodyType, + RigidBodyVelocity, +}; +use crate::geometry::{ColliderParent, InteractionGraph, NarrowPhase}; +use crate::math::Real; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct IslandManager { + pub(crate) active_dynamic_set: Vec, + pub(crate) active_kinematic_set: Vec, + pub(crate) active_islands: Vec, + active_set_timestamp: u32, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + can_sleep: Vec, // Workspace. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + stack: Vec, // Workspace. +} + +impl IslandManager { + pub fn new() -> Self { + Self { + active_dynamic_set: vec![], + active_kinematic_set: vec![], + active_islands: vec![], + active_set_timestamp: 0, + can_sleep: vec![], + stack: vec![], + } + } + + pub(crate) fn num_islands(&self) -> usize { + self.active_islands.len() - 1 + } + + pub fn cleanup_removed_rigid_bodies( + &mut self, + bodies: &mut impl ComponentSetMut, + ) { + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + + for active_set in &mut active_sets { + let mut i = 0; + + while i < active_set.len() { + let handle = active_set[i]; + if bodies.get(handle.0).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); + } + } else { + i += 1; + } + } + } + } + + pub fn rigid_body_removed( + &mut self, + removed_handle: RigidBodyHandle, + removed_ids: &RigidBodyIds, + bodies: &mut impl ComponentSetMut, + ) { + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + + for active_set in &mut active_sets { + if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { + active_set.swap_remove(removed_ids.active_set_id); + + if let Some(replacement) = active_set.get(removed_ids.active_set_id) { + bodies.map_mut_internal(replacement.0, |ids| { + ids.active_set_id = removed_ids.active_set_id; + }); + } + } + } + } + + /// Forces the specified rigid-body to wake up if it is dynamic. + /// + /// If `strong` is `true` then it is assured that the rigid-body will + /// remain awake during multiple subsequent timesteps. + pub fn wake_up(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool) + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + { + // TODO: what about kinematic bodies? + let status: RigidBodyType = *bodies.index(handle.0); + if status.is_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); + } + }); + } + } + + /// Iter through all the active kinematic rigid-bodies on this set. + pub fn active_kinematic_bodies(&self) -> &[RigidBodyHandle] { + &self.active_kinematic_set[..] + } + + /// Iter through all the active dynamic rigid-bodies on this set. + pub fn active_dynamic_bodies(&self) -> &[RigidBodyHandle] { + &self.active_dynamic_set[..] + } + + #[cfg(not(feature = "parallel"))] + pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { + let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + &self.active_dynamic_set[island_range] + } + + #[inline(always)] + pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator + 'a { + self.active_dynamic_set + .iter() + .copied() + .chain(self.active_kinematic_set.iter().copied()) + } + + /* + #[cfg(feature = "parallel")] + #[inline(always)] + #[allow(dead_code)] + pub(crate) fn foreach_active_island_body_mut_internal_parallel( + &self, + island_id: usize, + bodies: &mut Set, + f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync, + ) where + Set: ComponentSet, + { + use std::sync::atomic::Ordering; + + let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + let bodies = std::sync::atomic::AtomicPtr::new(&mut bodies as *mut _); + self.active_dynamic_set[island_range] + .par_iter() + .for_each_init( + || bodies.load(Ordering::Relaxed), + |bodies, handle| { + let bodies: &mut Set = unsafe { std::mem::transmute(*bodies) }; + if let Some(rb) = bodies.get_mut_internal(handle.0) { + f(*handle, rb) + } + }, + ); + } + */ + + #[cfg(feature = "parallel")] + pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { + self.active_islands[island_id]..self.active_islands[island_id + 1] + } + + pub(crate) fn update_active_set_with_contacts( + &mut self, + bodies: &mut Bodies, + colliders: &Colliders, + narrow_phase: &NarrowPhase, + joint_graph: &InteractionGraph, + min_island_size: usize, + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption, + { + assert!( + min_island_size > 0, + "The minimum island size must be at least 1." + ); + + // Update the energy of every rigid body and + // keep only those that may not sleep. + // let t = instant::now(); + self.active_set_timestamp += 1; + self.stack.clear(); + self.can_sleep.clear(); + + // NOTE: the `.rev()` is here so that two successive timesteps preserve + // the order of the bodies in the `active_dynamic_set` vec. This reversal + // does not seem to affect performances nor stability. However it makes + // debugging slightly nicer so we keep this rev. + for h in self.active_dynamic_set.drain(..).rev() { + let can_sleep = &mut self.can_sleep; + let stack = &mut self.stack; + + let vels: &RigidBodyVelocity = bodies.index(h.0); + let pseudo_kinetic_energy = vels.pseudo_kinetic_energy(); + + bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { + update_energy(activation, pseudo_kinetic_energy); + + if activation.energy <= activation.threshold { + // 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); + } + }); + } + + // Read all the contacts and push objects touching touching this rigid-body. + #[inline(always)] + fn push_contacting_bodies( + rb_colliders: &RigidBodyColliders, + colliders: &impl ComponentSetOption, + narrow_phase: &NarrowPhase, + stack: &mut Vec, + ) { + for collider_handle in &rb_colliders.0 { + if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { + for inter in contacts { + for manifold in &inter.2.manifolds { + if !manifold.data.solver_contacts.is_empty() { + let other = crate::utils::select_other( + (inter.0, inter.1), + *collider_handle, + ); + if let Some(other_body) = colliders.get(other.0) { + stack.push(other_body.handle); + } + break; + } + } + } + } + } + } + + // 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); + + if 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); + } + + // println!("Selection: {}", instant::now() - t); + + // let t = instant::now(); + // Propagation of awake state and awake island computation through the + // traversal of the interaction graph. + self.active_islands.clear(); + self.active_islands.push(0); + + // The max avoid underflow when the stack is empty. + 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); + + if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() { + // We already visited this body and its neighbors. + // Also, we don't propagate awake state through static bodies. + continue; + } + + if self.stack.len() < island_marker { + if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + >= min_island_size + { + // We are starting a new island. + self.active_islands.push(self.active_dynamic_set.len()); + } + + island_marker = self.stack.len(); + } + + // 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); + + for inter in joint_graph.interactions_with(rb_ids.joint_graph_index) { + let other = crate::utils::select_other((inter.0, inter.1), handle); + 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; + }); + + self.active_dynamic_set.push(handle); + } + + self.active_islands.push(self.active_dynamic_set.len()); + // println!( + // "Extraction: {}, num islands: {}", + // instant::now() - t, + // self.active_islands.len() - 1 + // ); + + // 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() + }); + } + } + } +} + +fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) { + let mix_factor = 0.01; + let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy; + activation.energy = new_energy.min(activation.threshold.abs() * 4.0); +} diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 2e4b394..8e4d9b8 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -2,31 +2,33 @@ use super::Joint; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; use crate::data::arena::Arena; -use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; +use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; +use crate::dynamics::{JointParams, RigidBodyHandle}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[repr(transparent)] -pub struct JointHandle(pub(crate) crate::data::arena::Index); +pub struct JointHandle(pub crate::data::arena::Index); impl JointHandle { /// Converts this handle into its (index, generation) components. - pub fn into_raw_parts(self) -> (usize, u64) { + pub fn into_raw_parts(self) -> (u32, u32) { self.0.into_raw_parts() } /// Reconstructs an handle from its (index, generation) components. - pub fn from_raw_parts(id: usize, generation: u64) -> Self { + pub fn from_raw_parts(id: u32, generation: u32) -> Self { Self(crate::data::arena::Index::from_raw_parts(id, generation)) } /// An always-invalid joint handle. pub fn invalid() -> Self { Self(crate::data::arena::Index::from_raw_parts( - crate::INVALID_USIZE, - crate::INVALID_U64, + crate::INVALID_U32, + crate::INVALID_U32, )) } } @@ -157,7 +159,7 @@ impl JointSet { /// Inserts a new joint into this set and retrieve its handle. pub fn insert( &mut self, - bodies: &mut RigidBodySet, + bodies: &mut impl ComponentSetMut, body1: RigidBodyHandle, body2: RigidBodyHandle, joint_params: J, @@ -177,57 +179,64 @@ impl JointSet { params: joint_params.into(), }; - let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2); - let (rb1, rb2) = ( - rb1.expect("Attempt to attach a joint to a non-existing body."), - rb2.expect("Attempt to attach a joint to a non-existing body."), - ); + let mut graph_index1 = bodies.index(joint.body1.0).joint_graph_index; + let mut graph_index2 = bodies.index(joint.body2.0).joint_graph_index; // NOTE: the body won't have a graph index if it does not // have any joint attached. - if !InteractionGraph::::is_graph_index_valid(rb1.joint_graph_index) - { - rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1); + if !InteractionGraph::::is_graph_index_valid(graph_index1) { + graph_index1 = self.joint_graph.graph.add_node(joint.body1); + bodies.map_mut_internal(joint.body1.0, |ids| ids.joint_graph_index = graph_index1); } - if !InteractionGraph::::is_graph_index_valid(rb2.joint_graph_index) - { - rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2); + if !InteractionGraph::::is_graph_index_valid(graph_index2) { + graph_index2 = self.joint_graph.graph.add_node(joint.body2); + bodies.map_mut_internal(joint.body2.0, |ids| ids.joint_graph_index = graph_index2); } - let id = self - .joint_graph - .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint); - - self.joint_ids[handle] = id; + self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint); JointHandle(handle) } /// Retrieve all the joints happening between two active bodies. // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. - pub(crate) fn select_active_interactions( + pub(crate) fn select_active_interactions( &self, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, out: &mut Vec>, - ) { - for out_island in &mut out[..bodies.num_islands()] { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { + for out_island in &mut out[..islands.num_islands()] { out_island.clear(); } // 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]; - if (rb1.is_dynamic() || rb2.is_dynamic()) - && (!rb1.is_dynamic() || !rb1.is_sleeping()) - && (!rb2.is_dynamic() || !rb2.is_sleeping()) + 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) { - let island_index = if !rb1.is_dynamic() { - rb2.active_island_id + let island_index = if !status1.is_dynamic() { + ids2.active_island_id } else { - rb1.active_island_id + ids1.active_island_id }; out[island_index].push(i); @@ -239,22 +248,28 @@ impl JointSet { /// /// If `wake_up` is set to `true`, then the bodies attached to this joint will be /// automatically woken up. - pub fn remove( + pub fn remove( &mut self, handle: JointHandle, - bodies: &mut RigidBodySet, + islands: &mut IslandManager, + bodies: &mut Bodies, wake_up: bool, - ) -> Option { + ) -> Option + where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + { let id = self.joint_ids.remove(handle.0)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?; if wake_up { // Wake-up the bodies attached to this joint. if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) { - bodies.wake_up(*rb_handle, true); + islands.wake_up(bodies, *rb_handle, true); } if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) { - bodies.wake_up(*rb_handle, true); + islands.wake_up(bodies, *rb_handle, true); } } @@ -267,11 +282,16 @@ impl JointSet { removed_joint } - pub(crate) fn remove_rigid_body( + pub(crate) fn remove_rigid_body( &mut self, deleted_id: RigidBodyGraphIndex, - bodies: &mut RigidBodySet, - ) { + islands: &mut IslandManager, + bodies: &mut Bodies, + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + { if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) { // We have to delete each joint one by one in order to: // - Wake-up the attached bodies. @@ -292,16 +312,16 @@ impl JointSet { } // Wake up the attached bodies. - bodies.wake_up(h1, true); - bodies.wake_up(h2, true); + islands.wake_up(bodies, h1, true); + islands.wake_up(bodies, h2, true); } if let Some(other) = self.joint_graph.remove_node(deleted_id) { // One rigid-body joint graph index may have been invalidated // so we need to update it. - if let Some(replacement) = bodies.get_mut_internal(other) { - replacement.joint_graph_index = deleted_id; - } + bodies.map_mut_internal(other.0, |ids: &mut RigidBodyIds| { + ids.joint_graph_index = deleted_id; + }); } } } diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 53d1b5b..825fa49 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -3,6 +3,7 @@ pub use self::ccd::CCDSolver; pub use self::coefficient_combine_rule::CoefficientCombineRule; pub use self::integration_parameters::IntegrationParameters; +pub use self::island_manager::IslandManager; pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::joint::JointIndex; #[cfg(feature = "dim3")] @@ -17,19 +18,27 @@ pub use self::joint::{ PrismaticJoint, SpringModel, // GenericJoint }; -pub(crate) use self::rigid_body::RigidBodyChanges; -pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; -pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; +pub use self::rigid_body_components::*; #[cfg(not(feature = "parallel"))] pub(crate) use self::solver::IslandSolver; #[cfg(feature = "parallel")] pub(crate) use self::solver::ParallelIslandSolver; pub use parry::mass_properties::MassProperties; +#[cfg(feature = "default-sets")] +pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; +#[cfg(feature = "default-sets")] +pub use self::rigid_body_set::{BodyPair, RigidBodySet}; + mod ccd; mod coefficient_combine_rule; mod integration_parameters; +mod island_manager; mod joint; -mod rigid_body; -mod rigid_body_set; +mod rigid_body_components; mod solver; + +#[cfg(feature = "default-sets")] +mod rigid_body; +#[cfg(feature = "default-sets")] +mod rigid_body_set; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index ba39873..1bd1ba5 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1,193 +1,131 @@ -use crate::dynamics::MassProperties; +use crate::dynamics::{ + MassProperties, RigidBodyActivation, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, + RigidBodyDamping, RigidBodyDominance, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, + RigidBodyMassPropsFlags, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, +}; use crate::geometry::{ - Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex, + Collider, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition, + ColliderShape, }; -use crate::math::{ - AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, -}; -use crate::utils::{self, WAngularInertia, WCross, WDot}; +use crate::math::{AngVector, Isometry, Point, Real, Rotation, Translation, Vector}; +use crate::utils::{self, WAngularInertia, WCross}; use na::ComplexField; use num::Zero; -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The status of a body, governing the way it is affected by external forces. -pub enum BodyStatus { - /// A `BodyStatus::Dynamic` body can be affected by all external forces. - Dynamic, - /// A `BodyStatus::Static` body cannot be affected by external forces. - Static, - /// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled - /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies. - /// - /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body - /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be - /// modified by the user and is independent from any contact or joint it is involved in. - Kinematic, - // Semikinematic, // A kinematic that performs automatic CCD with the static environment to avoid traversing it? - // Disabled, -} - -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags affecting the behavior of the constraints solver for a given contact manifold. - pub(crate) struct RigidBodyFlags: u8 { - const TRANSLATION_LOCKED = 1 << 0; - const ROTATION_LOCKED_X = 1 << 1; - const ROTATION_LOCKED_Y = 1 << 2; - const ROTATION_LOCKED_Z = 1 << 3; - const CCD_ENABLED = 1 << 4; - const CCD_ACTIVE = 1 << 5; - } -} - -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags describing how the rigid-body has been modified by the user. - pub(crate) struct RigidBodyChanges: u32 { - const MODIFIED = 1 << 0; - const POSITION = 1 << 1; - const SLEEP = 1 << 2; - const COLLIDERS = 1 << 3; - const BODY_STATUS = 1 << 4; - } -} - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A rigid body. /// /// To create a new rigid-body, use the `RigidBodyBuilder` structure. #[derive(Debug, Clone)] pub struct RigidBody { - /// The world-space position of the rigid-body. - pub(crate) position: Isometry, - /// The next position of the rigid-body. - /// - /// At the beginning of the timestep, and when the - /// timestep is complete we must have position == next_position - /// except for kinematic bodies. - /// - /// The next_position is updated after the velocity and position - /// resolution. Then it is either validated (ie. we set position := set_position) - /// or clamped by CCD. - pub(crate) next_position: Isometry, - /// The local mass properties of the rigid-body. - pub(crate) mass_properties: MassProperties, - /// The world-space center of mass of the rigid-body. - pub world_com: Point, - /// The inverse mass taking into account translation locking. - pub effective_inv_mass: Real, - /// The square-root of the world-space inverse angular inertia tensor of the rigid-body, - /// taking into account rotation locking. - pub effective_world_inv_inertia_sqrt: AngularInertia, - /// The linear velocity of the rigid-body. - pub(crate) linvel: Vector, - /// The angular velocity of the rigid-body. - pub(crate) angvel: AngVector, - /// Damping factor for gradually slowing down the translational motion of the rigid-body. - pub linear_damping: Real, - /// Damping factor for gradually slowing down the angular motion of the rigid-body. - pub angular_damping: Real, - /// Accumulation of external forces (only for dynamic bodies). - pub(crate) force: Vector, - /// Accumulation of external torques (only for dynamic bodies). - pub(crate) torque: AngVector, - pub(crate) colliders: Vec, - pub(crate) gravity_scale: Real, + pub rb_pos: RigidBodyPosition, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_mprops: RigidBodyMassProps, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_vels: RigidBodyVelocity, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_damping: RigidBodyDamping, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_forces: RigidBodyForces, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_ccd: RigidBodyCcd, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_ids: RigidBodyIds, // TODO ECS: public only for initial tests with bevy_rapier. + pub rb_colliders: RigidBodyColliders, // TODO ECS: public only for initial tests with bevy_rapier. /// Whether or not this rigid-body is sleeping. - pub activation: ActivationStatus, - pub(crate) joint_graph_index: RigidBodyGraphIndex, - pub(crate) active_island_id: usize, - pub(crate) active_set_id: usize, - pub(crate) active_set_offset: usize, - pub(crate) active_set_timestamp: u32, - flags: RigidBodyFlags, - pub(crate) changes: RigidBodyChanges, + pub rb_activation: RigidBodyActivation, // TODO ECS: public only for initial tests with bevy_rapier. + pub changes: RigidBodyChanges, // TODO ECS: public only for initial tests with bevy_rapier. /// The status of the body, governing how it is affected by external forces. - body_status: BodyStatus, + pub rb_type: RigidBodyType, // TODO ECS: public only for initial tests with bevy_rapier /// The dominance group this rigid-body is part of. - dominance_group: i8, + pub rb_dominance: RigidBodyDominance, /// User-defined data associated to this rigid-body. pub user_data: u128, - pub(crate) ccd_thickness: Real, - pub(crate) ccd_max_dist: Real, } impl RigidBody { fn new() -> Self { Self { - position: Isometry::identity(), - next_position: Isometry::identity(), - mass_properties: MassProperties::zero(), - world_com: Point::origin(), - effective_inv_mass: 0.0, - effective_world_inv_inertia_sqrt: AngularInertia::zero(), - linvel: Vector::zeros(), - angvel: na::zero(), - force: Vector::zeros(), - torque: na::zero(), - gravity_scale: 1.0, - linear_damping: 0.0, - angular_damping: 0.0, - colliders: Vec::new(), - activation: ActivationStatus::new_active(), - joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(), - active_island_id: 0, - active_set_id: 0, - active_set_offset: 0, - active_set_timestamp: 0, - flags: RigidBodyFlags::empty(), + rb_pos: RigidBodyPosition::default(), + rb_mprops: RigidBodyMassProps::default(), + rb_vels: RigidBodyVelocity::default(), + rb_damping: RigidBodyDamping::default(), + rb_forces: RigidBodyForces::default(), + rb_ccd: RigidBodyCcd::default(), + rb_ids: RigidBodyIds::default(), + rb_colliders: RigidBodyColliders::default(), + rb_activation: RigidBodyActivation::new_active(), changes: RigidBodyChanges::all(), - body_status: BodyStatus::Dynamic, - dominance_group: 0, + rb_type: RigidBodyType::Dynamic, + rb_dominance: RigidBodyDominance::default(), user_data: 0, - ccd_thickness: Real::MAX, - ccd_max_dist: 0.0, } } pub(crate) fn reset_internal_references(&mut self) { - self.colliders = Vec::new(); - self.joint_graph_index = InteractionGraph::<(), ()>::invalid_graph_index(); - self.active_island_id = 0; - self.active_set_id = 0; - self.active_set_offset = 0; - self.active_set_timestamp = 0; + self.rb_colliders.0 = Vec::new(); + self.rb_ids = Default::default(); } - pub(crate) fn add_gravity(&mut self, gravity: Vector) { - if self.effective_inv_mass != 0.0 { - self.force += gravity * self.gravity_scale * self.mass(); - } + pub fn user_data(&self) -> u128 { + self.user_data } - #[cfg(not(feature = "parallel"))] // in parallel solver this is not needed - pub(crate) fn integrate_accelerations(&mut self, dt: Real) { - let linear_acc = self.force * self.effective_inv_mass; - let angular_acc = self.effective_world_inv_inertia_sqrt - * (self.effective_world_inv_inertia_sqrt * self.torque); + pub fn set_user_data(&mut self, data: u128) { + self.user_data = data; + } - self.linvel += linear_acc * dt; - self.angvel += angular_acc * dt; + #[inline] + pub fn rb_activation(&self) -> &RigidBodyActivation { + &self.rb_activation + } + + #[inline] + pub fn activation_mut(&mut self) -> &mut RigidBodyActivation { + &mut self.rb_activation + } + + #[inline] + pub(crate) fn changes(&self) -> RigidBodyChanges { + self.changes + } + + #[inline] + pub(crate) fn changes_mut_internal(&mut self) -> &mut RigidBodyChanges { + &mut self.changes + } + + #[inline] + pub fn linear_damping(&self) -> Real { + self.rb_damping.linear_damping + } + + #[inline] + pub fn set_linear_damping(&mut self, damping: Real) { + self.rb_damping.linear_damping = damping; + } + + #[inline] + pub fn angular_damping(&self) -> Real { + self.rb_damping.angular_damping + } + + #[inline] + pub fn set_angular_damping(&mut self, damping: Real) { + self.rb_damping.angular_damping = damping } /// The status of this rigid-body. - pub fn body_status(&self) -> BodyStatus { - self.body_status + pub fn rb_type(&self) -> RigidBodyType { + self.rb_type } /// Sets the status of this rigid-body. - pub fn set_body_status(&mut self, status: BodyStatus) { - if status != self.body_status { - self.changes.insert(RigidBodyChanges::BODY_STATUS); - self.body_status = status; + pub fn set_rb_type(&mut self, status: RigidBodyType) { + if status != self.rb_type { + self.changes.insert(RigidBodyChanges::TYPE); + self.rb_type = status; } } /// The mass properties of this rigid-body. #[inline] pub fn mass_properties(&self) -> &MassProperties { - &self.mass_properties + &self.rb_mprops.mass_properties } /// The dominance group of this rigid-body. @@ -196,42 +134,48 @@ impl RigidBody { /// rigid-bodies. #[inline] pub fn effective_dominance_group(&self) -> i16 { - if self.is_dynamic() { - self.dominance_group as i16 - } else { - i8::MAX as i16 + 1 - } + self.rb_dominance.effective_group(&self.rb_type) } /// Are the translations of this rigid-body locked? pub fn is_translation_locked(&self) -> bool { - self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) + self.rb_mprops + .flags + .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED) } /// Are the rotations of this rigid-body locked? #[cfg(feature = "dim2")] pub fn is_rotation_locked(&self) -> bool { - self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) + self.rb_mprops + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z) } /// Returns `true` for each rotational degrees of freedom locked on this rigid-body. #[cfg(feature = "dim3")] pub fn is_rotation_locked(&self) -> [bool; 3] { [ - self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X), - self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y), - self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z), + self.rb_mprops + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X), + self.rb_mprops + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y), + self.rb_mprops + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z), ] } /// Enables of disable CCD (continuous collision-detection) for this rigid-body. pub fn enable_ccd(&mut self, enabled: bool) { - self.flags.set(RigidBodyFlags::CCD_ENABLED, enabled) + self.rb_ccd.ccd_enabled = enabled; } /// Is CCD (continous collision-detection) enabled for this rigid-body? pub fn is_ccd_enabled(&self) -> bool { - self.flags.contains(RigidBodyFlags::CCD_ENABLED) + self.rb_ccd.ccd_enabled } // This is different from `is_ccd_enabled`. This checks that CCD @@ -246,47 +190,7 @@ impl RigidBody { /// checks if CCD is allowed to run for this rigid-body or if /// it is completely disabled (independently from its velocity). pub fn is_ccd_active(&self) -> bool { - self.flags.contains(RigidBodyFlags::CCD_ACTIVE) - } - - pub(crate) fn update_ccd_active_flag(&mut self, dt: Real, include_forces: bool) { - let ccd_active = self.is_ccd_enabled() && self.is_moving_fast(dt, include_forces); - self.flags.set(RigidBodyFlags::CCD_ACTIVE, ccd_active); - } - - pub(crate) fn is_moving_fast(&self, dt: Real, include_forces: bool) -> bool { - if self.is_dynamic() { - // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we - // should use `self.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist` - // is the deepest contact (the contact with the largest penetration depth, i.e., the - // negative `dist` with the largest absolute value. - // However, getting this penetration depth assumes querying the contact graph from - // the narrow-phase, which can be pretty expensive. So we use the CCD thickness - // divided by 10 right now. We will see in practice if this value is OK or if we - // should use a smaller (to be less conservative) or larger divisor (to be more conservative). - let threshold = self.ccd_thickness / 10.0; - - if include_forces { - let linear_part = (self.linvel + self.force * dt).norm(); - #[cfg(feature = "dim2")] - let angular_part = (self.angvel + self.torque * dt).abs() * self.ccd_max_dist; - #[cfg(feature = "dim3")] - let angular_part = (self.angvel + self.torque * dt).norm() * self.ccd_max_dist; - let vel_with_forces = linear_part + angular_part; - vel_with_forces > threshold - } else { - self.max_point_velocity() * dt > threshold - } - } else { - false - } - } - - pub(crate) fn max_point_velocity(&self) -> Real { - #[cfg(feature = "dim2")] - return self.linvel.norm() + self.angvel.abs() * self.ccd_max_dist; - #[cfg(feature = "dim3")] - return self.linvel.norm() + self.angvel.norm() * self.ccd_max_dist; + self.rb_ccd.ccd_active } /// Sets the rigid-body's initial mass properties. @@ -299,41 +203,41 @@ impl RigidBody { self.wake_up(true); } - self.mass_properties = props; + self.rb_mprops.mass_properties = props; self.update_world_mass_properties(); } /// The handles of colliders attached to this rigid body. pub fn colliders(&self) -> &[ColliderHandle] { - &self.colliders[..] + &self.rb_colliders.0[..] } /// Is this rigid body dynamic? /// /// A dynamic body can move freely and is affected by forces. pub fn is_dynamic(&self) -> bool { - self.body_status == BodyStatus::Dynamic + self.rb_type == RigidBodyType::Dynamic } /// Is this rigid body kinematic? /// /// A kinematic body can move freely but is not affected by forces. pub fn is_kinematic(&self) -> bool { - self.body_status == BodyStatus::Kinematic + self.rb_type == RigidBodyType::Kinematic } /// Is this rigid body static? /// /// A static body cannot move and is not affected by forces. pub fn is_static(&self) -> bool { - self.body_status == BodyStatus::Static + self.rb_type == RigidBodyType::Static } /// The mass of this rigid body. /// /// Returns zero if this rigid body has an infinite mass. pub fn mass(&self) -> Real { - utils::inv(self.mass_properties.inv_mass) + utils::inv(self.rb_mprops.mass_properties.inv_mass) } /// The predicted position of this rigid-body. @@ -342,69 +246,56 @@ impl RigidBody { /// method and is used for estimating the kinematic body velocity at the next timestep. /// For non-kinematic bodies, this value is currently unspecified. pub fn next_position(&self) -> &Isometry { - &self.next_position + &self.rb_pos.next_position } /// The scale factor applied to the gravity affecting this rigid-body. pub fn gravity_scale(&self) -> Real { - self.gravity_scale + self.rb_forces.gravity_scale } /// Sets the gravity scale facter for this rigid-body. pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) { - if wake_up && self.activation.sleeping { + if wake_up && self.rb_activation.sleeping { self.changes.insert(RigidBodyChanges::SLEEP); - self.activation.sleeping = false; + self.rb_activation.sleeping = false; } - self.gravity_scale = scale; + self.rb_forces.gravity_scale = scale; } /// Adds a collider to this rigid-body. - pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) { - self.changes.set( - RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS, - true, - ); - - self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness()); - - let shape_bsphere = coll - .shape() - .compute_bounding_sphere(coll.position_wrt_parent()); - self.ccd_max_dist = self - .ccd_max_dist - .max(shape_bsphere.center.coords.norm() + shape_bsphere.radius); - - let mass_properties = coll - .mass_properties() - .transform_by(coll.position_wrt_parent()); - self.colliders.push(handle); - self.mass_properties += mass_properties; - self.update_world_mass_properties(); - } - - pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) { - for handle in &self.colliders { - // NOTE: we use `get_mut_internal_with_modification_tracking` here because we want to - // benefit from the modification tracking to know the colliders - // we need to update the broad-phase and narrow-phase for. - let collider = colliders - .get_mut_internal_with_modification_tracking(*handle) - .unwrap(); - collider.set_position(self.position * collider.delta); - } + // TODO ECS: we keep this public for now just to simply our experiments on bevy_rapier. + pub fn add_collider( + &mut self, + co_handle: ColliderHandle, + co_parent: &ColliderParent, + co_pos: &mut ColliderPosition, + co_shape: &ColliderShape, + co_mprops: &ColliderMassProperties, + ) { + self.rb_colliders.attach_collider( + &mut self.changes, + &mut self.rb_ccd, + &mut self.rb_mprops, + &self.rb_pos, + co_handle, + co_pos, + co_parent, + co_shape, + co_mprops, + ) } /// Removes a collider from this rigid-body. pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { - if let Some(i) = self.colliders.iter().position(|e| *e == handle) { + if let Some(i) = self.rb_colliders.0.iter().position(|e| *e == handle) { self.changes.set(RigidBodyChanges::COLLIDERS, true); - self.colliders.swap_remove(i); + self.rb_colliders.0.swap_remove(i); let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent()); - self.mass_properties -= mass_properties; + self.rb_mprops.mass_properties -= mass_properties; self.update_world_mass_properties(); } } @@ -415,10 +306,8 @@ impl RigidBody { /// it is waken up. It can be woken manually with `self.wake_up` or automatically due to /// external forces like contacts. pub fn sleep(&mut self) { - self.activation.energy = 0.0; - self.activation.sleeping = true; - self.linvel = na::zero(); - self.angvel = na::zero(); + self.rb_activation.sleep(); + self.rb_vels = RigidBodyVelocity::zero(); } /// Wakes up this rigid body if it is sleeping. @@ -426,21 +315,11 @@ impl RigidBody { /// If `strong` is `true` then it is assured that the rigid-body will /// remain awake during multiple subsequent timesteps. pub fn wake_up(&mut self, strong: bool) { - if self.activation.sleeping { + if self.rb_activation.sleeping { self.changes.insert(RigidBodyChanges::SLEEP); - self.activation.sleeping = false; } - if (strong || self.activation.energy == 0.0) && self.is_dynamic() { - self.activation.energy = self.activation.threshold.abs() * 2.0; - } - } - - pub(crate) fn update_energy(&mut self) { - let mix_factor = 0.01; - let new_energy = (1.0 - mix_factor) * self.activation.energy - + mix_factor * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel)); - self.activation.energy = new_energy.min(self.activation.threshold.abs() * 4.0); + self.rb_activation.wake_up(strong); } /// Is this rigid body sleeping? @@ -449,60 +328,45 @@ impl RigidBody { // - return false for static bodies. // - return true for non-sleeping dynamic bodies. // - return true only for kinematic bodies with non-zero velocity? - self.activation.sleeping + self.rb_activation.sleeping } /// Is the velocity of this body not zero? pub fn is_moving(&self) -> bool { - !self.linvel.is_zero() || !self.angvel.is_zero() + !self.rb_vels.linvel.is_zero() || !self.rb_vels.angvel.is_zero() } /// Computes the predict position of this rigid-body after `dt` seconds, taking /// into account its velocities and external forces applied to it. pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry { - let dlinvel = self.force * (self.effective_inv_mass * dt); + let dlinvel = self.rb_forces.force * (self.rb_mprops.effective_inv_mass * dt); let dangvel = self + .rb_mprops .effective_world_inv_inertia_sqrt - .transform_vector(self.torque * dt); - let linvel = self.linvel + dlinvel; - let angvel = self.angvel + dangvel; + .transform_vector(self.rb_forces.torque * dt); + let linvel = self.rb_vels.linvel + dlinvel; + let angvel = self.rb_vels.angvel + dangvel; - let com = self.position * self.mass_properties.local_com; + let com = self.rb_pos.position * self.rb_mprops.mass_properties.local_com; let shift = Translation::from(com.coords); - shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.position - } - - pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry { - let com = self.position * self.mass_properties.local_com; - let shift = Translation::from(com.coords); - shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() - } - - pub(crate) fn apply_damping(&mut self, dt: Real) { - self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); - self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); - } - - pub(crate) fn integrate_next_position(&mut self, dt: Real) { - self.next_position = self.integrate_velocity(dt) * self.position; - let _ = self.next_position.rotation.renormalize_fast(); + shift * Isometry::new(linvel * dt, angvel * dt) * shift.inverse() * self.rb_pos.position } /// The linear velocity of this rigid-body. pub fn linvel(&self) -> &Vector { - &self.linvel + &self.rb_vels.linvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim2")] pub fn angvel(&self) -> Real { - self.angvel + self.rb_vels.angvel } /// The angular velocity of this rigid-body. #[cfg(feature = "dim3")] pub fn angvel(&self) -> &Vector { - &self.angvel + &self.rb_vels.angvel } /// The linear velocity of this rigid-body. @@ -510,7 +374,7 @@ impl RigidBody { /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { - self.linvel = linvel; + self.rb_vels.linvel = linvel; if self.is_dynamic() && wake_up { self.wake_up(true) @@ -523,7 +387,7 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[cfg(feature = "dim2")] pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { - self.angvel = angvel; + self.rb_vels.angvel = angvel; if self.is_dynamic() && wake_up { self.wake_up(true) @@ -536,7 +400,7 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[cfg(feature = "dim3")] pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { - self.angvel = angvel; + self.rb_vels.angvel = angvel; if self.is_dynamic() && wake_up { self.wake_up(true) @@ -544,8 +408,9 @@ impl RigidBody { } /// The world-space position of this rigid-body. + #[inline] pub fn position(&self) -> &Isometry { - &self.position + &self.rb_pos.position } /// Sets the position and `next_kinematic_position` of this rigid body. @@ -559,8 +424,8 @@ impl RigidBody { /// put to sleep because it did not move for a while. pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { self.changes.insert(RigidBodyChanges::POSITION); - self.position = pos; - self.next_position = pos; + self.rb_pos.position = pos; + self.rb_pos.next_position = pos; // TODO: Do we really need to check that the body isn't dynamic? if wake_up && self.is_dynamic() { @@ -568,67 +433,16 @@ impl RigidBody { } } - pub(crate) fn set_next_position(&mut self, pos: Isometry) { - self.next_position = pos; - } - /// If this rigid body is kinematic, sets its future position after the next timestep integration. pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { - self.next_position = pos; + self.rb_pos.next_position = pos; } } - pub(crate) fn compute_velocity_from_next_position(&mut self, inv_dt: Real) { - let dpos = self.next_position * self.position.inverse(); - #[cfg(feature = "dim2")] - { - self.angvel = dpos.rotation.angle() * inv_dt; - } - #[cfg(feature = "dim3")] - { - self.angvel = dpos.rotation.scaled_axis() * inv_dt; - } - self.linvel = dpos.translation.vector * inv_dt; - } - pub(crate) fn update_world_mass_properties(&mut self) { - self.world_com = self.mass_properties.world_com(&self.position); - self.effective_inv_mass = self.mass_properties.inv_mass; - self.effective_world_inv_inertia_sqrt = self - .mass_properties - .world_inv_inertia_sqrt(&self.position.rotation); - - // Take into account translation/rotation locking. - if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) { - self.effective_inv_mass = 0.0; - } - - #[cfg(feature = "dim2")] - { - if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) { - self.effective_world_inv_inertia_sqrt = 0.0; - } - } - #[cfg(feature = "dim3")] - { - if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) { - self.effective_world_inv_inertia_sqrt.m11 = 0.0; - self.effective_world_inv_inertia_sqrt.m12 = 0.0; - self.effective_world_inv_inertia_sqrt.m13 = 0.0; - } - - if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) { - self.effective_world_inv_inertia_sqrt.m22 = 0.0; - self.effective_world_inv_inertia_sqrt.m12 = 0.0; - self.effective_world_inv_inertia_sqrt.m23 = 0.0; - } - if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) { - self.effective_world_inv_inertia_sqrt.m33 = 0.0; - self.effective_world_inv_inertia_sqrt.m13 = 0.0; - self.effective_world_inv_inertia_sqrt.m23 = 0.0; - } - } + self.rb_mprops + .update_world_mass_properties(&self.rb_pos.position); } } @@ -638,8 +452,8 @@ impl RigidBody { /// The force will be applied in the next simulation step. /// This does nothing on non-dynamic bodies. pub fn apply_force(&mut self, force: Vector, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.force += force; + if self.rb_type == RigidBodyType::Dynamic { + self.rb_forces.force += force; if wake_up { self.wake_up(true); @@ -652,8 +466,8 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim2")] pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.torque += torque; + if self.rb_type == RigidBodyType::Dynamic { + self.rb_forces.torque += torque; if wake_up { self.wake_up(true); @@ -666,8 +480,8 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim3")] pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.torque += torque; + if self.rb_type == RigidBodyType::Dynamic { + self.rb_forces.torque += torque; if wake_up { self.wake_up(true); @@ -679,9 +493,9 @@ impl RigidBody { /// The force will be applied in the next simulation step. /// This does nothing on non-dynamic bodies. pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.force += force; - self.torque += (point - self.world_com).gcross(force); + if self.rb_type == RigidBodyType::Dynamic { + self.rb_forces.force += force; + self.rb_forces.torque += (point - self.rb_mprops.world_com).gcross(force); if wake_up { self.wake_up(true); @@ -696,8 +510,8 @@ impl RigidBody { /// The impulse is applied right away, changing the linear velocity. /// This does nothing on non-dynamic bodies. pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.linvel += impulse * self.effective_inv_mass; + if self.rb_type == RigidBodyType::Dynamic { + self.rb_vels.linvel += impulse * self.rb_mprops.effective_inv_mass; if wake_up { self.wake_up(true); @@ -710,9 +524,9 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim2")] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.angvel += self.effective_world_inv_inertia_sqrt - * (self.effective_world_inv_inertia_sqrt * torque_impulse); + if self.rb_type == RigidBodyType::Dynamic { + self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt + * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -725,9 +539,9 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim3")] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.angvel += self.effective_world_inv_inertia_sqrt - * (self.effective_world_inv_inertia_sqrt * torque_impulse); + if self.rb_type == RigidBodyType::Dynamic { + self.rb_vels.angvel += self.rb_mprops.effective_world_inv_inertia_sqrt + * (self.rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -744,7 +558,7 @@ impl RigidBody { point: Point, wake_up: bool, ) { - let torque_impulse = (point - self.world_com).gcross(impulse); + let torque_impulse = (point - self.rb_mprops.world_com).gcross(impulse); self.apply_impulse(impulse, wake_up); self.apply_torque_impulse(torque_impulse, wake_up); } @@ -753,24 +567,27 @@ impl RigidBody { impl RigidBody { /// The velocity of the given world-space point on this rigid-body. pub fn velocity_at_point(&self, point: &Point) -> Vector { - let dpt = point - self.world_com; - self.linvel + self.angvel.gcross(dpt) + let dpt = point - self.rb_mprops.world_com; + self.rb_vels.linvel + self.rb_vels.angvel.gcross(dpt) } /// The kinetic energy of this body. pub fn kinetic_energy(&self) -> Real { - let mut energy = (self.mass() * self.linvel().norm_squared()) / 2.0; + let mut energy = (self.mass() * self.rb_vels.linvel.norm_squared()) / 2.0; #[cfg(feature = "dim2")] - if !self.effective_world_inv_inertia_sqrt.is_zero() { - let inertia_sqrt = 1.0 / self.effective_world_inv_inertia_sqrt; - energy += (inertia_sqrt * self.angvel).powi(2) / 2.0; + if !self.rb_mprops.effective_world_inv_inertia_sqrt.is_zero() { + let inertia_sqrt = 1.0 / self.rb_mprops.effective_world_inv_inertia_sqrt; + energy += (inertia_sqrt * self.rb_vels.angvel).powi(2) / 2.0; } #[cfg(feature = "dim3")] - if !self.effective_world_inv_inertia_sqrt.is_zero() { - let inertia_sqrt = self.effective_world_inv_inertia_sqrt.inverse_unchecked(); - energy += (inertia_sqrt * self.angvel).norm_squared() / 2.0; + if !self.rb_mprops.effective_world_inv_inertia_sqrt.is_zero() { + let inertia_sqrt = self + .rb_mprops + .effective_world_inv_inertia_sqrt + .inverse_unchecked(); + energy += (inertia_sqrt * self.rb_vels.angvel).norm_squared() / 2.0; } energy @@ -778,13 +595,17 @@ impl RigidBody { /// The potential energy of this body in a gravity field. pub fn gravitational_potential_energy(&self, dt: Real, gravity: Vector) -> Real { - let world_com = self.mass_properties().world_com(&self.position).coords; + let world_com = self + .rb_mprops + .mass_properties + .world_com(&self.rb_pos.position) + .coords; // Project position back along velocity vector one half-step (leap-frog) // to sync up the potential energy with the kinetic energy: - let world_com = world_com - self.linvel() * (dt / 2.0); + let world_com = world_com - self.rb_vels.linvel * (dt / 2.0); - -self.mass() * self.gravity_scale() * gravity.dot(&world_com) + -self.mass() * self.rb_forces.gravity_scale * gravity.dot(&world_com) } } @@ -796,8 +617,8 @@ pub struct RigidBodyBuilder { gravity_scale: Real, linear_damping: Real, angular_damping: Real, - body_status: BodyStatus, - flags: RigidBodyFlags, + rb_type: RigidBodyType, + mprops_flags: RigidBodyMassPropsFlags, mass_properties: MassProperties, can_sleep: bool, sleeping: bool, @@ -808,7 +629,7 @@ pub struct RigidBodyBuilder { impl RigidBodyBuilder { /// Initialize a new builder for a rigid body which is either static, dynamic, or kinematic. - pub fn new(body_status: BodyStatus) -> Self { + pub fn new(rb_type: RigidBodyType) -> Self { Self { position: Isometry::identity(), linvel: Vector::zeros(), @@ -816,8 +637,8 @@ impl RigidBodyBuilder { gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, - body_status, - flags: RigidBodyFlags::empty(), + rb_type, + mprops_flags: RigidBodyMassPropsFlags::empty(), mass_properties: MassProperties::zero(), can_sleep: true, sleeping: false, @@ -829,17 +650,17 @@ impl RigidBodyBuilder { /// Initializes the builder of a new static rigid body. pub fn new_static() -> Self { - Self::new(BodyStatus::Static) + Self::new(RigidBodyType::Static) } /// Initializes the builder of a new kinematic rigid body. pub fn new_kinematic() -> Self { - Self::new(BodyStatus::Kinematic) + Self::new(RigidBodyType::Kinematic) } /// Initializes the builder of a new dynamic rigid body. pub fn new_dynamic() -> Self { - Self::new(BodyStatus::Dynamic) + Self::new(RigidBodyType::Dynamic) } /// Sets the scale applied to the gravity force affecting the rigid-body to be created. @@ -906,15 +727,19 @@ impl RigidBodyBuilder { /// Prevents this rigid-body from translating because of forces. pub fn lock_translations(mut self) -> Self { - self.flags.set(RigidBodyFlags::TRANSLATION_LOCKED, true); + self.mprops_flags + .set(RigidBodyMassPropsFlags::TRANSLATION_LOCKED, true); self } /// Prevents this rigid-body from rotating because of forces. pub fn lock_rotations(mut self) -> Self { - self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true); - self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true); - self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true); + self.mprops_flags + .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_X, true); + self.mprops_flags + .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, true); + self.mprops_flags + .set(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, true); self } @@ -926,12 +751,18 @@ impl RigidBodyBuilder { allow_rotations_y: bool, allow_rotations_z: bool, ) -> Self { - self.flags - .set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x); - self.flags - .set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y); - self.flags - .set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z); + self.mprops_flags.set( + RigidBodyMassPropsFlags::ROTATION_LOCKED_X, + !allow_rotations_x, + ); + self.mprops_flags.set( + RigidBodyMassPropsFlags::ROTATION_LOCKED_Y, + !allow_rotations_y, + ); + self.mprops_flags.set( + RigidBodyMassPropsFlags::ROTATION_LOCKED_Z, + !allow_rotations_z, + ); self } @@ -1063,21 +894,54 @@ impl RigidBodyBuilder { self } + pub fn components( + &self, + ) -> ( + RigidBodyPosition, + RigidBodyMassProps, + RigidBodyVelocity, + RigidBodyDamping, + RigidBodyForces, + RigidBodyCcd, + RigidBodyIds, + RigidBodyColliders, + RigidBodyActivation, + RigidBodyChanges, + RigidBodyType, + RigidBodyDominance, + ) { + let rb = self.build(); + ( + rb.rb_pos, + rb.rb_mprops, + rb.rb_vels, + rb.rb_damping, + rb.rb_forces, + rb.rb_ccd, + rb.rb_ids, + rb.rb_colliders, + rb.rb_activation, + rb.changes, + rb.rb_type, + rb.rb_dominance, + ) + } + /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); - rb.next_position = self.position; // FIXME: compute the correct value? - rb.position = self.position; - rb.linvel = self.linvel; - rb.angvel = self.angvel; - rb.body_status = self.body_status; + rb.rb_pos.next_position = self.position; // FIXME: compute the correct value? + rb.rb_pos.position = self.position; + rb.rb_vels.linvel = self.linvel; + rb.rb_vels.angvel = self.angvel; + rb.rb_type = self.rb_type; rb.user_data = self.user_data; - rb.mass_properties = self.mass_properties; - rb.linear_damping = self.linear_damping; - rb.angular_damping = self.angular_damping; - rb.gravity_scale = self.gravity_scale; - rb.flags = self.flags; - rb.dominance_group = self.dominance_group; + rb.rb_mprops.mass_properties = self.mass_properties; + rb.rb_mprops.flags = self.mprops_flags; + rb.rb_damping.linear_damping = self.linear_damping; + rb.rb_damping.angular_damping = self.angular_damping; + rb.rb_forces.gravity_scale = self.gravity_scale; + rb.rb_dominance = RigidBodyDominance(self.dominance_group); rb.enable_ccd(self.ccd_enabled); if self.can_sleep && self.sleeping { @@ -1085,55 +949,9 @@ impl RigidBodyBuilder { } if !self.can_sleep { - rb.activation.threshold = -1.0; + rb.rb_activation.threshold = -1.0; } rb } } - -/// The activation status of a body. -/// -/// This controls whether a body is sleeping or not. -/// If the threshold is negative, the body never sleeps. -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct ActivationStatus { - /// The threshold pseudo-kinetic energy bellow which the body can fall asleep. - pub threshold: Real, - /// The current pseudo-kinetic energy of the body. - pub energy: Real, - /// Is this body already sleeping? - pub sleeping: bool, -} - -impl ActivationStatus { - /// The default amount of energy bellow which a body can be put to sleep by nphysics. - pub fn default_threshold() -> Real { - 0.01 - } - - /// Create a new activation status initialised with the default activation threshold and is active. - pub fn new_active() -> Self { - ActivationStatus { - threshold: Self::default_threshold(), - energy: Self::default_threshold() * 4.0, - sleeping: false, - } - } - - /// Create a new activation status initialised with the default activation threshold and is inactive. - pub fn new_inactive() -> Self { - ActivationStatus { - threshold: Self::default_threshold(), - energy: 0.0, - sleeping: true, - } - } - - /// Returns `true` if the body is not asleep. - #[inline] - pub fn is_active(&self) -> bool { - self.energy != 0.0 - } -} diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs new file mode 100644 index 0000000..855a0e8 --- /dev/null +++ b/src/dynamics/rigid_body_components.rs @@ -0,0 +1,659 @@ +use crate::data::{ComponentSetMut, ComponentSetOption}; +use crate::dynamics::MassProperties; +use crate::geometry::{ + ColliderChanges, ColliderHandle, ColliderMassProperties, ColliderParent, ColliderPosition, + ColliderShape, InteractionGraph, RigidBodyGraphIndex, +}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Translation, Vector}; +use crate::parry::partitioning::IndexedData; +use crate::utils::WDot; +use num::Zero; + +/// The unique handle of a rigid body added to a `RigidBodySet`. +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[repr(transparent)] +pub struct RigidBodyHandle(pub crate::data::arena::Index); + +impl RigidBodyHandle { + /// Converts this handle into its (index, generation) components. + pub fn into_raw_parts(self) -> (u32, u32) { + self.0.into_raw_parts() + } + + /// Reconstructs an handle from its (index, generation) components. + pub fn from_raw_parts(id: u32, generation: u32) -> Self { + Self(crate::data::arena::Index::from_raw_parts(id, generation)) + } + + /// An always-invalid rigid-body handle. + pub fn invalid() -> Self { + Self(crate::data::arena::Index::from_raw_parts( + crate::INVALID_U32, + crate::INVALID_U32, + )) + } +} + +impl IndexedData for RigidBodyHandle { + fn default() -> Self { + Self(IndexedData::default()) + } + + fn index(&self) -> usize { + self.0.index() + } +} + +/// The type of a body, governing the way it is affected by external forces. +#[deprecated(note = "renamed as RigidBodyType")] +pub type BodyStatus = RigidBodyType; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The status of a body, governing the way it is affected by external forces. +pub enum RigidBodyType { + /// A `RigidBodyType::Dynamic` body can be affected by all external forces. + Dynamic, + /// A `RigidBodyType::Static` body cannot be affected by external forces. + Static, + /// A `RigidBodyType::Kinematic` body cannot be affected by any external forces but can be controlled + /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies. + /// + /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body + /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be + /// modified by the user and is independent from any contact or joint it is involved in. + Kinematic, + // Semikinematic, // A kinematic that performs automatic CCD with the static environment to avoid traversing it? + // Disabled, +} + +impl RigidBodyType { + pub fn is_static(self) -> bool { + self == RigidBodyType::Static + } + + pub fn is_dynamic(self) -> bool { + self == RigidBodyType::Dynamic + } + + pub fn is_kinematic(self) -> bool { + self == RigidBodyType::Kinematic + } +} + +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + /// Flags describing how the rigid-body has been modified by the user. + pub struct RigidBodyChanges: u32 { + const MODIFIED = 1 << 0; + const POSITION = 1 << 1; + const SLEEP = 1 << 2; + const COLLIDERS = 1 << 3; + const TYPE = 1 << 4; + } +} + +impl Default for RigidBodyChanges { + fn default() -> Self { + RigidBodyChanges::empty() + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyPosition { + /// The world-space position of the rigid-body. + pub position: Isometry, + /// The next position of the rigid-body. + /// + /// At the beginning of the timestep, and when the + /// timestep is complete we must have position == next_position + /// except for kinematic bodies. + /// + /// The next_position is updated after the velocity and position + /// resolution. Then it is either validated (ie. we set position := set_position) + /// or clamped by CCD. + pub next_position: Isometry, +} + +impl Default for RigidBodyPosition { + fn default() -> Self { + Self { + position: Isometry::identity(), + next_position: Isometry::identity(), + } + } +} + +impl RigidBodyPosition { + #[must_use] + pub fn interpolate_velocity(&self, inv_dt: Real) -> RigidBodyVelocity { + let dpos = self.next_position * self.position.inverse(); + let angvel; + #[cfg(feature = "dim2")] + { + angvel = dpos.rotation.angle() * inv_dt; + } + #[cfg(feature = "dim3")] + { + angvel = dpos.rotation.scaled_axis() * inv_dt; + } + let linvel = dpos.translation.vector * inv_dt; + RigidBodyVelocity { linvel, angvel } + } + + #[must_use] + pub fn integrate_force_and_velocity( + &self, + dt: Real, + forces: &RigidBodyForces, + vels: &RigidBodyVelocity, + mprops: &RigidBodyMassProps, + ) -> Isometry { + let new_vels = forces.integrate(dt, vels, mprops); + new_vels.integrate(dt, &self.position, &mprops.mass_properties.local_com) + } +} + +impl From for RigidBodyPosition +where + Isometry: From, +{ + fn from(position: T) -> Self { + let position = position.into(); + Self { + position, + next_position: position, + } + } +} + +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + /// Flags affecting the behavior of the constraints solver for a given contact manifold. + pub struct RigidBodyMassPropsFlags: u8 { + const TRANSLATION_LOCKED = 1 << 0; + const ROTATION_LOCKED_X = 1 << 1; + const ROTATION_LOCKED_Y = 1 << 2; + const ROTATION_LOCKED_Z = 1 << 3; + const ROTATION_LOCKED = Self::ROTATION_LOCKED_X.bits | Self::ROTATION_LOCKED_Y.bits | Self::ROTATION_LOCKED_Z.bits; + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyMassProps { + /// Flags for locking rotation and translation. + pub flags: RigidBodyMassPropsFlags, + /// The local mass properties of the rigid-body. + pub mass_properties: MassProperties, + /// The world-space center of mass of the rigid-body. + pub world_com: Point, + /// The inverse mass taking into account translation locking. + pub effective_inv_mass: Real, + /// The square-root of the world-space inverse angular inertia tensor of the rigid-body, + /// taking into account rotation locking. + pub effective_world_inv_inertia_sqrt: AngularInertia, +} + +impl Default for RigidBodyMassProps { + fn default() -> Self { + Self { + flags: RigidBodyMassPropsFlags::empty(), + mass_properties: MassProperties::zero(), + world_com: Point::origin(), + effective_inv_mass: 0.0, + effective_world_inv_inertia_sqrt: AngularInertia::zero(), + } + } +} + +impl From for RigidBodyMassProps { + fn from(flags: RigidBodyMassPropsFlags) -> Self { + Self { + flags, + ..Self::default() + } + } +} + +impl RigidBodyMassProps { + #[must_use] + pub fn with_translations_locked(mut self) -> Self { + self.flags |= RigidBodyMassPropsFlags::TRANSLATION_LOCKED; + self + } + + pub fn effective_mass(&self) -> Real { + crate::utils::inv(self.effective_inv_mass) + } + + pub fn update_world_mass_properties(&mut self, position: &Isometry) { + self.world_com = self.mass_properties.world_com(&position); + self.effective_inv_mass = self.mass_properties.inv_mass; + self.effective_world_inv_inertia_sqrt = self + .mass_properties + .world_inv_inertia_sqrt(&position.rotation); + + // Take into account translation/rotation locking. + if self + .flags + .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED) + { + self.effective_inv_mass = 0.0; + } + + #[cfg(feature = "dim2")] + { + if self + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z) + { + self.effective_world_inv_inertia_sqrt = 0.0; + } + } + #[cfg(feature = "dim3")] + { + if self + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_X) + { + self.effective_world_inv_inertia_sqrt.m11 = 0.0; + self.effective_world_inv_inertia_sqrt.m12 = 0.0; + self.effective_world_inv_inertia_sqrt.m13 = 0.0; + } + + if self + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Y) + { + self.effective_world_inv_inertia_sqrt.m22 = 0.0; + self.effective_world_inv_inertia_sqrt.m12 = 0.0; + self.effective_world_inv_inertia_sqrt.m23 = 0.0; + } + if self + .flags + .contains(RigidBodyMassPropsFlags::ROTATION_LOCKED_Z) + { + self.effective_world_inv_inertia_sqrt.m33 = 0.0; + self.effective_world_inv_inertia_sqrt.m13 = 0.0; + self.effective_world_inv_inertia_sqrt.m23 = 0.0; + } + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyVelocity { + /// The linear velocity of the rigid-body. + pub linvel: Vector, + /// The angular velocity of the rigid-body. + pub angvel: AngVector, +} + +impl Default for RigidBodyVelocity { + fn default() -> Self { + Self::zero() + } +} + +impl RigidBodyVelocity { + #[must_use] + pub fn zero() -> Self { + Self { + linvel: na::zero(), + angvel: na::zero(), + } + } + + #[must_use] + pub fn pseudo_kinetic_energy(&self) -> Real { + self.linvel.norm_squared() + self.angvel.gdot(self.angvel) + } + + #[must_use] + pub fn apply_damping(&self, dt: Real, damping: &RigidBodyDamping) -> Self { + RigidBodyVelocity { + linvel: self.linvel * (1.0 / (1.0 + dt * damping.linear_damping)), + angvel: self.angvel * (1.0 / (1.0 + dt * damping.angular_damping)), + } + } + + #[must_use] + pub fn integrate( + &self, + dt: Real, + init_pos: &Isometry, + local_com: &Point, + ) -> Isometry { + let com = init_pos * local_com; + let shift = Translation::from(com.coords); + let mut result = + shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos; + result.rotation.renormalize_fast(); + result + } + + #[must_use] + pub fn is_zero(&self) -> bool { + self.linvel.is_zero() && self.angvel.is_zero() + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyDamping { + /// Damping factor for gradually slowing down the translational motion of the rigid-body. + pub linear_damping: Real, + /// Damping factor for gradually slowing down the angular motion of the rigid-body. + pub angular_damping: Real, +} + +impl Default for RigidBodyDamping { + fn default() -> Self { + Self { + linear_damping: 0.0, + angular_damping: 0.0, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyForces { + /// Accumulation of external forces (only for dynamic bodies). + pub force: Vector, + /// Accumulation of external torques (only for dynamic bodies). + pub torque: AngVector, + pub gravity_scale: Real, +} + +impl Default for RigidBodyForces { + fn default() -> Self { + Self { + force: na::zero(), + torque: na::zero(), + gravity_scale: 1.0, + } + } +} + +impl RigidBodyForces { + #[must_use] + pub fn integrate( + &self, + dt: Real, + init_vels: &RigidBodyVelocity, + mprops: &RigidBodyMassProps, + ) -> RigidBodyVelocity { + let linear_acc = self.force * mprops.effective_inv_mass; + let angular_acc = mprops.effective_world_inv_inertia_sqrt + * (mprops.effective_world_inv_inertia_sqrt * self.torque); + + RigidBodyVelocity { + linvel: init_vels.linvel + linear_acc * dt, + angvel: init_vels.angvel + angular_acc * dt, + } + } + + pub fn add_linear_acceleration(&mut self, gravity: &Vector, mass: Real) { + self.force += gravity * self.gravity_scale * mass; + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyCcd { + pub ccd_thickness: Real, + pub ccd_max_dist: Real, + pub ccd_active: bool, + pub ccd_enabled: bool, +} + +impl Default for RigidBodyCcd { + fn default() -> Self { + Self { + ccd_thickness: 0.0, + ccd_max_dist: 0.0, + ccd_active: false, + ccd_enabled: false, + } + } +} + +impl RigidBodyCcd { + pub fn max_point_velocity(&self, vels: &RigidBodyVelocity) -> Real { + #[cfg(feature = "dim2")] + return vels.linvel.norm() + vels.angvel.abs() * self.ccd_max_dist; + #[cfg(feature = "dim3")] + return vels.linvel.norm() + vels.angvel.norm() * self.ccd_max_dist; + } + + pub fn is_moving_fast( + &self, + dt: Real, + vels: &RigidBodyVelocity, + forces: Option<&RigidBodyForces>, + ) -> bool { + // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we + // should use `self.rb_ccd.ccd_thickness - smallest_contact_dist` where `smallest_contact_dist` + // is the deepest contact (the contact with the largest penetration depth, i.e., the + // negative `dist` with the largest absolute value. + // However, getting this penetration depth assumes querying the contact graph from + // the narrow-phase, which can be pretty expensive. So we use the CCD thickness + // divided by 10 right now. We will see in practice if this value is OK or if we + // should use a smaller (to be less conservative) or larger divisor (to be more conservative). + let threshold = self.ccd_thickness / 10.0; + + if let Some(forces) = forces { + let linear_part = (vels.linvel + forces.force * dt).norm(); + #[cfg(feature = "dim2")] + let angular_part = (vels.angvel + forces.torque * dt).abs() * self.ccd_max_dist; + #[cfg(feature = "dim3")] + let angular_part = (vels.angvel + forces.torque * dt).norm() * self.ccd_max_dist; + let vel_with_forces = linear_part + angular_part; + vel_with_forces > threshold + } else { + self.max_point_velocity(vels) * dt > threshold + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyIds { + pub joint_graph_index: RigidBodyGraphIndex, + pub active_island_id: usize, + pub active_set_id: usize, + pub active_set_offset: usize, + pub active_set_timestamp: u32, +} + +impl Default for RigidBodyIds { + fn default() -> Self { + Self { + joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(), + active_island_id: 0, + active_set_id: 0, + active_set_offset: 0, + active_set_timestamp: 0, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug)] +pub struct RigidBodyColliders(pub Vec); + +impl Default for RigidBodyColliders { + fn default() -> Self { + Self(vec![]) + } +} + +impl RigidBodyColliders { + pub fn detach_collider( + &mut self, + rb_changes: &mut RigidBodyChanges, + co_handle: ColliderHandle, + ) { + if let Some(i) = self.0.iter().position(|e| *e == co_handle) { + rb_changes.set( + RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS, + true, + ); + self.0.swap_remove(i); + } + } + + pub fn attach_collider( + &mut self, + rb_changes: &mut RigidBodyChanges, + rb_ccd: &mut RigidBodyCcd, + rb_mprops: &mut RigidBodyMassProps, + rb_pos: &RigidBodyPosition, + co_handle: ColliderHandle, + co_pos: &mut ColliderPosition, + co_parent: &ColliderParent, + co_shape: &ColliderShape, + co_mprops: &ColliderMassProperties, + ) { + rb_changes.set( + RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS, + true, + ); + + co_pos.0 = rb_pos.position * co_parent.pos_wrt_parent; + rb_ccd.ccd_thickness = rb_ccd.ccd_thickness.min(co_shape.ccd_thickness()); + + let shape_bsphere = co_shape.compute_bounding_sphere(&co_parent.pos_wrt_parent); + rb_ccd.ccd_max_dist = rb_ccd + .ccd_max_dist + .max(shape_bsphere.center.coords.norm() + shape_bsphere.radius); + + let mass_properties = co_mprops + .mass_properties(&**co_shape) + .transform_by(&co_parent.pos_wrt_parent); + self.0.push(co_handle); + rb_mprops.mass_properties += mass_properties; + rb_mprops.update_world_mass_properties(&rb_pos.position); + } + + pub fn update_positions( + &self, + colliders: &mut Colliders, + modified_colliders: &mut Vec, + parent_pos: &Isometry, + ) where + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetOption, + { + 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; + + // 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)); + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Copy)] +pub struct RigidBodyDominance(pub i8); + +impl Default for RigidBodyDominance { + fn default() -> Self { + RigidBodyDominance(0) + } +} + +impl RigidBodyDominance { + pub fn effective_group(&self, status: &RigidBodyType) -> i16 { + if status.is_dynamic() { + self.0 as i16 + } else { + i8::MAX as i16 + 1 + } + } +} + +/// The rb_activation status of a body. +/// +/// This controls whether a body is sleeping or not. +/// If the threshold is negative, the body never sleeps. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct RigidBodyActivation { + /// The threshold pseudo-kinetic energy bellow which the body can fall asleep. + pub threshold: Real, + /// The current pseudo-kinetic energy of the body. + pub energy: Real, + /// Is this body already sleeping? + pub sleeping: bool, +} + +impl Default for RigidBodyActivation { + fn default() -> Self { + Self::new_active() + } +} + +impl RigidBodyActivation { + /// The default amount of energy bellow which a body can be put to sleep by nphysics. + pub fn default_threshold() -> Real { + 0.01 + } + + /// Create a new rb_activation status initialised with the default rb_activation threshold and is active. + pub fn new_active() -> Self { + RigidBodyActivation { + threshold: Self::default_threshold(), + energy: Self::default_threshold() * 4.0, + sleeping: false, + } + } + + /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive. + pub fn new_inactive() -> Self { + RigidBodyActivation { + threshold: Self::default_threshold(), + energy: 0.0, + sleeping: true, + } + } + + /// Returns `true` if the body is not asleep. + #[inline] + pub fn is_active(&self) -> bool { + self.energy != 0.0 + } + + #[inline] + pub fn wake_up(&mut self, strong: bool) { + self.sleeping = false; + if strong || self.energy == 0.0 { + self.energy = self.threshold.abs() * 2.0; + } + } + + #[inline] + pub fn sleep(&mut self) { + self.energy = 0.0; + self.sleeping = true; + } +} diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 5a85ada..10e7bf8 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -1,48 +1,19 @@ #[cfg(feature = "parallel")] use rayon::prelude::*; -use crate::data::arena::Arena; -use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody, RigidBodyChanges}; -use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase}; +use crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{ + IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle, + RigidBodyType, +}; +use crate::dynamics::{ + JointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces, + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, +}; +use crate::geometry::ColliderSet; use parry::partitioning::IndexedData; use std::ops::{Index, IndexMut}; -/// The unique handle of a rigid body added to a `RigidBodySet`. -#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[repr(transparent)] -pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index); - -impl RigidBodyHandle { - /// Converts this handle into its (index, generation) components. - pub fn into_raw_parts(self) -> (usize, u64) { - self.0.into_raw_parts() - } - - /// Reconstructs an handle from its (index, generation) components. - pub fn from_raw_parts(id: usize, generation: u64) -> Self { - Self(crate::data::arena::Index::from_raw_parts(id, generation)) - } - - /// An always-invalid rigid-body handle. - pub fn invalid() -> Self { - Self(crate::data::arena::Index::from_raw_parts( - crate::INVALID_USIZE, - crate::INVALID_U64, - )) - } -} - -impl IndexedData for RigidBodyHandle { - fn default() -> Self { - Self(IndexedData::default()) - } - - fn index(&self) -> usize { - self.0.index() - } -} - #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A pair of rigid body handles. @@ -69,38 +40,75 @@ pub struct RigidBodySet { // parallelism because the `Receiver` breaks the Sync impl. // Could we avoid this? pub(crate) bodies: Arena, - pub(crate) active_dynamic_set: Vec, - pub(crate) active_kinematic_set: Vec, - // Set of inactive bodies which have been modified. - // This typically include static bodies which have been modified. - pub(crate) modified_inactive_set: Vec, - pub(crate) active_islands: Vec, - active_set_timestamp: u32, pub(crate) modified_bodies: Vec, - pub(crate) modified_all_bodies: bool, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - can_sleep: Vec, // Workspace. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - stack: Vec, // Workspace. } +macro_rules! impl_field_component_set( + ($T: ty, $field: ident) => { + impl ComponentSetOption<$T> for RigidBodySet { + fn get(&self, handle: crate::data::Index) -> Option<&$T> { + self.get(RigidBodyHandle(handle)).map(|b| &b.$field) + } + } + + impl ComponentSet<$T> for RigidBodySet { + fn size_hint(&self) -> usize { + self.len() + } + + #[inline(always)] + fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) { + for (handle, body) in self.bodies.iter() { + f(handle, &body.$field) + } + } + } + + impl ComponentSetMut<$T> for RigidBodySet { + fn set_internal(&mut self, handle: crate::data::Index, val: $T) { + if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) { + rb.$field = val; + } + } + + #[inline(always)] + fn map_mut_internal( + &mut self, + handle: crate::data::Index, + f: impl FnOnce(&mut $T) -> Result, + ) -> Option { + self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field)) + } + } + } +); + +impl_field_component_set!(RigidBodyPosition, rb_pos); +impl_field_component_set!(RigidBodyMassProps, rb_mprops); +impl_field_component_set!(RigidBodyVelocity, rb_vels); +impl_field_component_set!(RigidBodyDamping, rb_damping); +impl_field_component_set!(RigidBodyForces, rb_forces); +impl_field_component_set!(RigidBodyCcd, rb_ccd); +impl_field_component_set!(RigidBodyIds, rb_ids); +impl_field_component_set!(RigidBodyType, rb_type); +impl_field_component_set!(RigidBodyActivation, rb_activation); +impl_field_component_set!(RigidBodyColliders, rb_colliders); +impl_field_component_set!(RigidBodyDominance, rb_dominance); +impl_field_component_set!(RigidBodyChanges, changes); + impl RigidBodySet { /// Create a new empty set of rigid bodies. pub fn new() -> Self { RigidBodySet { bodies: Arena::new(), - active_dynamic_set: Vec::new(), - active_kinematic_set: Vec::new(), - modified_inactive_set: Vec::new(), - active_islands: Vec::new(), - active_set_timestamp: 0, modified_bodies: Vec::new(), - modified_all_bodies: false, - can_sleep: Vec::new(), - stack: Vec::new(), } } + pub(crate) fn take_modified(&mut self) -> Vec { + std::mem::replace(&mut self.modified_bodies, vec![]) + } + /// The number of rigid bodies on this set. pub fn len(&self) -> usize { self.bodies.len() @@ -121,18 +129,10 @@ impl RigidBodySet { // Make sure the internal links are reset, they may not be // if this rigid-body was obtained by cloning another one. rb.reset_internal_references(); - rb.changes.set(RigidBodyChanges::all(), true); + rb.changes_mut_internal().set(RigidBodyChanges::all(), true); let handle = RigidBodyHandle(self.bodies.insert(rb)); self.modified_bodies.push(handle); - - let rb = &mut self.bodies[handle.0]; - - if rb.is_kinematic() { - rb.active_set_id = self.active_kinematic_set.len(); - self.active_kinematic_set.push(handle); - } - handle } @@ -140,6 +140,7 @@ impl RigidBodySet { pub fn remove( &mut self, handle: RigidBodyHandle, + islands: &mut IslandManager, colliders: &mut ColliderSet, joints: &mut JointSet, ) -> Option { @@ -147,55 +148,23 @@ impl RigidBodySet { /* * Update active sets. */ - let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; - - for active_set in &mut active_sets { - if active_set.get(rb.active_set_id) == Some(&handle) { - active_set.swap_remove(rb.active_set_id); - - if let Some(replacement) = active_set.get(rb.active_set_id) { - self.bodies[replacement.0].active_set_id = rb.active_set_id; - } - } - } + islands.rigid_body_removed(handle, &rb.rb_ids, self); /* * Remove colliders attached to this rigid-body. */ - for collider in &rb.colliders { - colliders.remove(*collider, self, false); + for collider in rb.colliders() { + colliders.remove(*collider, islands, self, false); } /* * Remove joints attached to this rigid-body. */ - joints.remove_rigid_body(rb.joint_graph_index, self); + joints.remove_rigid_body(rb.rb_ids.joint_graph_index, islands, self); Some(rb) } - pub(crate) fn num_islands(&self) -> usize { - self.active_islands.len() - 1 - } - - /// Forces the specified rigid-body to wake up if it is dynamic. - /// - /// If `strong` is `true` then it is assured that the rigid-body will - /// remain awake during multiple subsequent timesteps. - pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) { - if let Some(rb) = self.bodies.get_mut(handle.0) { - // TODO: what about kinematic bodies? - if rb.is_dynamic() { - rb.wake_up(strong); - - if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) { - rb.active_set_id = self.active_dynamic_set.len(); - self.active_dynamic_set.push(handle); - } - } - } - } - /// Gets the rigid-body with the given handle without a known generation. /// /// This is useful when you know you want the rigid-body at position `i` but @@ -224,12 +193,7 @@ impl RigidBodySet { pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> { let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?; let handle = RigidBodyHandle(handle); - Self::mark_as_modified( - handle, - rb, - &mut self.modified_bodies, - self.modified_all_bodies, - ); + Self::mark_as_modified(handle, rb, &mut self.modified_bodies); Some((rb, handle)) } @@ -238,14 +202,13 @@ impl RigidBodySet { self.bodies.get(handle.0) } - fn mark_as_modified( + pub(crate) fn mark_as_modified( handle: RigidBodyHandle, rb: &mut RigidBody, modified_bodies: &mut Vec, - modified_all_bodies: bool, ) { - if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) { - rb.changes = RigidBodyChanges::MODIFIED; + if !rb.changes().contains(RigidBodyChanges::MODIFIED) { + *rb.changes_mut_internal() = RigidBodyChanges::MODIFIED; modified_bodies.push(handle); } } @@ -254,12 +217,7 @@ impl RigidBodySet { #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> { let result = self.bodies.get_mut(handle.0)?; - Self::mark_as_modified( - handle, - result, - &mut self.modified_bodies, - self.modified_all_bodies, - ); + Self::mark_as_modified(handle, result, &mut self.modified_bodies); Some(result) } @@ -274,23 +232,10 @@ impl RigidBodySet { handle: RigidBodyHandle, ) -> Option<&mut RigidBody> { let result = self.bodies.get_mut(handle.0)?; - Self::mark_as_modified( - handle, - result, - &mut self.modified_bodies, - self.modified_all_bodies, - ); + Self::mark_as_modified(handle, result, &mut self.modified_bodies); Some(result) } - pub(crate) fn get2_mut_internal( - &mut self, - h1: RigidBodyHandle, - h2: RigidBodyHandle, - ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) { - self.bodies.get2_mut(h1.0, h2.0) - } - /// Iterates through all the rigid-bodies on this set. pub fn iter(&self) -> impl Iterator { self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b)) @@ -300,431 +245,11 @@ impl RigidBodySet { #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn iter_mut(&mut self) -> impl Iterator { self.modified_bodies.clear(); - self.modified_all_bodies = true; - self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b)) - } - - /// Iter through all the active kinematic rigid-bodies on this set. - pub fn iter_active_kinematic<'a>( - &'a self, - ) -> impl Iterator { - let bodies: &'a _ = &self.bodies; - self.active_kinematic_set - .iter() - .filter_map(move |h| Some((*h, bodies.get(h.0)?))) - } - - /// Iter through all the active dynamic rigid-bodies on this set. - pub fn iter_active_dynamic<'a>( - &'a self, - ) -> impl Iterator { - let bodies: &'a _ = &self.bodies; - self.active_dynamic_set - .iter() - .filter_map(move |h| Some((*h, bodies.get(h.0)?))) - } - - #[cfg(not(feature = "parallel"))] - pub(crate) fn iter_active_island<'a>( - &'a self, - island_id: usize, - ) -> impl Iterator { - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - let bodies: &'a _ = &self.bodies; - self.active_dynamic_set[island_range] - .iter() - .filter_map(move |h| Some((*h, bodies.get(h.0)?))) - } - - /// Applies the given function on all the active dynamic rigid-bodies - /// contained by this set. - #[inline(always)] - #[cfg(not(feature = "dev-remove-slow-accessors"))] - pub fn foreach_active_dynamic_body_mut( - &mut self, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - for handle in &self.active_dynamic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - Self::mark_as_modified( - *handle, - rb, - &mut self.modified_bodies, - self.modified_all_bodies, - ); - f(*handle, rb) - } - } - } - - #[inline(always)] - pub(crate) fn foreach_active_body_mut_internal( - &mut self, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - for handle in &self.active_dynamic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - - for handle in &self.active_kinematic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } - - #[inline(always)] - pub(crate) fn foreach_active_dynamic_body_mut_internal( - &mut self, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - for handle in &self.active_dynamic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } - - #[inline(always)] - pub(crate) fn foreach_active_kinematic_body_mut_internal( - &mut self, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - for handle in &self.active_kinematic_set { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } - - #[inline(always)] - #[cfg(not(feature = "parallel"))] - pub(crate) fn foreach_active_island_body_mut_internal( - &mut self, - island_id: usize, - mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), - ) { - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - for handle in &self.active_dynamic_set[island_range] { - if let Some(rb) = self.bodies.get_mut(handle.0) { - f(*handle, rb) - } - } - } - - #[cfg(feature = "parallel")] - #[inline(always)] - #[allow(dead_code)] - pub(crate) fn foreach_active_island_body_mut_internal_parallel( - &mut self, - island_id: usize, - f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync, - ) { - use std::sync::atomic::Ordering; - - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _); - self.active_dynamic_set[island_range] - .par_iter() - .for_each_init( - || bodies.load(Ordering::Relaxed), - |bodies, handle| { - let bodies: &mut Arena = unsafe { std::mem::transmute(*bodies) }; - if let Some(rb) = bodies.get_mut(handle.0) { - f(*handle, rb) - } - }, - ); - } - - // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] { - // &self.active_dynamic_set - // } - - pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { - self.active_islands[island_id]..self.active_islands[island_id + 1] - } - - pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { - &self.active_dynamic_set[self.active_island_range(island_id)] - } - - // Utility function to avoid some borrowing issue in the `maintain` method. - fn maintain_one( - bodies: &mut Arena, - colliders: &mut ColliderSet, - handle: RigidBodyHandle, - modified_inactive_set: &mut Vec, - active_kinematic_set: &mut Vec, - active_dynamic_set: &mut Vec, - ) { - enum FinalAction { - UpdateActiveKinematicSetId, - UpdateActiveDynamicSetId, - } - - if let Some(rb) = bodies.get_mut(handle.0) { - let mut final_action = None; - - // The body's status changed. We need to make sure - // it is on the correct active set. - if rb.changes.contains(RigidBodyChanges::BODY_STATUS) { - match rb.body_status() { - BodyStatus::Dynamic => { - // Remove from the active kinematic set if it was there. - if active_kinematic_set.get(rb.active_set_id) == Some(&handle) { - active_kinematic_set.swap_remove(rb.active_set_id); - final_action = - Some((FinalAction::UpdateActiveKinematicSetId, rb.active_set_id)); - } - - // Add to the active dynamic set. - rb.wake_up(true); - // Make sure the sleep change flag is set (even if for some - // reasons the rigid-body was already awake) to make - // sure the code handling sleeping change adds the body to - // the active_dynamic_set. - rb.changes.set(RigidBodyChanges::SLEEP, true); - } - BodyStatus::Kinematic => { - // Remove from the active dynamic set if it was there. - if active_dynamic_set.get(rb.active_set_id) == Some(&handle) { - active_dynamic_set.swap_remove(rb.active_set_id); - final_action = - Some((FinalAction::UpdateActiveDynamicSetId, rb.active_set_id)); - } - - // Add to the active kinematic set. - if active_kinematic_set.get(rb.active_set_id) != Some(&handle) { - rb.active_set_id = active_kinematic_set.len(); - active_kinematic_set.push(handle); - } - } - BodyStatus::Static => {} - } - } - - // Update the positions of the colliders. - if rb.changes.contains(RigidBodyChanges::POSITION) - || rb.changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.update_colliders_positions(colliders); - - if rb.is_static() { - modified_inactive_set.push(handle); - } - - if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle) - { - rb.active_set_id = active_kinematic_set.len(); - active_kinematic_set.push(handle); - } - } - - // Push the body to the active set if it is not - // sleeping and if it is not already inside of the active set. - if rb.changes.contains(RigidBodyChanges::SLEEP) - && !rb.is_sleeping() // May happen if the body was put to sleep manually. - && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. - && active_dynamic_set.get(rb.active_set_id) != Some(&handle) - { - rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. - active_dynamic_set.push(handle); - } - - rb.changes = RigidBodyChanges::empty(); - - // Adjust some ids, if needed. - if let Some((action, id)) = final_action { - let active_set = match action { - FinalAction::UpdateActiveKinematicSetId => active_kinematic_set, - FinalAction::UpdateActiveDynamicSetId => active_dynamic_set, - }; - - if id < active_set.len() { - if let Some(rb2) = bodies.get_mut(active_set[id].0) { - rb2.active_set_id = id; - } - } - } - } - } - - pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) { - if self.modified_all_bodies { - // Unfortunately, we have to push all the bodies to `modified_bodies` - // instead of just calling `maintain_one` on each element i - // `self.bodies.iter_mut()` because otherwise it would be difficult to - // handle the final change of active_set_id in Self::maintain_one - // (because it has to modify another rigid-body because of the swap-remove. - // So this causes borrowing problems if we do this while iterating - // through self.bodies.iter_mut()). - for (handle, _) in self.bodies.iter_mut() { - self.modified_bodies.push(RigidBodyHandle(handle)); - } - } - - for handle in self.modified_bodies.drain(..) { - Self::maintain_one( - &mut self.bodies, - colliders, - handle, - &mut self.modified_inactive_set, - &mut self.active_kinematic_set, - &mut self.active_dynamic_set, - ) - } - - if self.modified_all_bodies { - self.modified_bodies.shrink_to_fit(); // save some memory. - self.modified_all_bodies = false; - } - } - - pub(crate) fn update_active_set_with_contacts( - &mut self, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - joint_graph: &InteractionGraph, - min_island_size: usize, - ) { - assert!( - min_island_size > 0, - "The minimum island size must be at least 1." - ); - - // Update the energy of every rigid body and - // keep only those that may not sleep. - // let t = instant::now(); - self.active_set_timestamp += 1; - self.stack.clear(); - self.can_sleep.clear(); - - // NOTE: the `.rev()` is here so that two successive timesteps preserve - // the order of the bodies in the `active_dynamic_set` vec. This reversal - // does not seem to affect performances nor stability. However it makes - // debugging slightly nicer so we keep this rev. - for h in self.active_dynamic_set.drain(..).rev() { - let rb = &mut self.bodies[h.0]; - rb.update_energy(); - if rb.activation.energy <= rb.activation.threshold { - // 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; - self.can_sleep.push(h); - } else { - self.stack.push(h); - } - } - - // Read all the contacts and push objects touching touching this rigid-body. - #[inline(always)] - fn push_contacting_bodies( - rb: &RigidBody, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - stack: &mut Vec, - ) { - for collider_handle in &rb.colliders { - if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { - for inter in contacts { - for manifold in &inter.2.manifolds { - if !manifold.data.solver_contacts.is_empty() { - let other = crate::utils::select_other( - (inter.0, inter.1), - *collider_handle, - ); - let other_body = colliders[other].parent; - stack.push(other_body); - break; - } - } - } - } - } - } - - // 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 rb = &self.bodies[h.0]; - - if !rb.is_moving() { - // If the kinematic body does not move, it does not have - // to wake up any dynamic body. - continue; - } - - push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack); - } - - // println!("Selection: {}", instant::now() - t); - - // let t = instant::now(); - // Propagation of awake state and awake island computation through the - // traversal of the interaction graph. - self.active_islands.clear(); - self.active_islands.push(0); - - // The max avoid underflow when the stack is empty. - let mut island_marker = self.stack.len().max(1) - 1; - - while let Some(handle) = self.stack.pop() { - let rb = &mut self.bodies[handle.0]; - - if rb.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 static bodies. - continue; - } - - if self.stack.len() < island_marker { - if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() - >= min_island_size - { - // We are starting a new island. - self.active_islands.push(self.active_dynamic_set.len()); - } - - island_marker = self.stack.len(); - } - - rb.wake_up(false); - rb.active_island_id = self.active_islands.len() - 1; - rb.active_set_id = self.active_dynamic_set.len(); - rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id]; - rb.active_set_timestamp = self.active_set_timestamp; - self.active_dynamic_set.push(handle); - - // Transmit the active state to all the rigid-bodies with colliders - // in contact or joined with this collider. - push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack); - - for inter in joint_graph.interactions_with(rb.joint_graph_index) { - let other = crate::utils::select_other((inter.0, inter.1), handle); - self.stack.push(other); - } - } - - self.active_islands.push(self.active_dynamic_set.len()); - // println!( - // "Extraction: {}, num islands: {}", - // instant::now() - t, - // self.active_islands.len() - 1 - // ); - - // Actually put to sleep bodies which have not been detected as awake. - // let t = instant::now(); - for h in &self.can_sleep { - let b = &mut self.bodies[h.0]; - if b.activation.sleeping { - b.sleep(); - } - } - // println!("Activation: {}", instant::now() - t); + let modified_bodies = &mut self.modified_bodies; + self.bodies.iter_mut().map(move |(h, b)| { + modified_bodies.push(RigidBodyHandle(h)); + (RigidBodyHandle(h), b) + }) } } @@ -736,16 +261,19 @@ impl Index for RigidBodySet { } } +impl Index for RigidBodySet { + type Output = RigidBody; + + fn index(&self, index: crate::data::Index) -> &RigidBody { + &self.bodies[index] + } +} + #[cfg(not(feature = "dev-remove-slow-accessors"))] impl IndexMut for RigidBodySet { fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody { let rb = &mut self.bodies[handle.0]; - Self::mark_as_modified( - handle, - rb, - &mut self.modified_bodies, - self.modified_all_bodies, - ); + Self::mark_as_modified(handle, rb, &mut self.modified_bodies); rb } } diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index e3327c6..366a5b3 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,8 +1,9 @@ -use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; +use crate::data::ComponentSet; +use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( - _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code. + _bodies: &impl ComponentSet, // Unused but useful to simplify the parallel code. manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec, @@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts( } pub(crate) fn categorize_joints( - bodies: &RigidBodySet, + bodies: &impl ComponentSet, joints: &[JointGraphEdge], joint_indices: &[JointIndex], ground_joints: &mut Vec, @@ -28,10 +29,10 @@ pub(crate) fn categorize_joints( ) { for joint_i in joint_indices { let joint = &joints[*joint_i].weight; - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; + let status1 = bodies.index(joint.body1.0); + let status2 = bodies.index(joint.body2.0); - if !rb1.is_dynamic() || !rb2.is_dynamic() { + if !status1.is_dynamic() || !status2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 6b8de5a..86f9cff 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -1,4 +1,8 @@ -use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::data::{BundleSet, ComponentSet}; +#[cfg(feature = "parallel")] +use crate::dynamics::BodyPair; +use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyType}; +use crate::dynamics::{JointGraphEdge, JointIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use { @@ -6,16 +10,19 @@ use { vec_map::VecMap, }; +#[cfg(feature = "parallel")] pub(crate) trait PairInteraction { fn body_pair(&self) -> BodyPair; } +#[cfg(feature = "parallel")] impl<'a> PairInteraction for &'a mut ContactManifold { fn body_pair(&self) -> BodyPair { self.data.body_pair } } +#[cfg(feature = "parallel")] impl<'a> PairInteraction for JointGraphEdge { fn body_pair(&self) -> BodyPair { BodyPair::new(self.weight.body1, self.weight.body2) @@ -54,7 +61,7 @@ impl ParallelInteractionGroups { pub fn group_interactions( &mut self, island_id: usize, - bodies: &RigidBodySet, + bodies: &impl ComponentSet, interactions: &[Interaction], interaction_indices: &[usize], ) { @@ -78,8 +85,8 @@ impl ParallelInteractionGroups { .zip(self.interaction_colors.iter_mut()) { let body_pair = interactions[*interaction_id].body_pair(); - let rb1 = &bodies[body_pair.body1]; - let rb2 = &bodies[body_pair.body2]; + let rb1 = bodies.index(body_pair.body1); + let rb2 = bodies.index(body_pair.body2); match (rb1.is_static(), rb2.is_static()) { (false, false) => { @@ -168,14 +175,15 @@ impl InteractionGroups { self.nongrouped_interactions.clear(); } - // FIXME: there is a lot of duplicated code with group_manifolds here. + // TODO: there is a lot of duplicated code with group_manifolds here. // But we don't refactor just now because we may end up with distinct // grouping strategies in the future. #[cfg(not(feature = "simd-is-enabled"))] pub fn group_joints( &mut self, _island_id: usize, - _bodies: &RigidBodySet, + _islands: &IslandManager, + _bodies: &impl ComponentSet, _interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], ) { @@ -184,13 +192,16 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_joints( + pub fn group_joints( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, interactions: &[JointGraphEdge], interaction_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet + ComponentSet, + { // NOTE: in 3D we have up to 10 different joint types. // In 2D we only have 5 joint types. #[cfg(feature = "dim3")] @@ -204,11 +215,11 @@ impl InteractionGroups { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. - // FIXME: currently, this is a bit overconservative because when a bucket + // TODO: currently, this is a bit overconservative because when a bucket // is full, we don't clear the corresponding body mask bit. This may result // in less grouped constraints. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -216,10 +227,14 @@ impl InteractionGroups { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i].weight; - let body1 = &bodies[interaction.body1]; - let body2 = &bodies[interaction.body2]; - let is_static1 = !body1.is_dynamic(); - let is_static2 = !body2.is_dynamic(); + + let (status1, ids1): (&RigidBodyType, &RigidBodyIds) = + bodies.index_bundle(interaction.body1.0); + let (status2, ids2): (&RigidBodyType, &RigidBodyIds) = + bodies.index_bundle(interaction.body2.0); + + let is_static1 = !status1.is_dynamic(); + let is_static2 = !status2.is_dynamic(); if is_static1 && is_static2 { continue; @@ -232,8 +247,8 @@ impl InteractionGroups { } let ijoint = interaction.params.type_id(); - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = ids1.active_set_offset; + let i2 = ids2.active_set_offset; let conflicts = self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. @@ -325,7 +340,8 @@ impl InteractionGroups { pub fn group_manifolds( &mut self, _island_id: usize, - _bodies: &RigidBodySet, + _islands: &IslandManager, + _bodies: &impl ComponentSet, _interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], ) { @@ -334,21 +350,24 @@ impl InteractionGroups { } #[cfg(feature = "simd-is-enabled")] - pub fn group_manifolds( + pub fn group_manifolds( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, interactions: &[&mut ContactManifold], interaction_indices: &[ContactManifoldIndex], - ) { + ) where + Bodies: ComponentSet + ComponentSet, + { // Note: each bit of a body mask indicates what bucket already contains // a constraints involving this body. - // FIXME: currently, this is a bit overconservative because when a bucket + // TODO: currently, this is a bit overconservative because when a bucket // is full, we don't clear the corresponding body mask bit. This may result // in less grouped contacts. // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. self.body_masks - .resize(bodies.active_island(island_id).len(), 0u128); + .resize(islands.active_island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -359,31 +378,44 @@ impl InteractionGroups { .max() .unwrap_or(1); - // FIXME: find a way to reduce the number of iteration. + // TODO: find a way to reduce the number of iteration. // There must be a way to iterate just once on every interaction indices // instead of MAX_MANIFOLD_POINTS times. for k in 1..=max_interaction_points { for interaction_i in interaction_indices { let interaction = &interactions[*interaction_i]; - // FIXME: how could we avoid iterating + // TODO: how could we avoid iterating // on each interaction at every iteration on k? if interaction.data.num_active_contacts() != k { continue; } - let body1 = &bodies[interaction.data.body_pair.body1]; - let body2 = &bodies[interaction.data.body_pair.body2]; - let is_static1 = !body1.is_dynamic(); - let is_static2 = !body2.is_dynamic(); + let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 + { + let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0); + (*data.0, data.1.active_set_offset) + } else { + (RigidBodyType::Static, 0) + }; + let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 + { + let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0); + (*data.0, data.1.active_set_offset) + } else { + (RigidBodyType::Static, 0) + }; - // FIXME: don't generate interactions between static bodies in the first place. + let is_static1 = !status1.is_dynamic(); + let is_static2 = !status2.is_dynamic(); + + // TODO: don't generate interactions between static bodies in the first place. if is_static1 && is_static2 { continue; } - let i1 = body1.active_set_offset; - let i2 = body2.active_set_offset; + let i1 = active_set_offset1; + let i2 = active_set_offset2; let conflicts = self.body_masks[i1] | self.body_masks[i2]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index c684cc5..1e65341 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,10 +1,15 @@ use super::{PositionSolver, VelocitySolver}; use crate::counters::Counters; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, SolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces, + RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, +}; +use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub struct IslandSolver { @@ -24,17 +29,21 @@ impl IslandSolver { } } - pub fn solve_position_constraints( + pub fn solve_position_constraints( &mut self, island_id: usize, + islands: &IslandManager, counters: &mut Counters, params: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + bodies: &mut Bodies, + ) where + Bodies: ComponentSet + ComponentSetMut, + { counters.solver.position_resolution_time.resume(); self.position_solver.solve( island_id, params, + islands, bodies, &self.contact_constraints.position_constraints, &self.joint_constraints.position_constraints, @@ -42,31 +51,47 @@ impl IslandSolver { counters.solver.position_resolution_time.pause(); } - pub fn init_constraints_and_solve_velocity_constraints( + pub fn init_constraints_and_solve_velocity_constraints( &mut self, island_id: usize, counters: &mut Counters, params: &IntegrationParameters, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, manifolds: &mut [&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], joints: &mut [JointGraphEdge], joint_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0; if has_constraints { counters.solver.velocity_assembly_time.resume(); - self.contact_constraints - .init(island_id, params, bodies, manifolds, manifold_indices); + self.contact_constraints.init( + island_id, + params, + islands, + bodies, + manifolds, + manifold_indices, + ); self.joint_constraints - .init(island_id, params, bodies, joints, joint_indices); + .init(island_id, params, islands, bodies, joints, joint_indices); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); self.velocity_solver.solve( island_id, params, + islands, bodies, manifolds, joints, @@ -76,21 +101,50 @@ impl IslandSolver { counters.solver.velocity_resolution_time.pause(); counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); - }); + + for handle in islands.active_island(island_id) { + let (poss, vels, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_poss = *poss; + let new_vels = vels.apply_damping(params.dt, damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } + counters.solver.velocity_update_time.pause(); } else { self.contact_constraints.clear(); self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { + + for handle in islands.active_island(island_id) { // Since we didn't run the velocity solver we need to integrate the accelerations here - rb.integrate_accelerations(params.dt); - rb.apply_damping(params.dt); - rb.integrate_next_position(params.dt); - }); + let (poss, vels, forces, damping, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyDamping, + &RigidBodyMassProps, + ) = bodies.index_bundle(handle.0); + + let mut new_poss = *poss; + let new_vels = forces + .integrate(params.dt, vels, mprops) + .apply_damping(params.dt, &damping); + new_poss.next_position = + vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + + bodies.set_internal(handle.0, new_vels); + bodies.set_internal(handle.0, new_poss); + } counters.solver.velocity_update_time.pause(); } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 93996f4..159cc77 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; @@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint { } impl BallPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &BallJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + Self { - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, - im1: rb1.effective_inv_mass, - im2: rb2.effective_inv_mass, - ii1: rb1.effective_world_inv_inertia_sqrt.squared(), - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, + im1: mprops1.effective_inv_mass, + im2: mprops2.effective_inv_mass, + ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, } } @@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint { impl BallPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &BallJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + if flipped { // Note the only thing that is flipped here // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.next_position * cparams.local_anchor2, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor2, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } else { Self { - anchor1: rb1.next_position * cparams.local_anchor1, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + anchor1: poss1.next_position * cparams.local_anchor1, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, - position2: rb2.active_set_offset, - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + local_com2: mprops2.mass_properties.local_com, } } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index e9162a4..6b00639 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; @@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint { impl WBallPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], ) -> Self { - let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); - let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + let (mprops1, ids1) = rbs1; + let (mprops2, ids2) = rbs2; + + let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii1 = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + let ii2 = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let position1 = gather![|ii| ids1[ii].active_set_offset]; + let position2 = gather![|ii| ids2[ii].active_set_offset]; Self { local_com1, @@ -61,8 +72,8 @@ impl WBallPositionConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); - let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]); + let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); let anchor1 = position1 * self.local_anchor1; let anchor2 = position2 * self.local_anchor2; @@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint { impl WBallPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]); + let poss1 = rbs1; + let (mprops2, ids2) = rbs2; + + let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]); let anchor1 = position1 - * Point::from(array![|ii| if flipped[ii] { + * Point::from(gather![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 - }; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ) + }]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2 = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]) .squared(); - let local_anchor2 = Point::from(array![|ii| if flipped[ii] { + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 - }; SIMD_WIDTH]); - let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + }]); + let position2 = gather![|ii| ids2[ii].active_set_offset]; + let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); Self { anchor1, @@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint { } pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { - let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]); let anchor2 = position2 * self.local_anchor2; let com2 = position2 * self.local_com2; diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 508ac65..9e5227e 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -36,19 +37,32 @@ impl BallVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &BallJoint, ) -> Self { - let anchor_world1 = rb1.position * joint.local_anchor1; - let anchor_world2 = rb2.position * joint.local_anchor2; - let anchor1 = anchor_world1 - rb1.world_com; - let anchor2 = anchor_world2 - rb2.world_com; + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; - let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); - let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + let anchor_world1 = poss1.position * joint.local_anchor1; + let anchor_world2 = poss2.position * joint.local_anchor2; + let anchor1 = anchor_world1 - mprops1.world_com; + let anchor2 = anchor_world2 - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); + let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -59,12 +73,12 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] { - lhs = rb2 + lhs = mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2) - + rb1 + + mprops1 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat1) @@ -75,8 +89,8 @@ impl BallVelocityConstraint { // it's just easier that way. #[cfg(feature = "dim2")] { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; @@ -100,8 +114,8 @@ impl BallVelocityConstraint { ); if stiffness != 0.0 { - let dpos = rb2.position.rotation - * (rb1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = poss2.position.rotation + * (poss1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -113,15 +127,15 @@ impl BallVelocityConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; + let curr_vel = vels2.angvel - vels1.angvel; motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } #[cfg(feature = "dim2")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / (ii1 + ii2)) } else { Some(gamma) @@ -132,8 +146,8 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some((ii1 + ii2).inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -151,8 +165,8 @@ impl BallVelocityConstraint { BallVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, im2, impulse: joint.impulse * params.warmstart_coeff, @@ -164,8 +178,8 @@ impl BallVelocityConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, } } @@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &BallJoint, flipped: bool, ) -> Self { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let (anchor_world1, anchor_world2) = if flipped { ( - rb1.position * joint.local_anchor2, - rb2.position * joint.local_anchor1, + poss1.position * joint.local_anchor2, + poss2.position * joint.local_anchor1, ) } else { ( - rb1.position * joint.local_anchor1, - rb2.position * joint.local_anchor2, + poss1.position * joint.local_anchor1, + poss2.position * joint.local_anchor2, ) }; - let anchor1 = anchor_world1 - rb1.world_com; - let anchor2 = anchor_world2 - rb2.world_com; + let anchor1 = anchor_world1 - mprops1.world_com; + let anchor2 = anchor_world2 - mprops2.world_com; - let im2 = rb2.effective_inv_mass; - let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); - let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let im2 = mprops2.effective_inv_mass; + let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1); + let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2); let rhs = (vel2 - vel1) * params.velocity_solve_fraction + (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt(); @@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] { - lhs = rb2 + lhs = mprops2 .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) @@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim2")] { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); let m11 = im2 + cmat2.x * cmat2.x * ii2; let m12 = cmat2.x * cmat2.y * ii2; let m22 = im2 + cmat2.y * cmat2.y * ii2; @@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint { ); if stiffness != 0.0 { - let dpos = rb2.position.rotation - * (rb1.position.rotation * joint.motor_target_pos).inverse(); + let dpos = poss2.position.rotation + * (poss1.position.rotation * joint.motor_target_pos).inverse(); #[cfg(feature = "dim2")] { motor_rhs += dpos.angle() * stiffness; @@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint { } if damping != 0.0 { - let curr_vel = rb2.angvel - rb1.angvel; + let curr_vel = vels2.angvel - vels1.angvel; motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } #[cfg(feature = "dim2")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(gamma / ii2) } else { Some(gamma) @@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] if stiffness != 0.0 || damping != 0.0 { motor_inv_lhs = if keep_lhs { - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); Some(ii2.inverse_unchecked() * gamma) } else { Some(SdpMatrix::diagonal(gamma)) @@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint { BallVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, impulse: joint.impulse * params.warmstart_coeff, r2: anchor2, @@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint { motor_impulse, motor_inv_lhs, motor_max_impulse: joint.motor_max_impulse, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, } } diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index ff5e001..ee465cd 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, @@ -34,33 +35,46 @@ impl WBallVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); let anchor_world1 = position1 * local_anchor1; let anchor_world2 = position2 * local_anchor2; @@ -114,20 +128,16 @@ impl WBallVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; mj_lambda1.linear += self.impulse * self.im1; @@ -147,20 +157,16 @@ impl WBallVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let local_anchor1 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], - ); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let local_anchor1 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); - let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let impulse = Vector::from(gather![|ii| cparams[ii].impulse]); let anchor_world1 = position1 * local_anchor1; let anchor_world2 = position2 * local_anchor2; @@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; mj_lambda2.linear -= self.impulse * self.im2; @@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index c98660f..239138f 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::WAngularInertia; @@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint { } impl FixedPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &FixedJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); Self { local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, im1, im2, ii1, ii2, - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, lin_inv_lhs, ang_inv_lhs, } @@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint { impl FixedPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &FixedJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + let anchor1; let local_anchor2; if flipped { - anchor1 = rb1.next_position * cparams.local_anchor2; + anchor1 = poss1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; } else { - anchor1 = rb1.next_position * cparams.local_anchor1; + anchor1 = poss1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; }; Self { anchor1, local_anchor2, - position2: rb2.active_set_offset, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), - local_com2: rb2.mass_properties.local_com, + position2: ids2.active_set_offset, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), + local_com2: mprops2.mass_properties.local_com, impulse: 0.0, } } diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs index 2648816..0c0a6fd 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs @@ -1,5 +1,7 @@ use super::{FixedPositionConstraint, FixedPositionGroundConstraint}; -use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{Isometry, Real, SIMD_WIDTH}; // TODO: this does not uses SIMD optimizations yet. @@ -10,12 +12,22 @@ pub(crate) struct WFixedPositionConstraint { impl WFixedPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| FixedPositionConstraint::from_params( + (rbs1.0[ii], rbs1.1[ii]), + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii] + )], } } @@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint { impl WFixedPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + constraints: gather![|ii| FixedPositionGroundConstraint::from_params( + rbs1[ii], + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii], + flipped[ii] + )], } } diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index 11ade23..a0c0739 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -45,18 +46,31 @@ impl FixedVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), cparams: &FixedJoint, ) -> Self { - let anchor1 = rb1.position * cparams.local_anchor1; - let anchor2 = rb2.position * cparams.local_anchor2; - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1.translation.vector - rb1.world_com.coords; - let r2 = anchor2.translation.vector - rb2.world_com.coords; + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + + let anchor1 = poss1.position * cparams.local_anchor1; + let anchor2 = poss2.position * cparams.local_anchor2; + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - mprops1.world_com.coords; + let r2 = anchor2.translation.vector - mprops2.world_com.coords; let rmat1 = r1.gcross_matrix(); let rmat2 = r2.gcross_matrix(); @@ -99,8 +113,9 @@ impl FixedVelocityConstraint { #[cfg(feature = "dim3")] let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); - let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); - let ang_dvel = -rb1.angvel + rb2.angvel; + let lin_dvel = + -vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2); + let ang_dvel = -vels1.angvel + vels2.angvel; #[cfg(feature = "dim2")] let mut rhs = @@ -133,14 +148,14 @@ impl FixedVelocityConstraint { FixedVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, im2, ii1, ii2, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, r1, @@ -250,28 +265,36 @@ impl FixedVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), cparams: &FixedJoint, flipped: bool, ) -> Self { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let (anchor1, anchor2) = if flipped { ( - rb1.position * cparams.local_anchor2, - rb2.position * cparams.local_anchor1, + poss1.position * cparams.local_anchor2, + poss2.position * cparams.local_anchor1, ) } else { ( - rb1.position * cparams.local_anchor1, - rb2.position * cparams.local_anchor2, + poss1.position * cparams.local_anchor1, + poss2.position * cparams.local_anchor2, ) }; - let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r1 = anchor1.translation.vector - mprops1.world_com.coords; - let im2 = rb2.effective_inv_mass; - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r2 = anchor2.translation.vector - rb2.world_com.coords; + let im2 = mprops2.effective_inv_mass; + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r2 = anchor2.translation.vector - mprops2.world_com.coords; let rmat2 = r2.gcross_matrix(); #[allow(unused_mut)] // For 2D. @@ -310,8 +333,9 @@ impl FixedVelocityGroundConstraint { #[cfg(feature = "dim3")] let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); - let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); - let ang_dvel = rb2.angvel - rb1.angvel; + let lin_dvel = + vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1); + let ang_dvel = vels2.angvel - vels1.angvel; #[cfg(feature = "dim2")] let mut rhs = @@ -343,10 +367,10 @@ impl FixedVelocityGroundConstraint { FixedVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, ii2, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, r2, diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index b93bd3d..84e1fca 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -2,7 +2,8 @@ use simba::simd::SimdValue; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector, @@ -53,33 +54,46 @@ impl WFixedVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; - let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]); + let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -157,8 +171,7 @@ impl WFixedVelocityConstraint { #[cfg(feature = "dim3")] { - let ang_err = - Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]); rhs += Vector6::new( lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, ) * velocity_based_erp_inv_dt; @@ -185,20 +198,16 @@ impl WFixedVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -229,20 +238,16 @@ impl WFixedVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -326,33 +331,49 @@ impl WFixedVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&FixedJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); - let local_anchor1 = Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], - ); - let local_anchor2 = Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); + let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -423,8 +444,7 @@ impl WFixedVelocityGroundConstraint { #[cfg(feature = "dim3")] { - let ang_err = - Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]); rhs += Vector6::new( lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, ) * velocity_based_erp_inv_dt; @@ -446,12 +466,10 @@ impl WFixedVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -473,12 +491,10 @@ impl WFixedVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index be12258..76901b1 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -1,6 +1,6 @@ use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint}; use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody}; +use crate::dynamics::{GenericJoint, IntegrationParameters}; use crate::math::{ AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector, DIM, diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs index 9ceea67..d44c761 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs @@ -15,7 +15,11 @@ impl WGenericPositionConstraint { cparams: [&GenericJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| GenericPositionConstraint::from_params( + rbs1[ii], + rbs2[ii], + cparams[ii] + )], } } @@ -39,7 +43,12 @@ impl WGenericPositionGroundConstraint { flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + constraints: gather![|ii| GenericPositionGroundConstraint::from_params( + rbs1[ii], + rbs2[ii], + cparams[ii], + flipped[ii] + )], } } diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs index f391873..1eb04a9 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs @@ -50,8 +50,8 @@ impl GenericVelocityConstraint { rb1: &RigidBody, rb2: &RigidBody, ) -> SpatialVector { - let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1))) - + basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2))); + let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1))) + + basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2))); let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel) + basis2.inverse_transform_vector(&rb2.angvel); @@ -203,8 +203,8 @@ impl GenericVelocityConstraint { rb2: &RigidBody, joint: &GenericJoint, ) -> Self { - let anchor1 = rb1.position * joint.local_anchor1; - let anchor2 = rb2.position * joint.local_anchor2; + let anchor1 = rb1.position() * joint.local_anchor1; + let anchor2 = rb2.position() * joint.local_anchor2; let basis1 = anchor1.rotation; let basis2 = anchor2.rotation; let im1 = rb1.effective_inv_mass; @@ -405,13 +405,13 @@ impl GenericVelocityGroundConstraint { ) -> Self { let (anchor1, anchor2) = if flipped { ( - rb1.position * joint.local_anchor2, - rb2.position * joint.local_anchor1, + rb1.position() * joint.local_anchor2, + rb2.position() * joint.local_anchor1, ) } else { ( - rb1.position * joint.local_anchor1, - rb2.position * joint.local_anchor2, + rb1.position() * joint.local_anchor1, + rb2.position() * joint.local_anchor2, ) }; diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs index 8a6be8c..2751373 100644 --- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs @@ -57,29 +57,29 @@ impl WGenericVelocityConstraint { rbs2: [&RigidBody; SIMD_WIDTH], cparams: [&GenericJoint; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| rbs1[ii].position]); + let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]); + let angvel1 = AngVector::::from(gather![|ii| *rbs1[ii].angvel()]); + let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| rbs1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset]; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position2 = Isometry::from(gather![|ii| rbs2[ii].position]); + let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]); + let angvel2 = AngVector::::from(gather![|ii| *rbs2[ii].angvel()]); + let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| rbs2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset]; - let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]); + let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -160,20 +160,16 @@ impl WGenericVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -204,20 +200,16 @@ impl WGenericVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -306,28 +298,32 @@ impl WGenericVelocityGroundConstraint { cparams: [&GenericJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let position1 = Isometry::from(gather![|ii| rbs1[ii].position]); + let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]); + let angvel1 = AngVector::::from(gather![|ii| *rbs1[ii].angvel()]); + let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]); - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position2 = Isometry::from(gather![|ii| rbs2[ii].position]); + let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]); + let angvel2 = AngVector::::from(gather![|ii| *rbs2[ii].angvel()]); + let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| rbs2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset]; - let local_anchor1 = Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], - ); - let local_anchor2 = Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); + let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -395,12 +391,10 @@ impl WGenericVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); @@ -422,12 +416,10 @@ impl WGenericVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2: DeltaVel = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs index ed27e51..d723387 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -16,9 +16,11 @@ use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint}; // use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{ // GenericVelocityConstraint, GenericVelocityGroundConstraint, // }; +use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, + IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, }; use crate::math::Real; #[cfg(feature = "simd-is-enabled")] @@ -69,14 +71,30 @@ impl AnyJointVelocityConstraint { 1 } - pub fn from_joint( + pub fn from_joint( params: &IntegrationParameters, joint_id: JointIndex, joint: &Joint, - bodies: &RigidBodySet, - ) -> Self { - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; + bodies: &Bodies, + ) -> Self + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let rb1 = ( + bodies.index(joint.body1.0), + bodies.index(joint.body1.0), + bodies.index(joint.body1.0), + bodies.index(joint.body1.0), + ); + let rb2 = ( + bodies.index(joint.body2.0), + bodies.index(joint.body2.0), + bodies.index(joint.body2.0), + bodies.index(joint.body2.0), + ); match &joint.params { JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint( @@ -99,45 +117,59 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint( + pub fn from_wide_joint( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], joints: [&Joint; SIMD_WIDTH], - bodies: &RigidBodySet, - ) -> Self { - let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + bodies: &Bodies, + ) -> Self + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let rbs1 = ( + gather![|ii| bodies.index(joints[ii].body1.0)], + gather![|ii| bodies.index(joints[ii].body1.0)], + gather![|ii| bodies.index(joints[ii].body1.0)], + gather![|ii| bodies.index(joints[ii].body1.0)], + ); + let rbs2 = ( + gather![|ii| bodies.index(joints[ii].body2.0)], + gather![|ii| bodies.index(joints[ii].body2.0)], + gather![|ii| bodies.index(joints[ii].body2.0)], + gather![|ii| bodies.index(joints[ii].body2.0)], + ); match &joints[0].params { JointParams::BallJoint(_) => { - let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()]; AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params( params, joint_id, rbs1, rbs2, joints, )) } JointParams::FixedJoint(_) => { - let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()]; AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params( params, joint_id, rbs1, rbs2, joints, )) } // JointParams::GenericJoint(_) => { - // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()]; // AnyJointVelocityConstraint::WGenericConstraint( // WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), // ) // } JointParams::PrismaticJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()]; AnyJointVelocityConstraint::WPrismaticConstraint( WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), ) } #[cfg(feature = "dim3")] JointParams::RevoluteJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()]; AnyJointVelocityConstraint::WRevoluteConstraint( WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), ) @@ -145,20 +177,31 @@ impl AnyJointVelocityConstraint { } } - pub fn from_joint_ground( + pub fn from_joint_ground( params: &IntegrationParameters, joint_id: JointIndex, joint: &Joint, - bodies: &RigidBodySet, - ) -> Self { - let mut rb1 = &bodies[joint.body1]; - let mut rb2 = &bodies[joint.body2]; - let flipped = !rb2.is_dynamic(); + bodies: &Bodies, + ) -> Self + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let mut handle1 = joint.body1; + let mut handle2 = joint.body2; + let status2: &RigidBodyType = bodies.index(handle2.0); + let flipped = !status2.is_dynamic(); if flipped { - std::mem::swap(&mut rb1, &mut rb2); + std::mem::swap(&mut handle1, &mut handle2); } + let rb1 = bodies.index_bundle(handle1.0); + let rb2 = bodies.index_bundle(handle2.0); + match &joint.params { JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint( BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped), @@ -186,26 +229,46 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint_ground( + pub fn from_wide_joint_ground( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], joints: [&Joint; SIMD_WIDTH], - bodies: &RigidBodySet, - ) -> Self { - let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + bodies: &Bodies, + ) -> Self + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let mut handles1 = gather![|ii| joints[ii].body1]; + let mut handles2 = gather![|ii| joints[ii].body2]; + let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { - if !rbs2[ii].is_dynamic() { - std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + if !status2[ii].is_dynamic() { + std::mem::swap(&mut handles1[ii], &mut handles2[ii]); flipped[ii] = true; } } + let rbs1 = ( + gather![|ii| bodies.index(handles1[ii].0)], + gather![|ii| bodies.index(handles1[ii].0)], + gather![|ii| bodies.index(handles1[ii].0)], + ); + let rbs2 = ( + 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)], + ); + match &joints[0].params { JointParams::BallJoint(_) => { - let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()]; AnyJointVelocityConstraint::WBallGroundConstraint( WBallVelocityGroundConstraint::from_params( params, joint_id, rbs1, rbs2, joints, flipped, @@ -213,7 +276,7 @@ impl AnyJointVelocityConstraint { ) } JointParams::FixedJoint(_) => { - let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()]; AnyJointVelocityConstraint::WFixedGroundConstraint( WFixedVelocityGroundConstraint::from_params( params, joint_id, rbs1, rbs2, joints, flipped, @@ -221,7 +284,7 @@ impl AnyJointVelocityConstraint { ) } // JointParams::GenericJoint(_) => { - // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()]; // AnyJointVelocityConstraint::WGenericGroundConstraint( // WGenericVelocityGroundConstraint::from_params( // params, joint_id, rbs1, rbs2, joints, flipped, @@ -229,8 +292,7 @@ impl AnyJointVelocityConstraint { // ) // } JointParams::PrismaticJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()]; AnyJointVelocityConstraint::WPrismaticGroundConstraint( WPrismaticVelocityGroundConstraint::from_params( params, joint_id, rbs1, rbs2, joints, flipped, @@ -239,8 +301,7 @@ impl AnyJointVelocityConstraint { } #[cfg(feature = "dim3")] JointParams::RevoluteJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()]; AnyJointVelocityConstraint::WRevoluteGroundConstraint( WRevoluteVelocityGroundConstraint::from_params( params, joint_id, rbs1, rbs2, joints, flipped, diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 635e0b1..56e19fc 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -13,7 +13,11 @@ use super::{ WFixedPositionGroundConstraint, WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint, }; -use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; +use crate::data::{BundleSet, ComponentSet}; +use crate::dynamics::{ + IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, + RigidBodyType, +}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; use crate::math::{Isometry, Real}; @@ -56,9 +60,12 @@ pub(crate) enum AnyJointPositionConstraint { } impl AnyJointPositionConstraint { - pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self { - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; + pub fn from_joint(joint: &Joint, bodies: &Bodies) -> Self + where + Bodies: ComponentSet + ComponentSet, + { + let rb1 = bodies.index_bundle(joint.body1.0); + let rb2 = bodies.index_bundle(joint.body2.0); match &joint.params { JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint( @@ -81,40 +88,47 @@ impl AnyJointPositionConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self { - let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self + where + Bodies: ComponentSet + ComponentSet, + { + let rbs1 = ( + gather![|ii| bodies.index(joints[ii].body1.0)], + gather![|ii| bodies.index(joints[ii].body1.0)], + ); + let rbs2 = ( + gather![|ii| bodies.index(joints[ii].body2.0)], + gather![|ii| bodies.index(joints[ii].body2.0)], + ); match &joints[0].params { JointParams::BallJoint(_) => { - let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()]; AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params( rbs1, rbs2, joints, )) } JointParams::FixedJoint(_) => { - let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()]; AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params( rbs1, rbs2, joints, )) } // JointParams::GenericJoint(_) => { - // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()]; // AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params( // rbs1, rbs2, joints, // )) // } JointParams::PrismaticJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()]; AnyJointPositionConstraint::WPrismaticJoint( WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints), ) } #[cfg(feature = "dim3")] JointParams::RevoluteJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()]; AnyJointPositionConstraint::WRevoluteJoint( WRevolutePositionConstraint::from_params(rbs1, rbs2, joints), ) @@ -122,15 +136,26 @@ impl AnyJointPositionConstraint { } } - pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self { - let mut rb1 = &bodies[joint.body1]; - let mut rb2 = &bodies[joint.body2]; - let flipped = !rb2.is_dynamic(); + pub fn from_joint_ground(joint: &Joint, bodies: &Bodies) -> Self + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let mut handle1 = joint.body1; + let mut handle2 = joint.body2; + + let status2: &RigidBodyType = bodies.index(handle2.0); + let flipped = !status2.is_dynamic(); if flipped { - std::mem::swap(&mut rb1, &mut rb2); + std::mem::swap(&mut handle1, &mut handle2); } + let rb1 = bodies.index(handle1.0); + let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0)); + match &joint.params { JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint( BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped), @@ -154,48 +179,60 @@ impl AnyJointPositionConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self { - let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self + where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + let mut handles1 = gather![|ii| joints[ii].body1]; + let mut handles2 = gather![|ii| joints[ii].body2]; + let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; + let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { - if !rbs2[ii].is_dynamic() { - std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + if !status2[ii].is_dynamic() { + std::mem::swap(&mut handles1[ii], &mut handles2[ii]); flipped[ii] = true; } } + let rbs1 = gather![|ii| bodies.index(handles1[ii].0)]; + let rbs2 = ( + gather![|ii| bodies.index(handles2[ii].0)], + gather![|ii| bodies.index(handles2[ii].0)], + ); + match &joints[0].params { JointParams::BallJoint(_) => { - let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()]; AnyJointPositionConstraint::WBallGroundConstraint( WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) } JointParams::FixedJoint(_) => { - let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()]; AnyJointPositionConstraint::WFixedGroundConstraint( WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) } // JointParams::GenericJoint(_) => { - // let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH]; + // let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()]; // AnyJointPositionConstraint::WGenericGroundConstraint( // WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), // ) // } JointParams::PrismaticJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()]; AnyJointPositionConstraint::WPrismaticGroundConstraint( WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) } #[cfg(feature = "dim3")] JointParams::RevoluteJoint(_) => { - let joints = - array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()]; AnyJointPositionConstraint::WRevoluteGroundConstraint( WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), ) diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index ed52a52..d68f7ef 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody}; +use crate::dynamics::{ + IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::WAngularInertia; use na::Unit; @@ -27,11 +29,18 @@ pub(crate) struct PrismaticPositionConstraint { } impl PrismaticPositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &PrismaticJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); @@ -46,8 +55,8 @@ impl PrismaticPositionConstraint { local_frame2: cparams.local_frame2(), local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, limits: cparams.limits, } } @@ -108,25 +117,28 @@ pub(crate) struct PrismaticPositionGroundConstraint { impl PrismaticPositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &PrismaticJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (_, ids2) = rb2; + let frame1; let local_frame2; let axis1; let local_axis2; if flipped { - frame1 = rb1.next_position * cparams.local_frame2(); + frame1 = poss1.next_position * cparams.local_frame2(); local_frame2 = cparams.local_frame1(); - axis1 = rb1.next_position * cparams.local_axis2; + axis1 = poss1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; } else { - frame1 = rb1.next_position * cparams.local_frame1(); + frame1 = poss1.next_position * cparams.local_frame1(); local_frame2 = cparams.local_frame2(); - axis1 = rb1.next_position * cparams.local_axis1; + axis1 = poss1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; }; @@ -135,7 +147,7 @@ impl PrismaticPositionGroundConstraint { local_frame2, axis1, local_axis2, - position2: rb2.active_set_offset, + position2: ids2.active_set_offset, limits: cparams.limits, } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs index b64006a..95c1bcb 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs @@ -1,5 +1,7 @@ use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint}; -use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody}; +use crate::dynamics::{ + IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{Isometry, Real, SIMD_WIDTH}; // TODO: this does not uses SIMD optimizations yet. @@ -10,12 +12,22 @@ pub(crate) struct WPrismaticPositionConstraint { impl WPrismaticPositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&PrismaticJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| PrismaticPositionConstraint::from_params( + (rbs1.0[ii], rbs1.1[ii]), + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii] + )], } } @@ -33,13 +45,21 @@ pub(crate) struct WPrismaticPositionGroundConstraint { impl WPrismaticPositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&PrismaticJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params( + rbs1[ii], + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii], + flipped[ii] + )], } } diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 9d64193..1e7570e 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngularInertia, Real, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot}; @@ -74,32 +75,45 @@ impl PrismaticVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &PrismaticJoint, ) -> Self { + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + // Linear part. - let anchor1 = rb1.position * joint.local_anchor1; - let anchor2 = rb2.position * joint.local_anchor2; - let axis1 = rb1.position * joint.local_axis1; - let axis2 = rb2.position * joint.local_axis2; + let anchor1 = poss1.position * joint.local_anchor1; + let anchor2 = poss2.position * joint.local_anchor2; + let axis1 = poss1.position * joint.local_axis1; + let axis2 = poss2.position * joint.local_axis2; #[cfg(feature = "dim2")] - let basis1 = rb1.position * joint.basis1[0]; + let basis1 = poss1.position * joint.basis1[0]; #[cfg(feature = "dim3")] let basis1 = Matrix3x2::from_columns(&[ - rb1.position * joint.basis1[0], - rb1.position * joint.basis1[1], + poss1.position * joint.basis1[0], + poss1.position * joint.basis1[1], ]); - let im1 = rb1.effective_inv_mass; - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1 - rb1.world_com; + let im1 = mprops1.effective_inv_mass; + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - mprops1.world_com; let r1_mat = r1.gcross_matrix(); - let im2 = rb2.effective_inv_mass; - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r2 = anchor2 - rb2.world_com; + let im2 = mprops2.effective_inv_mass; + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r2 = anchor2 - mprops2.world_com; let r2_mat = r2.gcross_matrix(); #[allow(unused_mut)] // For 2D. @@ -131,8 +145,8 @@ impl PrismaticVelocityConstraint { lhs = SdpMatrix2::new(m11, m12, m22); } - let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1); - let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2); + let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1); + let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2); // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix // for which a textbook inverse is still efficient. @@ -142,7 +156,7 @@ impl PrismaticVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); - let angvel_err = rb2.angvel - rb1.angvel; + let angvel_err = vels2.angvel - vels1.angvel; #[cfg(feature = "dim2")] let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction; @@ -159,8 +173,8 @@ impl PrismaticVelocityConstraint { if velocity_based_erp_inv_dt != 0.0 { let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); - let frame1 = rb1.position * joint.local_frame1(); - let frame2 = rb2.position * joint.local_frame2(); + let frame1 = poss1.position * joint.local_frame1(); + let frame2 = poss2.position * joint.local_frame2(); let ang_err = frame2.rotation * frame1.rotation.inverse(); #[cfg(feature = "dim2")] @@ -195,9 +209,9 @@ impl PrismaticVelocityConstraint { } if damping != 0.0 { - let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.gdot(gcross2) - - rb1.linvel.dot(&axis1) - - rb1.angvel.gdot(gcross1); + let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2) + - vels1.linvel.dot(&axis1) + - vels1.angvel.gdot(gcross1); motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } @@ -266,12 +280,12 @@ impl PrismaticVelocityConstraint { PrismaticVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, im2, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: joint.impulse * params.warmstart_coeff, limits_active, limits_impulse: limits_impulse * params.warmstart_coeff, @@ -501,11 +515,19 @@ impl PrismaticVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &PrismaticJoint, flipped: bool, ) -> Self { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let anchor2; let anchor1; let axis2; @@ -513,35 +535,35 @@ impl PrismaticVelocityGroundConstraint { let basis1; if flipped { - anchor2 = rb2.position * joint.local_anchor1; - anchor1 = rb1.position * joint.local_anchor2; - axis2 = rb2.position * joint.local_axis1; - axis1 = rb1.position * joint.local_axis2; + anchor2 = poss2.position * joint.local_anchor1; + anchor1 = poss1.position * joint.local_anchor2; + axis2 = poss2.position * joint.local_axis1; + axis1 = poss1.position * joint.local_axis2; #[cfg(feature = "dim2")] { - basis1 = rb1.position * joint.basis2[0]; + basis1 = poss1.position * joint.basis2[0]; } #[cfg(feature = "dim3")] { basis1 = Matrix3x2::from_columns(&[ - rb1.position * joint.basis2[0], - rb1.position * joint.basis2[1], + poss1.position * joint.basis2[0], + poss1.position * joint.basis2[1], ]); } } else { - anchor2 = rb2.position * joint.local_anchor2; - anchor1 = rb1.position * joint.local_anchor1; - axis2 = rb2.position * joint.local_axis2; - axis1 = rb1.position * joint.local_axis1; + anchor2 = poss2.position * joint.local_anchor2; + anchor1 = poss1.position * joint.local_anchor1; + axis2 = poss2.position * joint.local_axis2; + axis1 = poss1.position * joint.local_axis1; #[cfg(feature = "dim2")] { - basis1 = rb1.position * joint.basis1[0]; + basis1 = poss1.position * joint.basis1[0]; } #[cfg(feature = "dim3")] { basis1 = Matrix3x2::from_columns(&[ - rb1.position * joint.basis1[0], - rb1.position * joint.basis1[1], + poss1.position * joint.basis1[0], + poss1.position * joint.basis1[1], ]); } }; @@ -560,10 +582,10 @@ impl PrismaticVelocityGroundConstraint { // simplifications of the computation without introducing // much instabilities. - let im2 = rb2.effective_inv_mass; - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1 - rb1.world_com; - let r2 = anchor2 - rb2.world_com; + let im2 = mprops2.effective_inv_mass; + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - mprops1.world_com; + let r2 = anchor2 - mprops2.world_com; let r2_mat = r2.gcross_matrix(); #[allow(unused_mut)] // For 2D. @@ -592,8 +614,8 @@ impl PrismaticVelocityGroundConstraint { lhs = SdpMatrix2::new(m11, m12, m22); } - let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1); - let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2); + let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1); + let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2); // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix // for which a textbook inverse is still efficient. @@ -603,7 +625,7 @@ impl PrismaticVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); - let angvel_err = rb2.angvel - rb1.angvel; + let angvel_err = vels2.angvel - vels1.angvel; #[cfg(feature = "dim2")] let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction; @@ -622,11 +644,11 @@ impl PrismaticVelocityGroundConstraint { let (frame1, frame2); if flipped { - frame1 = rb1.position * joint.local_frame2(); - frame2 = rb2.position * joint.local_frame1(); + frame1 = poss1.position * joint.local_frame2(); + frame2 = poss2.position * joint.local_frame1(); } else { - frame1 = rb1.position * joint.local_frame1(); - frame2 = rb2.position * joint.local_frame2(); + frame1 = poss1.position * joint.local_frame1(); + frame2 = poss2.position * joint.local_frame2(); } let ang_err = frame2.rotation * frame1.rotation.inverse(); @@ -660,7 +682,7 @@ impl PrismaticVelocityGroundConstraint { } if damping != 0.0 { - let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1); + let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1); motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } @@ -714,9 +736,9 @@ impl PrismaticVelocityGroundConstraint { PrismaticVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: joint.impulse * params.warmstart_coeff, limits_active, limits_forcedir2, diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index a60f5ca..984adcb 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -2,7 +2,8 @@ use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH, @@ -71,47 +72,60 @@ impl WPrismaticVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&PrismaticJoint; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; - let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); - let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]); - let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]); + let local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]); + let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]); #[cfg(feature = "dim2")] - let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])]; + let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])]; #[cfg(feature = "dim3")] let local_basis1 = [ - Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]), - Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]), + Vector::from(gather![|ii| cparams[ii].basis1[0]]), + Vector::from(gather![|ii| cparams[ii].basis1[1]]), ]; #[cfg(feature = "dim2")] - let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]); #[cfg(feature = "dim3")] - let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -207,8 +221,8 @@ impl WPrismaticVelocityConstraint { let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); - let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]); - let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]); + let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]); + let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]); let frame1 = position1 * local_frame1; let frame2 = position2 * local_frame2; @@ -221,8 +235,7 @@ impl WPrismaticVelocityConstraint { #[cfg(feature = "dim3")] { - let ang_err = - Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]); rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z) * velocity_based_erp_inv_dt; } @@ -237,15 +250,15 @@ impl WPrismaticVelocityConstraint { let mut limits_inv_lhs = zero; let mut limits_impulse_limits = (zero, zero); - let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); + let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]); if limits_enabled.any() { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); // TODO: we should allow predictive constraint activation. - let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]); + let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]); let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); @@ -265,10 +278,9 @@ impl WPrismaticVelocityConstraint { - (min_limit - dist).simd_max(zero)) * SimdReal::splat(velocity_based_erp_inv_dt); - limits_impulse = - SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]) - .simd_max(limits_impulse_limits.0) - .simd_min(limits_impulse_limits.1); + limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse]) + .simd_max(limits_impulse_limits.0) + .simd_min(limits_impulse_limits.1); limits_inv_lhs = SimdReal::splat(1.0) / (im1 @@ -303,20 +315,16 @@ impl WPrismaticVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); @@ -428,20 +436,16 @@ impl WPrismaticVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2); @@ -510,59 +514,85 @@ impl WPrismaticVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&PrismaticJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; #[cfg(feature = "dim2")] - let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]); #[cfg(feature = "dim3")] - let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]); - let local_anchor1 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], - ); - let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], - ); - let local_axis1 = Vector::from( - array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH], - ); - let local_axis2 = Vector::from( - array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH], - ); + let local_anchor1 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }]); + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }]); + let local_axis1 = Vector::from(gather![|ii| if flipped[ii] { + *cparams[ii].local_axis2 + } else { + *cparams[ii].local_axis1 + }]); + let local_axis2 = Vector::from(gather![|ii| if flipped[ii] { + *cparams[ii].local_axis1 + } else { + *cparams[ii].local_axis2 + }]); #[cfg(feature = "dim2")] let basis1 = position1 - * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], - ); + * Vector::from(gather![|ii| if flipped[ii] { + cparams[ii].basis2[0] + } else { + cparams[ii].basis1[0] + }]); #[cfg(feature = "dim3")] let basis1 = Matrix3x2::from_columns(&[ position1 - * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], - ), + * Vector::from(gather![|ii| if flipped[ii] { + cparams[ii].basis2[0] + } else { + cparams[ii].basis1[0] + }]), position1 - * Vector::from( - array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH], - ), + * Vector::from(gather![|ii| if flipped[ii] { + cparams[ii].basis2[1] + } else { + cparams[ii].basis1[1] + }]), ]); let anchor1 = position1 * local_anchor1; @@ -634,13 +664,17 @@ impl WPrismaticVelocityGroundConstraint { let linear_err = basis1.tr_mul(&(anchor2 - anchor1)); let frame1 = position1 - * Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH], - ); + * Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_frame2() + } else { + cparams[ii].local_frame1() + }]); let frame2 = position2 - * Isometry::from( - array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH], - ); + * Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_frame1() + } else { + cparams[ii].local_frame2() + }]); let ang_err = frame2.rotation * frame1.rotation.inverse(); @@ -651,8 +685,7 @@ impl WPrismaticVelocityGroundConstraint { #[cfg(feature = "dim3")] { - let ang_err = - Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]); + let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]); rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z) * velocity_based_erp_inv_dt; } @@ -666,14 +699,14 @@ impl WPrismaticVelocityGroundConstraint { let mut limits_impulse = zero; let mut limits_impulse_limits = (zero, zero); - let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); + let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]); if limits_enabled.any() { let danchor = anchor2 - anchor1; let dist = danchor.dot(&axis1); // TODO: we should allow predictive constraint activation. - let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); - let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]); + let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]); let min_enabled = dist.simd_lt(min_limit); let max_enabled = dist.simd_gt(max_limit); @@ -690,10 +723,9 @@ impl WPrismaticVelocityGroundConstraint { - (min_limit - dist).simd_max(zero)) * SimdReal::splat(velocity_based_erp_inv_dt); - limits_impulse = - SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]) - .simd_max(limits_impulse_limits.0) - .simd_min(limits_impulse_limits.1); + limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse]) + .simd_max(limits_impulse_limits.0) + .simd_min(limits_impulse_limits.1); } } @@ -718,12 +750,10 @@ impl WPrismaticVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); @@ -791,12 +821,10 @@ impl WPrismaticVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; self.solve_dofs(&mut mj_lambda2); diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 96391a2..1b3e23a 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; +use crate::dynamics::{ + IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::Unit; @@ -29,11 +31,18 @@ pub(crate) struct RevolutePositionConstraint { } impl RevolutePositionConstraint { - pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self { - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + pub fn from_params( + rb1: (&RigidBodyMassProps, &RigidBodyIds), + rb2: (&RigidBodyMassProps, &RigidBodyIds), + cparams: &RevoluteJoint, + ) -> Self { + let (mprops1, ids1) = rb1; + let (mprops2, ids2) = rb2; + + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; let ang_inv_lhs = (ii1 + ii2).inverse(); Self { @@ -42,14 +51,14 @@ impl RevolutePositionConstraint { ii1, ii2, ang_inv_lhs, - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, + local_com1: mprops1.mass_properties.local_com, + local_com2: mprops2.mass_properties.local_com, local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, local_axis2: cparams.local_axis2, - position1: rb1.active_set_offset, - position2: rb2.active_set_offset, + position1: ids1.active_set_offset, + position2: ids2.active_set_offset, local_basis1: cparams.basis1, local_basis2: cparams.basis2, } @@ -132,11 +141,14 @@ pub(crate) struct RevolutePositionGroundConstraint { impl RevolutePositionGroundConstraint { pub fn from_params( - rb1: &RigidBody, - rb2: &RigidBody, + rb1: &RigidBodyPosition, + rb2: (&RigidBodyMassProps, &RigidBodyIds), cparams: &RevoluteJoint, flipped: bool, ) -> Self { + let poss1 = rb1; + let (mprops2, ids2) = rb2; + let anchor1; let local_anchor2; let axis1; @@ -145,23 +157,23 @@ impl RevolutePositionGroundConstraint { let local_basis2; if flipped { - anchor1 = rb1.next_position * cparams.local_anchor2; + anchor1 = poss1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; - axis1 = rb1.next_position * cparams.local_axis2; + axis1 = poss1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; basis1 = [ - rb1.next_position * cparams.basis2[0], - rb1.next_position * cparams.basis2[1], + poss1.next_position * cparams.basis2[0], + poss1.next_position * cparams.basis2[1], ]; local_basis2 = cparams.basis1; } else { - anchor1 = rb1.next_position * cparams.local_anchor1; + anchor1 = poss1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; - axis1 = rb1.next_position * cparams.local_axis1; + axis1 = poss1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; basis1 = [ - rb1.next_position * cparams.basis1[0], - rb1.next_position * cparams.basis1[1], + poss1.next_position * cparams.basis1[0], + poss1.next_position * cparams.basis1[1], ]; local_basis2 = cparams.basis2; }; @@ -169,12 +181,12 @@ impl RevolutePositionGroundConstraint { Self { anchor1, local_anchor2, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), - local_com2: rb2.mass_properties.local_com, + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), + local_com2: mprops2.mass_properties.local_com, axis1, local_axis2, - position2: rb2.active_set_offset, + position2: ids2.active_set_offset, basis1, local_basis2, } diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs index e2d140d..5881cb3 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs @@ -1,5 +1,7 @@ use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; -use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; +use crate::dynamics::{ + IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, +}; use crate::math::{Isometry, Real, SIMD_WIDTH}; // TODO: this does not uses SIMD optimizations yet. @@ -10,12 +12,22 @@ pub(crate) struct WRevolutePositionConstraint { impl WRevolutePositionConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&RevoluteJoint; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + constraints: gather![|ii| RevolutePositionConstraint::from_params( + (rbs1.0[ii], rbs1.1[ii]), + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii] + )], } } @@ -33,13 +45,21 @@ pub(crate) struct WRevolutePositionGroundConstraint { impl WRevolutePositionGroundConstraint { pub fn from_params( - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: [&RigidBodyPosition; SIMD_WIDTH], + rbs2: ( + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), cparams: [&RevoluteJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { Self { - constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + constraints: gather![|ii| RevolutePositionGroundConstraint::from_params( + rbs1[ii], + (rbs2.0[ii], rbs2.1[ii]), + cparams[ii], + flipped[ii] + )], } } diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 61a55d3..bf45c11 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -1,6 +1,7 @@ use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel}; use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{AngularInertia, Real, Rotation, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; @@ -43,34 +44,47 @@ impl RevoluteVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &RevoluteJoint, ) -> Self { + let (poss1, vels1, mprops1, ids1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + // Linear part. - let anchor1 = rb1.position * joint.local_anchor1; - let anchor2 = rb2.position * joint.local_anchor2; + let anchor1 = poss1.position * joint.local_anchor1; + let anchor2 = poss2.position * joint.local_anchor2; let basis1 = Matrix3x2::from_columns(&[ - rb1.position * joint.basis1[0], - rb1.position * joint.basis1[1], + poss1.position * joint.basis1[0], + poss1.position * joint.basis1[1], ]); let basis2 = Matrix3x2::from_columns(&[ - rb2.position * joint.basis2[0], - rb2.position * joint.basis2[1], + poss2.position * joint.basis2[0], + poss2.position * joint.basis2[1], ]); let basis_projection2 = basis2 * basis2.transpose(); let basis2 = basis_projection2 * basis1; - let im1 = rb1.effective_inv_mass; - let im2 = rb2.effective_inv_mass; + let im1 = mprops1.effective_inv_mass; + let im2 = mprops2.effective_inv_mass; - let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1 - rb1.world_com; + let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - mprops1.world_com; let r1_mat = r1.gcross_matrix(); - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r2 = anchor2 - rb2.world_com; + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r2 = anchor2 - mprops2.world_com; let r2_mat = r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); @@ -90,8 +104,8 @@ impl RevoluteVelocityConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let linvel_err = - (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); - let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + (vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1)); + let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel); let mut rhs = Vector5::new( linvel_err.x, @@ -105,8 +119,8 @@ impl RevoluteVelocityConstraint { if velocity_based_erp_inv_dt != 0.0 { let lin_err = anchor2 - anchor1; - let axis1 = rb1.position * joint.local_axis1; - let axis2 = rb2.position * joint.local_axis2; + let axis1 = poss1.position * joint.local_axis1; + let axis2 = poss2.position * joint.local_axis2; let axis_error = axis1.cross(&axis2); let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5; @@ -118,8 +132,8 @@ impl RevoluteVelocityConstraint { /* * Motor. */ - let motor_axis1 = rb1.position * *joint.local_axis1; - let motor_axis2 = rb2.position * *joint.local_axis2; + let motor_axis1 = poss1.position * *joint.local_axis1; + let motor_axis2 = poss2.position * *joint.local_axis2; let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; let mut motor_angle = 0.0; @@ -132,12 +146,12 @@ impl RevoluteVelocityConstraint { ); if stiffness != 0.0 { - motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position); + motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position); motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness; } if damping != 0.0 { - let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1); + let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1); motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } @@ -171,14 +185,14 @@ impl RevoluteVelocityConstraint { RevoluteVelocityConstraint { joint_id, - mj_lambda1: rb1.active_set_offset, - mj_lambda2: rb2.active_set_offset, + mj_lambda1: ids1.active_set_offset, + mj_lambda2: ids2.active_set_offset, im1, - ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt, basis1, basis2, im2, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse, inv_lhs, rhs, @@ -330,11 +344,19 @@ impl RevoluteVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: JointIndex, - rb1: &RigidBody, - rb2: &RigidBody, + rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps), + rb2: ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyMassProps, + &RigidBodyIds, + ), joint: &RevoluteJoint, flipped: bool, ) -> AnyJointVelocityConstraint { + let (poss1, vels1, mprops1) = rb1; + let (poss2, vels2, mprops2, ids2) = rb2; + let anchor2; let anchor1; let axis1; @@ -343,39 +365,39 @@ impl RevoluteVelocityGroundConstraint { let basis2; if flipped { - axis1 = rb1.position * *joint.local_axis2; - axis2 = rb2.position * *joint.local_axis1; - anchor1 = rb1.position * joint.local_anchor2; - anchor2 = rb2.position * joint.local_anchor1; + axis1 = poss1.position * *joint.local_axis2; + axis2 = poss2.position * *joint.local_axis1; + anchor1 = poss1.position * joint.local_anchor2; + anchor2 = poss2.position * joint.local_anchor1; basis1 = Matrix3x2::from_columns(&[ - rb1.position * joint.basis2[0], - rb1.position * joint.basis2[1], + poss1.position * joint.basis2[0], + poss1.position * joint.basis2[1], ]); basis2 = Matrix3x2::from_columns(&[ - rb2.position * joint.basis1[0], - rb2.position * joint.basis1[1], + poss2.position * joint.basis1[0], + poss2.position * joint.basis1[1], ]); } else { - axis1 = rb1.position * *joint.local_axis1; - axis2 = rb2.position * *joint.local_axis2; - anchor1 = rb1.position * joint.local_anchor1; - anchor2 = rb2.position * joint.local_anchor2; + axis1 = poss1.position * *joint.local_axis1; + axis2 = poss2.position * *joint.local_axis2; + anchor1 = poss1.position * joint.local_anchor1; + anchor2 = poss2.position * joint.local_anchor2; basis1 = Matrix3x2::from_columns(&[ - rb1.position * joint.basis1[0], - rb1.position * joint.basis1[1], + poss1.position * joint.basis1[0], + poss1.position * joint.basis1[1], ]); basis2 = Matrix3x2::from_columns(&[ - rb2.position * joint.basis2[0], - rb2.position * joint.basis2[1], + poss2.position * joint.basis2[0], + poss2.position * joint.basis2[1], ]); }; let basis_projection2 = basis2 * basis2.transpose(); let basis2 = basis_projection2 * basis1; - let im2 = rb2.effective_inv_mass; - let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); - let r1 = anchor1 - rb1.world_com; - let r2 = anchor2 - rb2.world_com; + let im2 = mprops2.effective_inv_mass; + let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - mprops1.world_com; + let r2 = anchor2 - mprops2.world_com; let r2_mat = r2.gcross_matrix(); let mut lhs = Matrix5::zeros(); @@ -393,8 +415,8 @@ impl RevoluteVelocityGroundConstraint { let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); let linvel_err = - (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1)); - let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel); + (vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1)); + let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel); let mut rhs = Vector5::new( linvel_err.x, linvel_err.y, @@ -409,11 +431,11 @@ impl RevoluteVelocityGroundConstraint { let (axis1, axis2); if flipped { - axis1 = rb1.position * joint.local_axis2; - axis2 = rb2.position * joint.local_axis1; + axis1 = poss1.position * joint.local_axis2; + axis2 = poss2.position * joint.local_axis1; } else { - axis1 = rb1.position * joint.local_axis1; - axis2 = rb2.position * joint.local_axis2; + axis1 = poss1.position * joint.local_axis1; + axis2 = poss2.position * joint.local_axis2; } let axis_error = axis1.cross(&axis2); let ang_err = basis2.tr_mul(&axis_error); @@ -437,12 +459,12 @@ impl RevoluteVelocityGroundConstraint { ); if stiffness != 0.0 { - motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position); + motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position); motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness; } if damping != 0.0 { - let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1); + let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1); motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } @@ -460,9 +482,9 @@ impl RevoluteVelocityGroundConstraint { let result = RevoluteVelocityGroundConstraint { joint_id, - mj_lambda2: rb2.active_set_offset, + mj_lambda2: ids2.active_set_offset, im2, - ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, + ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt, impulse: joint.impulse * params.warmstart_coeff, basis2, inv_lhs, diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 5fd72bd..6e65b76 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -2,7 +2,8 @@ use simba::simd::SimdValue; use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ - IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, }; use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH, @@ -39,41 +40,54 @@ impl WRevoluteVelocityConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), joints: [&RevoluteJoint; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let (poss1, vels1, mprops1, ids1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1_sqrt = AngularInertia::::from(gather![ + |ii| mprops1[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; - let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]); - let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + + let local_anchor1 = Point::from(gather![|ii| joints[ii].local_anchor1]); + let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]); let local_basis1 = [ - Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]), - Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]), + Vector::from(gather![|ii| joints[ii].basis1[0]]), + Vector::from(gather![|ii| joints[ii].basis1[1]]), ]; let local_basis2 = [ - Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]), - Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]), + Vector::from(gather![|ii| joints[ii].basis2[0]]), + Vector::from(gather![|ii| joints[ii].basis2[1]]), ]; - let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]); + let impulse = Vector5::from(gather![|ii| joints[ii].impulse]); let anchor1 = position1 * local_anchor1; let anchor2 = position2 * local_anchor2; @@ -124,10 +138,8 @@ impl WRevoluteVelocityConstraint { let lin_err = anchor2 - anchor1; - let local_axis1 = - Unit::>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]); - let local_axis2 = - Unit::>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]); + let local_axis1 = Unit::>::from(gather![|ii| joints[ii].local_axis1]); + let local_axis2 = Unit::>::from(gather![|ii| joints[ii].local_axis2]); let axis1 = position1 * local_axis1; let axis2 = position2 * local_axis2; @@ -150,12 +162,12 @@ impl WRevoluteVelocityConstraint { let warmstart_coeff = SimdReal::splat(params.warmstart_coeff); let mut impulse = impulse * warmstart_coeff; - let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH]; - let rotated_impulse = Vector::from(array![|ii| { + let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1]; + let rotated_impulse = Vector::from(gather![|ii| { let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii]) .unwrap_or_else(Rotation::identity); axis_rot * joints[ii].world_ang_impulse - }; SIMD_WIDTH]); + }]); let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse); impulse[3] = rotated_basis_impulse.x * warmstart_coeff; @@ -182,20 +194,16 @@ impl WRevoluteVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned(); @@ -225,20 +233,16 @@ impl WRevoluteVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); @@ -314,52 +318,76 @@ impl WRevoluteVelocityGroundConstraint { pub fn from_params( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], - rbs1: [&RigidBody; SIMD_WIDTH], - rbs2: [&RigidBody; SIMD_WIDTH], + rbs1: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + ), + rbs2: ( + [&RigidBodyPosition; SIMD_WIDTH], + [&RigidBodyVelocity; SIMD_WIDTH], + [&RigidBodyMassProps; SIMD_WIDTH], + [&RigidBodyIds; SIMD_WIDTH], + ), joints: [&RevoluteJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let (poss1, vels1, mprops1) = rbs1; + let (poss2, vels2, mprops2, ids2) = rbs2; - let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]); + let position1 = Isometry::from(gather![|ii| poss1[ii].position]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); - let local_anchor1 = Point::from( - array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH], - ); - let local_anchor2 = Point::from( - array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH], - ); + let position2 = Isometry::from(gather![|ii| poss2[ii].position]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2_sqrt = AngularInertia::::from(gather![ + |ii| mprops2[ii].effective_world_inv_inertia_sqrt + ]); + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; + let impulse = Vector5::from(gather![|ii| joints[ii].impulse]); + + let local_anchor1 = Point::from(gather![|ii| if flipped[ii] { + joints[ii].local_anchor2 + } else { + joints[ii].local_anchor1 + }]); + let local_anchor2 = Point::from(gather![|ii| if flipped[ii] { + joints[ii].local_anchor1 + } else { + joints[ii].local_anchor2 + }]); let basis1 = Matrix3x2::from_columns(&[ position1 - * Vector::from( - array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH], - ), + * Vector::from(gather![|ii| if flipped[ii] { + joints[ii].basis2[0] + } else { + joints[ii].basis1[0] + }]), position1 - * Vector::from( - array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH], - ), + * Vector::from(gather![|ii| if flipped[ii] { + joints[ii].basis2[1] + } else { + joints[ii].basis1[1] + }]), ]); let basis2 = Matrix3x2::from_columns(&[ position2 - * Vector::from( - array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH], - ), + * Vector::from(gather![|ii| if flipped[ii] { + joints[ii].basis1[0] + } else { + joints[ii].basis2[0] + }]), position2 - * Vector::from( - array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH], - ), + * Vector::from(gather![|ii| if flipped[ii] { + joints[ii].basis1[1] + } else { + joints[ii].basis2[1] + }]), ]); let basis_projection2 = basis2 * basis2.transpose(); let basis2 = basis_projection2 * basis1; @@ -403,12 +431,16 @@ impl WRevoluteVelocityGroundConstraint { let lin_err = anchor2 - anchor1; - let local_axis1 = Unit::>::from( - array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH], - ); - let local_axis2 = Unit::>::from( - array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH], - ); + let local_axis1 = Unit::>::from(gather![|ii| if flipped[ii] { + joints[ii].local_axis2 + } else { + joints[ii].local_axis1 + }]); + let local_axis2 = Unit::>::from(gather![|ii| if flipped[ii] { + joints[ii].local_axis1 + } else { + joints[ii].local_axis2 + }]); let axis1 = position1 * local_axis1; let axis2 = position2 * local_axis2; @@ -434,12 +466,10 @@ impl WRevoluteVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned(); @@ -458,12 +488,10 @@ impl WRevoluteVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index add5f2c..115b099 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -1,9 +1,10 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; +use crate::data::ComponentSet; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Isometry, Real}; use crate::utils::WAngularInertia; @@ -150,13 +151,15 @@ impl ParallelIslandSolver { } } - pub fn solve_position_constraints<'s>( + pub fn solve_position_constraints<'s, Bodies>( &'s mut self, scope: &Scope<'s>, island_id: usize, params: &'s IntegrationParameters, - bodies: &'s mut RigidBodySet, - ) { + bodies: &'s mut Bodies, + ) where + Bodies: ComponentSet, + { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? @@ -179,7 +182,7 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let positions: &mut Vec> = unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let parallel_contact_constraints: &mut ParallelSolverConstraints = unsafe { std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed)) @@ -197,8 +200,8 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { + let rb = &mut bodies.index(handle.0); positions[rb.active_set_offset] = rb.next_position; } } @@ -216,8 +219,8 @@ impl ParallelIslandSolver { // Write results back to rigid bodies. concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.position_writeback_index] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.position_writeback_index) { + let rb = &mut bodies.index(handle.0); rb.set_next_position(positions[rb.active_set_offset]); } } @@ -225,17 +228,19 @@ impl ParallelIslandSolver { } } - pub fn init_constraints_and_solve_velocity_constraints<'s>( + pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>( &'s mut self, scope: &Scope<'s>, island_id: usize, params: &'s IntegrationParameters, - bodies: &'s mut RigidBodySet, + bodies: &'s mut Bodies, manifolds: &'s mut Vec<&'s mut ContactManifold>, manifold_indices: &'s [ContactManifoldIndex], joints: &'s mut Vec, joint_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet, + { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? @@ -280,7 +285,7 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let mj_lambdas: &mut Vec> = unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let manifolds: &mut Vec<&mut ContactManifold> = unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; @@ -303,8 +308,8 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) { + let rb = &mut bodies.index(handle.0); let dvel = &mut mj_lambdas[rb.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied @@ -348,8 +353,8 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { + let rb = &mut bodies.index(handle.0); let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index fcdcbff..dd1a19f 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -5,7 +5,7 @@ use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, JointGraphEdge}; use crate::geometry::ContactManifold; #[cfg(feature = "simd-is-enabled")] use crate::{ @@ -20,7 +20,7 @@ use std::sync::atomic::Ordering; // pub fn init_constraint_groups( // &mut self, // island_id: usize, -// bodies: &RigidBodySet, +// bodies: &impl ComponentSet, // manifolds: &mut [&mut ContactManifold], // manifold_groups: &ParallelInteractionGroups, // joints: &mut [JointGraphEdge], @@ -36,9 +36,9 @@ pub(crate) enum ConstraintDesc { NongroundNongrouped(usize), GroundNongrouped(usize), #[cfg(feature = "simd-is-enabled")] - NongroundGrouped([usize; SIMD_WIDTH]), + NongroundGrouped([usize]), #[cfg(feature = "simd-is-enabled")] - GroundGrouped([usize; SIMD_WIDTH]), + GroundGrouped([usize]), } pub(crate) struct ParallelSolverConstraints { @@ -78,7 +78,7 @@ macro_rules! impl_init_constraints_group { pub fn init_constraint_groups( &mut self, island_id: usize, - bodies: &RigidBodySet, + bodies: &impl ComponentSet, interactions: &mut [$Interaction], interaction_groups: &ParallelInteractionGroups, ) { @@ -144,7 +144,7 @@ macro_rules! impl_init_constraints_group { self.constraint_descs.push(( total_num_constraints, ConstraintDesc::NongroundGrouped( - array![|ii| interaction_i[ii]; SIMD_WIDTH], + gather![|ii| interaction_i[ii]], ), )); total_num_constraints += $num_active_constraints(interaction); @@ -172,7 +172,7 @@ macro_rules! impl_init_constraints_group { self.constraint_descs.push(( total_num_constraints, ConstraintDesc::GroundGrouped( - array![|ii| interaction_i[ii]; SIMD_WIDTH], + gather![|ii| interaction_i[ii]], ), )); total_num_constraints += $num_active_constraints(interaction); @@ -223,7 +223,7 @@ impl ParallelSolverConstraints { &mut self, thread: &ThreadContext, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &impl ComponentSet, manifolds_all: &[&mut ContactManifold], ) { let descs = &self.constraint_descs; @@ -244,13 +244,13 @@ impl ParallelSolverConstraints { } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::NongroundGrouped(manifold_id) => { - let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false); WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::GroundGrouped(manifold_id) => { - let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false); WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false); } @@ -265,7 +265,7 @@ impl ParallelSolverConstraints, joints_all: &[JointGraphEdge], ) { let descs = &self.constraint_descs; @@ -290,7 +290,7 @@ impl ParallelSolverConstraints { - let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; + let joints = gather![|ii| &joints_all[joint_id[ii]].weight]; let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies); let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies); self.velocity_constraints[joints[0].constraint_index] = velocity_constraint; @@ -298,7 +298,7 @@ impl ParallelSolverConstraints { - let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; + let joints = gather![|ii| &joints_all[joint_id[ii]].weight]; let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies); let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies); self.velocity_constraints[joints[0].constraint_index] = velocity_constraint; diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 844b1cd..dca0655 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -1,7 +1,8 @@ +use crate::data::ComponentSet; use crate::dynamics::solver::PositionGroundConstraint; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition}; use crate::geometry::ContactManifold; use crate::math::{ AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, @@ -51,15 +52,26 @@ pub(crate) struct PositionConstraint { } impl PositionConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold: &ContactManifold, - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { - let rb1 = &bodies[manifold.data.body_pair.body1]; - let rb2 = &bodies[manifold.data.body_pair.body2]; + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { + let handle1 = manifold.data.rigid_body1.unwrap(); + let handle2 = manifold.data.rigid_body2.unwrap(); + + let ids1: &RigidBodyIds = bodies.index(handle1.0); + let ids2: &RigidBodyIds = bodies.index(handle2.0); + let poss1: &RigidBodyPosition = bodies.index(handle1.0); + let poss2: &RigidBodyPosition = bodies.index(handle2.0); + let mprops1: &RigidBodyMassProps = bodies.index(handle1.0); + let mprops2: &RigidBodyMassProps = bodies.index(handle2.0); for (l, manifold_points) in manifold .data @@ -72,26 +84,28 @@ impl PositionConstraint { let mut dists = [0.0; MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = rb1 + local_p1[l] = poss1 .position .inverse_transform_point(&manifold_points[l].point); - local_p2[l] = rb2 + local_p2[l] = poss2 .position .inverse_transform_point(&manifold_points[l].point); dists[l] = manifold_points[l].dist; } let constraint = PositionConstraint { - rb1: rb1.active_set_offset, - rb2: rb2.active_set_offset, + rb1: ids1.active_set_offset, + rb2: ids2.active_set_offset, local_p1, local_p2, - local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal), + local_n1: poss1 + .position + .inverse_transform_vector(&manifold.data.normal), dists, - im1: rb1.effective_inv_mass, - im2: rb2.effective_inv_mass, - ii1: rb1.effective_world_inv_inertia_sqrt.squared(), - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + im1: mprops1.effective_inv_mass, + im2: mprops2.effective_inv_mass, + ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_points.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 67b5cdb..0b8e762 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -1,5 +1,5 @@ use super::AnyPositionConstraint; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition}; use crate::geometry::ContactManifold; use crate::math::{ AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, @@ -7,6 +7,7 @@ use crate::math::{ }; use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::data::ComponentSet; use num::Zero; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; @@ -28,39 +29,47 @@ pub(crate) struct WPositionConstraint { } impl WPositionConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { - let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; - let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { + let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; + let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let sqrt_ii1: AngularInertia = AngularInertia::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = AngularInertia::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); + let poss1: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; + let poss2: [&RigidBodyPosition; 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 pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); - let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let sqrt_ii1: AngularInertia = + AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let sqrt_ii2: AngularInertia = + AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); - let local_n1 = pos1.inverse_transform_vector(&Vector::from( - array![|ii| manifolds[ii].data.normal; SIMD_WIDTH], - )); + let pos1 = Isometry::from(gather![|ii| poss1[ii].position]); + let pos2 = Isometry::from(gather![|ii| poss2[ii].position]); - let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_n1 = + pos1.inverse_transform_vector(&Vector::from(gather![|ii| manifolds[ii].data.normal])); + + let rb1 = gather![|ii| ids1[ii].active_set_offset]; + let rb2 = gather![|ii| ids2[ii].active_set_offset]; let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; + let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WPositionConstraint { @@ -80,8 +89,8 @@ impl WPositionConstraint { }; for i in 0..num_points { - let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]); - let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]); + let point = Point::from(gather![|ii| manifold_points[ii][i].point]); + let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]); constraint.local_p1[i] = pos1.inverse_transform_point(&point); constraint.local_p2[i] = pos2.inverse_transform_point(&point); constraint.dists[i] = dist; @@ -99,8 +108,8 @@ impl WPositionConstraint { pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. - let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); - let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); + let mut pos1 = Isometry::from(gather![|ii| positions[self.rb1[ii]]]); + let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]); let allowed_err = SimdReal::splat(params.allowed_linear_error); for k in 0..self.num_contacts as usize { diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index e1a4016..53df7f8 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -1,5 +1,6 @@ use super::AnyPositionConstraint; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::data::{BundleSet, ComponentSet}; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition}; use crate::geometry::ContactManifold; use crate::math::{ AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, @@ -21,24 +22,28 @@ pub(crate) struct PositionGroundConstraint { } impl PositionGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold: &ContactManifold, - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { - let mut rb1 = &bodies[manifold.data.body_pair.body1]; - let mut rb2 = &bodies[manifold.data.body_pair.body2]; + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { let flip = manifold.data.relative_dominance < 0; - let n1 = if flip { - std::mem::swap(&mut rb1, &mut rb2); - -manifold.data.normal + let (handle2, n1) = if flip { + (manifold.data.rigid_body1.unwrap(), -manifold.data.normal) } else { - manifold.data.normal + (manifold.data.rigid_body2.unwrap(), manifold.data.normal) }; + let (ids2, poss2, mprops2): (&RigidBodyIds, &RigidBodyPosition, &RigidBodyMassProps) = + bodies.index_bundle(handle2.0); + for (l, manifold_contacts) in manifold .data .solver_contacts @@ -51,20 +56,20 @@ impl PositionGroundConstraint { for k in 0..manifold_contacts.len() { p1[k] = manifold_contacts[k].point; - local_p2[k] = rb2 + local_p2[k] = poss2 .position .inverse_transform_point(&manifold_contacts[k].point); dists[k] = manifold_contacts[k].dist; } let constraint = PositionGroundConstraint { - rb2: rb2.active_set_offset, + rb2: ids2.active_set_offset, p1, local_p2, n1, dists, - im2: rb2.effective_inv_mass, - ii2: rb2.effective_world_inv_inertia_sqrt.squared(), + im2: mprops2.effective_inv_mass, + ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_contacts.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 1869c9c..7d4ff96 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -1,5 +1,5 @@ use super::AnyPositionConstraint; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition}; use crate::geometry::ContactManifold; use crate::math::{ AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, @@ -7,6 +7,7 @@ use crate::math::{ }; use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::data::ComponentSet; use num::Zero; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; @@ -25,42 +26,51 @@ pub(crate) struct WPositionGroundConstraint { } impl WPositionGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { - let mut rbs1 = - array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; - let mut rbs2 = - array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { + let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; + let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { if manifolds[ii].data.relative_dominance < 0 { flipped[ii] = true; - std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + std::mem::swap(&mut handles1[ii], &mut handles2[ii]); } } - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = AngularInertia::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); + let poss2: [&RigidBodyPosition; SIMD_WIDTH] = + gather![|ii| bodies.index(handles2[ii].unwrap().0)]; + let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)]; + let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = + gather![|ii| bodies.index(handles2[ii].unwrap().0)]; - let n1 = Vector::from( - array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH], - ); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let sqrt_ii2: AngularInertia = + AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); - let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); - let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let n1 = Vector::from(gather![|ii| if flipped[ii] { + -manifolds[ii].data.normal + } else { + manifolds[ii].data.normal + }]); + + let pos2 = Isometry::from(gather![|ii| poss2[ii].position]); + let rb2 = gather![|ii| ids2[ii].active_set_offset]; let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; + let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WPositionGroundConstraint { @@ -77,8 +87,8 @@ impl WPositionGroundConstraint { }; for i in 0..num_points { - let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]); - let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]); + let point = Point::from(gather![|ii| manifold_points[ii][i].point]); + let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]); constraint.p1[i] = point; constraint.local_p2[i] = pos2.inverse_transform_point(&point); constraint.dists[i] = dist; @@ -96,7 +106,7 @@ impl WPositionGroundConstraint { pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. - let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); + let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]); let allowed_err = SimdReal::splat(params.allowed_linear_error); for k in 0..self.num_contacts as usize { diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index 2fa4aee..48b71aa 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -1,5 +1,7 @@ use super::AnyJointPositionConstraint; -use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet}; +use crate::data::{ComponentSet, ComponentSetMut}; +use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters}; +use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyPosition}; use crate::math::{Isometry, Real}; pub(crate) struct PositionSolver { @@ -13,25 +15,28 @@ impl PositionSolver { } } - pub fn solve( + pub fn solve( &mut self, island_id: usize, params: &IntegrationParameters, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, contact_constraints: &[AnyPositionConstraint], joint_constraints: &[AnyJointPositionConstraint], - ) { + ) where + Bodies: ComponentSet + ComponentSetMut, + { if contact_constraints.is_empty() && joint_constraints.is_empty() { // Nothing to do. return; } self.positions.clear(); - self.positions.extend( - bodies - .iter_active_island(island_id) - .map(|(_, b)| b.next_position), - ); + self.positions + .extend(islands.active_island(island_id).iter().map(|h| { + let poss: &RigidBodyPosition = bodies.index(h.0); + poss.next_position + })); for _ in 0..params.max_position_iterations { for constraint in joint_constraints { @@ -43,8 +48,10 @@ impl PositionSolver { } } - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.set_next_position(self.positions[rb.active_set_offset]) - }); + for handle in islands.active_island(island_id) { + let ids: &RigidBodyIds = bodies.index(handle.0); + let next_pos = &self.positions[ids.active_set_offset]; + bodies.map_mut_internal(handle.0, |poss| poss.next_position = *next_pos); + } } } diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index 3a4ecb7..a9aa780 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -5,13 +5,16 @@ use super::{ use super::{ WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint, }; +use crate::data::ComponentSet; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, }; use crate::dynamics::{ - solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, + solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds, + RigidBodyMassProps, RigidBodyPosition, RigidBodyType, }; +use crate::dynamics::{IslandManager, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; @@ -50,13 +53,16 @@ impl } impl SolverConstraints { - pub fn init_constraint_groups( + pub fn init_constraint_groups( &mut self, island_id: usize, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - ) { + ) where + Bodies: ComponentSet + ComponentSet, + { self.not_ground_interactions.clear(); self.ground_interactions.clear(); categorize_contacts( @@ -70,6 +76,7 @@ impl SolverConstraints { self.interaction_groups.clear_groups(); self.interaction_groups.group_manifolds( island_id, + islands, bodies, manifolds, &self.not_ground_interactions, @@ -78,6 +85,7 @@ impl SolverConstraints { self.ground_interaction_groups.clear_groups(); self.ground_interaction_groups.group_manifolds( island_id, + islands, bodies, manifolds, &self.ground_interactions, @@ -92,18 +100,25 @@ impl SolverConstraints { // .append(&mut self.ground_interaction_groups.grouped_interactions); } - pub fn init( + pub fn init( &mut self, island_id: usize, params: &IntegrationParameters, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { self.velocity_constraints.clear(); self.position_constraints.clear(); - self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); + self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices); #[cfg(feature = "simd-is-enabled")] { @@ -118,19 +133,24 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_constraints( + fn compute_grouped_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, manifolds_all: &[&mut ContactManifold], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for manifolds_i in self .interaction_groups .grouped_interactions .chunks_exact(SIMD_WIDTH) { - let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; - let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + let manifold_id = gather![|ii| manifolds_i[ii]]; + let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; WVelocityConstraint::generate( params, manifold_id, @@ -149,12 +169,17 @@ impl SolverConstraints { } } - fn compute_nongrouped_constraints( + fn compute_nongrouped_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, manifolds_all: &[&mut ContactManifold], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for manifold_i in &self.interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; VelocityConstraint::generate( @@ -176,19 +201,24 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_ground_constraints( + fn compute_grouped_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, manifolds_all: &[&mut ContactManifold], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for manifolds_i in self .ground_interaction_groups .grouped_interactions .chunks_exact(SIMD_WIDTH) { - let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; - let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + let manifold_id = gather![|ii| manifolds_i[ii]]; + let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; WVelocityGroundConstraint::generate( params, manifold_id, @@ -207,12 +237,17 @@ impl SolverConstraints { } } - fn compute_nongrouped_ground_constraints( + fn compute_nongrouped_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, manifolds_all: &[&mut ContactManifold], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; VelocityGroundConstraint::generate( @@ -235,14 +270,21 @@ impl SolverConstraints { } impl SolverConstraints { - pub fn init( + pub fn init( &mut self, island_id: usize, params: &IntegrationParameters, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, joints: &[JointGraphEdge], joint_constraint_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { // Generate constraints for joints. self.not_ground_interactions.clear(); self.ground_interactions.clear(); @@ -260,6 +302,7 @@ impl SolverConstraints { self.interaction_groups.clear_groups(); self.interaction_groups.group_joints( island_id, + islands, bodies, joints, &self.not_ground_interactions, @@ -268,6 +311,7 @@ impl SolverConstraints { self.ground_interaction_groups.clear_groups(); self.ground_interaction_groups.group_joints( island_id, + islands, bodies, joints, &self.ground_interactions, @@ -292,12 +336,18 @@ impl SolverConstraints { } } - fn compute_nongrouped_joint_ground_constraints( + fn compute_nongrouped_joint_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, joints_all: &[JointGraphEdge], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for joint_i in &self.ground_interaction_groups.nongrouped_interactions { let joint = &joints_all[*joint_i].weight; let vel_constraint = @@ -309,19 +359,25 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_ground_constraints( + fn compute_grouped_joint_ground_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, joints_all: &[JointGraphEdge], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for joints_i in self .ground_interaction_groups .grouped_interactions .chunks_exact(SIMD_WIDTH) { - let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH]; - let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH]; + let joints_id = gather![|ii| joints_i[ii]]; + let joints = gather![|ii| &joints_all[joints_i[ii]].weight]; let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground( params, joints_id, joints, bodies, ); @@ -332,12 +388,17 @@ impl SolverConstraints { } } - fn compute_nongrouped_joint_constraints( + fn compute_nongrouped_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, joints_all: &[JointGraphEdge], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for joint_i in &self.interaction_groups.nongrouped_interactions { let joint = &joints_all[*joint_i].weight; let vel_constraint = @@ -349,19 +410,24 @@ impl SolverConstraints { } #[cfg(feature = "simd-is-enabled")] - fn compute_grouped_joint_constraints( + fn compute_grouped_joint_constraints( &mut self, params: &IntegrationParameters, - bodies: &RigidBodySet, + bodies: &Bodies, joints_all: &[JointGraphEdge], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { for joints_i in self .interaction_groups .grouped_interactions .chunks_exact(SIMD_WIDTH) { - let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH]; - let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH]; + let joints_id = gather![|ii| joints_i[ii]]; + let joints = gather![|ii| &joints_all[joints_i[ii]].weight]; let vel_constraint = AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies); self.velocity_constraints.push(vel_constraint); diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index c339ce4..e15f2d8 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -1,7 +1,8 @@ +use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::solver::VelocityGroundConstraint; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; @@ -102,23 +103,32 @@ impl VelocityConstraint { manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize } - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { assert_eq!(manifold.data.relative_dominance, 0); let inv_dt = params.inv_dt(); let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); - let rb1 = &bodies[manifold.data.body_pair.body1]; - let rb2 = &bodies[manifold.data.body_pair.body2]; - let mj_lambda1 = rb1.active_set_offset; - let mj_lambda2 = rb2.active_set_offset; + let 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 force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; @@ -126,7 +136,7 @@ impl VelocityConstraint { let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] let (tangents1, tangent_rot1) = - super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel); + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); for (_l, manifold_points) in manifold .data @@ -142,8 +152,8 @@ impl VelocityConstraint { #[cfg(feature = "dim3")] tangent_rot1, elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: rb1.effective_inv_mass, - im2: rb2.effective_inv_mass, + im1: mprops1.effective_inv_mass, + im2: mprops2.effective_inv_mass, limit: 0.0, mj_lambda1, mj_lambda2, @@ -190,8 +200,8 @@ impl VelocityConstraint { constraint.tangent1 = tangents1[0]; constraint.tangent_rot1 = tangent_rot1; } - constraint.im1 = rb1.effective_inv_mass; - constraint.im2 = rb2.effective_inv_mass; + constraint.im1 = mprops1.effective_inv_mass; + constraint.im2 = mprops2.effective_inv_mass; constraint.limit = 0.0; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; @@ -202,11 +212,11 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - rb1.world_com; - let dp2 = manifold_point.point - rb2.world_com; + let dp1 = manifold_point.point - mprops1.world_com; + let dp2 = manifold_point.point - mprops2.world_com; - let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); - let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); let warmstart_correction; @@ -215,16 +225,16 @@ impl VelocityConstraint { // Normal part. { - let gcross1 = rb1 + let gcross1 = mprops1 .effective_world_inv_inertia_sqrt .transform_vector(dp1.gcross(force_dir1)); - let gcross2 = rb2 + let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); let r = 1.0 - / (rb1.effective_inv_mass - + rb2.effective_inv_mass + / (mprops1.effective_inv_mass + + mprops2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -261,15 +271,15 @@ impl VelocityConstraint { constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { - let gcross1 = rb1 + let gcross1 = mprops1 .effective_world_inv_inertia_sqrt .transform_vector(dp1.gcross(tangents1[j])); - let gcross2 = rb2 + let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let r = 1.0 - / (rb1.effective_inv_mass - + rb2.effective_inv_mass + / (mprops1.effective_inv_mass + + mprops2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 7d5f468..baaf643 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -1,7 +1,8 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, }; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::data::ComponentSet; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, @@ -32,14 +33,18 @@ pub(crate) struct WVelocityConstraint { } impl WVelocityConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { for ii in 0..SIMD_WIDTH { assert_eq!(manifolds[ii].data.relative_dominance, 0); } @@ -49,36 +54,39 @@ impl WVelocityConstraint { let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt()); - let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; + let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; + let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; - let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); - let ii1: AngularInertia = AngularInertia::from( - array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); + 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 linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); + let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let ii1: AngularInertia = + AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = AngularInertia::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2: AngularInertia = + AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]); - let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); - - let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset]; + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; let warmstart_multiplier = - SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); let num_active_contacts = manifolds[0].data.num_active_contacts(); @@ -89,9 +97,8 @@ impl WVelocityConstraint { super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| - &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH - ]; + let manifold_points = + gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WVelocityConstraint { @@ -112,24 +119,20 @@ impl WVelocityConstraint { }; for k in 0..num_points { - let friction = - SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); - let restitution = - SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); - let is_bouncy = SimdReal::from( - array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH], - ); + let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]); + let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]); + let is_bouncy = SimdReal::from(gather![ + |ii| manifold_points[ii][k].is_bouncy() as u32 as Real + ]); let is_resting = SimdReal::splat(1.0) - is_bouncy; - let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); - let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let point = Point::from(gather![|ii| manifold_points[ii][k].point]); + let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); let tangent_velocity = - Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); + Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); - let impulse = SimdReal::from( - array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH], - ); - let prev_rhs = - SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]); + let impulse = + SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]); + let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]); let dp1 = point - world_com1; let dp2 = point - world_com2; @@ -140,8 +143,7 @@ impl WVelocityConstraint { let warmstart_correction; constraint.limit = friction; - constraint.manifold_contact_id[k] = - array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH]; + constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id]; // Normal part. { @@ -172,15 +174,15 @@ impl WVelocityConstraint { // tangent parts. #[cfg(feature = "dim2")] - let impulse = [SimdReal::from( - array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], - ) * warmstart_correction]; + let impulse = [SimdReal::from(gather![ + |ii| manifold_points[ii][k].warmstart_tangent_impulse + ]) * warmstart_correction]; #[cfg(feature = "dim3")] let impulse = tangent_rot1 - * na::Vector2::from( - array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], - ) + * na::Vector2::from(gather![ + |ii| manifold_points[ii][k].warmstart_tangent_impulse + ]) * warmstart_correction; constraint.elements[k].tangent_part.impulse = impulse; @@ -210,21 +212,17 @@ impl WVelocityConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; VelocityConstraintElement::warmstart_group( @@ -250,21 +248,17 @@ impl WVelocityConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular + ]), }; let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; VelocityConstraintElement::solve_group( diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 0e195d5..d1d5e8c 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -2,12 +2,13 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, }; -use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; #[cfg(feature = "dim2")] use crate::utils::WBasis; use crate::utils::{WAngularInertia, WCross, WDot}; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::data::{BundleSet, ComponentSet}; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; #[derive(Copy, Clone, Debug)] @@ -28,35 +29,50 @@ pub(crate) struct VelocityGroundConstraint { } impl VelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: ContactManifoldIndex, manifold: &ContactManifold, - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { let inv_dt = params.inv_dt(); let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); - let mut rb1 = &bodies[manifold.data.body_pair.body1]; - let mut rb2 = &bodies[manifold.data.body_pair.body2]; + let mut handle1 = manifold.data.rigid_body1; + let mut handle2 = manifold.data.rigid_body2; let flipped = manifold.data.relative_dominance < 0; let (force_dir1, flipped_multiplier) = if flipped { - std::mem::swap(&mut rb1, &mut rb2); + std::mem::swap(&mut handle1, &mut handle2); (manifold.data.normal, -1.0) } else { (-manifold.data.normal, 1.0) }; + let (vels1, world_com1) = if let Some(handle1) = handle1 { + let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = + bodies.index_bundle(handle1.0); + (*vels1, mprops1.world_com) + } else { + (RigidBodyVelocity::zero(), Point::origin()) + }; + + let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) = + bodies.index_bundle(handle2.unwrap().0); + #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] let (tangents1, tangent_rot1) = - super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel); + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - let mj_lambda2 = rb2.active_set_offset; + let mj_lambda2 = ids2.active_set_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (_l, manifold_points) in manifold @@ -73,7 +89,7 @@ impl VelocityGroundConstraint { #[cfg(feature = "dim3")] tangent_rot1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: rb2.effective_inv_mass, + im2: mprops2.effective_inv_mass, limit: 0.0, mj_lambda2, manifold_id, @@ -119,7 +135,7 @@ impl VelocityGroundConstraint { constraint.tangent1 = tangents1[0]; constraint.tangent_rot1 = tangent_rot1; } - constraint.im2 = rb2.effective_inv_mass; + constraint.im2 = mprops2.effective_inv_mass; constraint.limit = 0.0; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -129,10 +145,10 @@ impl VelocityGroundConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp2 = manifold_point.point - rb2.world_com; - let dp1 = manifold_point.point - rb1.world_com; - let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); - let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + let dp2 = manifold_point.point - mprops2.world_com; + let dp1 = manifold_point.point - world_com1; + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); let warmstart_correction; constraint.limit = manifold_point.friction; @@ -140,11 +156,11 @@ impl VelocityGroundConstraint { // Normal part. { - let gcross2 = rb2 + let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2)); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; @@ -178,10 +194,10 @@ impl VelocityGroundConstraint { constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { - let gcross2 = rb2 + let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); - let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + flipped_multiplier * manifold_point.tangent_velocity) .dot(&tangents1[j]); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 4237d99..3aede0b 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,8 @@ use super::{ AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart, }; -use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::data::ComponentSet; +use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, @@ -31,52 +32,71 @@ pub(crate) struct WVelocityGroundConstraint { } impl WVelocityGroundConstraint { - pub fn generate( + pub fn generate( params: &IntegrationParameters, manifold_id: [ContactManifoldIndex; SIMD_WIDTH], manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &RigidBodySet, + bodies: &Bodies, out_constraints: &mut Vec, push: bool, - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt()); - let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; + let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; + let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; let mut flipped = [1.0; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { if manifolds[ii].data.relative_dominance < 0 { - std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + std::mem::swap(&mut handles1[ii], &mut handles2[ii]); flipped[ii] = -1.0; } } + let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| { + handles1[ii] + .map(|h| *bodies.index(h.0)) + .unwrap_or_else(RigidBodyVelocity::zero) + }]; + let world_com1 = Point::from(gather![|ii| { + handles1[ii] + .map(|h| ComponentSet::::index(bodies, h.0).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)]; + let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = + gather![|ii| bodies.index(handles2[ii].unwrap().0)]; + let flipped_sign = SimdReal::from(flipped); - let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = AngularInertia::from( - array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], - ); + let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let ii2: AngularInertia = + AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); - let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); + let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); - let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); + let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); - let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); - let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); + let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]); let force_dir1 = normal1 * -flipped_sign; - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; let warmstart_multiplier = - SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope); let num_active_contacts = manifolds[0].data.num_active_contacts(); @@ -88,7 +108,7 @@ impl WVelocityGroundConstraint { super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; + let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WVelocityGroundConstraint { @@ -107,24 +127,20 @@ impl WVelocityGroundConstraint { }; for k in 0..num_points { - let friction = - SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); - let restitution = - SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); - let is_bouncy = SimdReal::from( - array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH], - ); + let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]); + let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]); + let is_bouncy = SimdReal::from(gather![ + |ii| manifold_points[ii][k].is_bouncy() as u32 as Real + ]); let is_resting = SimdReal::splat(1.0) - is_bouncy; - let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); - let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let point = Point::from(gather![|ii| manifold_points[ii][k].point]); + let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); let tangent_velocity = - Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); + Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); - let impulse = SimdReal::from( - array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH], - ); - let prev_rhs = - SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]); + let impulse = + SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]); + let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]); let dp1 = point - world_com1; let dp2 = point - world_com2; @@ -133,8 +149,7 @@ impl WVelocityGroundConstraint { let warmstart_correction; constraint.limit = friction; - constraint.manifold_contact_id[k] = - array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH]; + constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id]; // Normal part. { @@ -162,14 +177,14 @@ impl WVelocityGroundConstraint { // tangent parts. #[cfg(feature = "dim2")] - let impulse = [SimdReal::from( - array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], - ) * warmstart_correction]; + let impulse = [SimdReal::from(gather![ + |ii| manifold_points[ii][k].warmstart_tangent_impulse + ]) * warmstart_correction]; #[cfg(feature = "dim3")] let impulse = tangent_rot1 - * na::Vector2::from( - array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], - ) + * na::Vector2::from(gather![ + |ii| manifold_points[ii][k].warmstart_tangent_impulse + ]) * warmstart_correction; constraint.elements[k].tangent_part.impulse = impulse; @@ -195,12 +210,10 @@ impl WVelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; VelocityGroundConstraintElement::warmstart_group( @@ -220,12 +233,10 @@ impl WVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { - linear: Vector::from( - array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], - ), - angular: AngVector::from( - array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], - ), + linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]), + angular: AngVector::from(gather![ + |ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular + ]), }; VelocityGroundConstraintElement::solve_group( diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 4f35f3b..9ceb9d9 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,8 +1,10 @@ use super::AnyJointVelocityConstraint; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::dynamics::{ solver::{AnyVelocityConstraint, DeltaVel}, - IntegrationParameters, JointGraphEdge, RigidBodySet, + IntegrationParameters, JointGraphEdge, RigidBodyForces, RigidBodyVelocity, }; +use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps}; use crate::geometry::ContactManifold; use crate::math::Real; use crate::utils::WAngularInertia; @@ -18,31 +20,38 @@ impl VelocitySolver { } } - pub fn solve( + pub fn solve( &mut self, island_id: usize, params: &IntegrationParameters, - bodies: &mut RigidBodySet, + islands: &IslandManager, + bodies: &mut Bodies, manifolds_all: &mut [&mut ContactManifold], joints_all: &mut [JointGraphEdge], contact_constraints: &mut [AnyVelocityConstraint], joint_constraints: &mut [AnyJointVelocityConstraint], - ) { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSetMut + + ComponentSet, + { self.mj_lambdas.clear(); self.mj_lambdas - .resize(bodies.active_island(island_id).len(), DeltaVel::zero()); + .resize(islands.active_island(island_id).len(), DeltaVel::zero()); // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc): - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - let dvel = &mut self.mj_lambdas[rb.active_set_offset]; + for handle in islands.active_island(island_id) { + let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) = + bodies.index_bundle(handle.0); - dvel.linear += rb.force * (rb.effective_inv_mass * params.dt); - rb.force = na::zero(); + let dvel = &mut self.mj_lambdas[ids.active_set_offset]; - // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor: - dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt; - rb.torque = na::zero(); - }); + // 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 * (mprops.effective_inv_mass * params.dt); + } /* * Warmstart constraints. @@ -69,13 +78,19 @@ impl VelocitySolver { } // Update velocities. - bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - let dvel = self.mj_lambdas[rb.active_set_offset]; - rb.linvel += dvel.linear; - rb.angvel += rb + for handle in islands.active_island(island_id) { + let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = bodies.index_bundle(handle.0); + + let dvel = self.mj_lambdas[ids.active_set_offset]; + let dangvel = mprops .effective_world_inv_inertia_sqrt .transform_vector(dvel.angular); - }); + + bodies.map_mut_internal(handle.0, |vels| { + vels.linvel += dvel.linear; + vels.angvel += dangvel; + }); + } // Write impulses back into the manifold structures. for constraint in &*joint_constraints { diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index fc79d6d..890d851 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -1,15 +1,17 @@ use super::{ BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool, }; -use crate::data::pubsub::Subscription; use crate::geometry::broad_phase_multi_sap::SAPProxyIndex; -use crate::geometry::collider::ColliderChanges; -use crate::geometry::{ColliderSet, RemovedCollider}; +use crate::geometry::{ + ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape, +}; use crate::math::Real; use crate::utils::IndexMut2; use parry::bounding_volume::BoundingVolume; use parry::utils::hashmap::HashMap; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; + /// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune. /// /// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws: @@ -78,8 +80,19 @@ pub struct BroadPhase { layers: Vec, smallest_layer: u8, largest_layer: u8, - removed_colliders: Option>, deleted_any: bool, + // NOTE: we maintain this hashmap to simplify collider removal. + // This information is also present in the ColliderProxyId + // component. However if that component is removed, we need + // a way to access it to do some cleanup. + // Note that we could just remove the ColliderProxyId component + // altogether but that would be slow because of the need to + // always access this hashmap. Instead, we access this hashmap + // only when the collider has been added/removed. + // Another alternative would be to remove ColliderProxyId and + // just use a Coarena. But this seems like it could use too + // much memory. + colliders_proxy_ids: HashMap, #[cfg_attr(feature = "serde-serialize", serde(skip))] region_pool: SAPRegionPool, // To avoid repeated allocations. // We could think serializing this workspace is useless. @@ -107,13 +120,13 @@ impl BroadPhase { /// Create a new empty broad-phase. pub fn new() -> Self { BroadPhase { - removed_colliders: None, proxies: SAPProxies::new(), layers: Vec::new(), smallest_layer: 0, largest_layer: 0, region_pool: Vec::new(), reporting: HashMap::default(), + colliders_proxy_ids: HashMap::default(), deleted_any: false, } } @@ -123,26 +136,13 @@ impl BroadPhase { /// For each colliders marked as removed, we make their containing layer mark /// its proxy as pre-deleted. The actual proxy removal will happen at the end /// of the `BroadPhase::update`. - fn handle_removed_colliders(&mut self, colliders: &mut ColliderSet) { - // Ensure we already subscribed the collider-removed events. - if self.removed_colliders.is_none() { - self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) { + // For each removed collider, remove the corresponding proxy. + for removed in removed_colliders { + if let Some(proxy_id) = self.colliders_proxy_ids.get(removed).copied() { + self.predelete_proxy(proxy_id); + } } - - // Extract the cursor to avoid borrowing issues. - let cursor = self.removed_colliders.take().unwrap(); - - // Read all the collider-removed events, and remove the corresponding proxy. - for collider in colliders.removed_colliders.read(&cursor) { - self.predelete_proxy(collider.proxy_index); - } - - // NOTE: We don't acknowledge the cursor just yet because we need - // to traverse the set of removed colliders one more time after - // the broad-phase update. - - // Re-insert the cursor we extracted to avoid borrowing issues. - self.removed_colliders = Some(cursor); } /// Pre-deletes a proxy from this broad-phase. @@ -173,7 +173,7 @@ impl BroadPhase { /// This method will actually remove from the proxy list all the proxies /// marked as deletable by `self.predelete_proxy`, making their proxy /// handles re-usable by new proxies. - fn complete_removals(&mut self, colliders: &mut ColliderSet) { + fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) { // If there is no layer, there is nothing to remove. if self.layers.is_empty() { return; @@ -215,13 +215,13 @@ impl BroadPhase { /* * Actually remove the colliders proxies. */ - let cursor = self.removed_colliders.as_ref().unwrap(); - for collider in colliders.removed_colliders.read(&cursor) { - if collider.proxy_index != crate::INVALID_U32 { - self.proxies.remove(collider.proxy_index); + for removed in removed_colliders { + if let Some(proxy_id) = self.colliders_proxy_ids.remove(removed) { + if proxy_id != crate::INVALID_U32 { + self.proxies.remove(proxy_id); + } } } - colliders.removed_colliders.ack(&cursor); } /// Finalize the insertion of the layer identified by `layer_id`. @@ -336,67 +336,127 @@ impl BroadPhase { } } - /// Updates the broad-phase, taking into account the new collider positions. - pub fn update( + fn handle_modified_collider( &mut self, prediction_distance: Real, - colliders: &mut ColliderSet, + handle: ColliderHandle, + proxy_index: &mut u32, + collider: (&ColliderPosition, &ColliderShape, &ColliderChanges), + ) -> bool { + let (co_pos, co_shape, co_changes) = collider; + + let mut aabb = co_shape + .compute_aabb(co_pos) + .loosened(prediction_distance / 2.0); + + aabb.mins = super::clamp_point(aabb.mins); + aabb.maxs = super::clamp_point(aabb.maxs); + + let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) { + let mut layer_id = proxy.layer_id; + proxy.aabb = aabb; + + if co_changes.contains(ColliderChanges::SHAPE) { + // If the shape was changed, then we need to see if this proxy should be + // migrated to a larger layer. Indeed, if the shape was replaced by + // a much larger shape, we need to promote the proxy to a bigger layer + // to avoid the O(n²) discretization problem. + let new_layer_depth = super::layer_containing_aabb(&aabb); + if new_layer_depth > proxy.layer_depth { + self.layers[proxy.layer_id as usize] + .proper_proxy_moved_to_bigger_layer(&mut self.proxies, *proxy_index); + + // We need to promote the proxy to the bigger layer. + layer_id = self.ensure_layer_exists(new_layer_depth); + self.proxies[*proxy_index].layer_id = layer_id; + } + } + + layer_id + } else { + let layer_depth = super::layer_containing_aabb(&aabb); + let layer_id = self.ensure_layer_exists(layer_depth); + + // Create the proxy. + let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth); + *proxy_index = self.proxies.insert(proxy); + layer_id + }; + + let layer = &mut self.layers[layer_id as usize]; + + // Preupdate the collider in the layer. + layer.preupdate_collider( + *proxy_index, + &aabb, + &mut self.proxies, + &mut self.region_pool, + ); + let need_region_propagation = !layer.created_regions.is_empty(); + + need_region_propagation + } + + /// Updates the broad-phase, taking into account the new collider positions. + pub fn update( + &mut self, + prediction_distance: Real, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], events: &mut Vec, - ) { + ) where + Colliders: ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet, + { // Phase 1: pre-delete the collisions that have been deleted. - self.handle_removed_colliders(colliders); + self.handle_removed_colliders(removed_colliders); let mut need_region_propagation = false; // Phase 2: pre-delete the collisions that have been deleted. - colliders.foreach_modified_colliders_mut_internal(|handle, collider| { - if !collider.changes.needs_broad_phase_update() { - return; - } + 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); - let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); - aabb.mins = super::clamp_point(aabb.mins); - aabb.maxs = super::clamp_point(aabb.maxs); + if let Some(co_changes) = co_changes { + let (co_bf_data, co_pos, co_shape): ( + &ColliderBroadPhaseData, + &ColliderPosition, + &ColliderShape, + ) = colliders.index_bundle(handle.0); - let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { - let mut layer_id = proxy.layer_id; - proxy.aabb = aabb; + if !co_changes.needs_broad_phase_update() { + return; + } + let mut new_proxy_id = co_bf_data.proxy_index; - if collider.changes.contains(ColliderChanges::SHAPE) { - // If the shape was changed, then we need to see if this proxy should be - // migrated to a larger layer. Indeed, if the shape was replaced by - // a much larger shape, we need to promote the proxy to a bigger layer - // to avoid the O(n²) discretization problem. - let new_layer_depth = super::layer_containing_aabb(&aabb); - if new_layer_depth > proxy.layer_depth { - self.layers[proxy.layer_id as usize].proper_proxy_moved_to_bigger_layer( - &mut self.proxies, - collider.proxy_index, - ); - - // We need to promote the proxy to the bigger layer. - layer_id = self.ensure_layer_exists(new_layer_depth); - self.proxies[collider.proxy_index].layer_id = layer_id; - } + if self.handle_modified_collider( + prediction_distance, + *handle, + &mut new_proxy_id, + (co_pos, co_shape, co_changes), + ) { + need_region_propagation = true; } - layer_id - } else { - let layer_depth = super::layer_containing_aabb(&aabb); - let layer_id = self.ensure_layer_exists(layer_depth); + if co_bf_data.proxy_index != new_proxy_id { + self.colliders_proxy_ids.insert(*handle, new_proxy_id); - // Create the proxy. - let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth); - collider.proxy_index = self.proxies.insert(proxy); - layer_id - }; - - let layer = &mut self.layers[layer_id as usize]; - - // Preupdate the collider in the layer. - layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool); - need_region_propagation = need_region_propagation || !layer.created_regions.is_empty(); - }); + // 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, + }, + ); + } + } + } // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers. if need_region_propagation { @@ -408,7 +468,7 @@ impl BroadPhase { // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller // layers to possible remove regions from larger layers that would become empty that way. - self.complete_removals(colliders); + self.complete_removals(removed_colliders); } /// Propagate regions from the smallest layers up to the larger layers. diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs index 9a9c296..e4a9f42 100644 --- a/src/geometry/broad_phase_multi_sap/sap_layer.rs +++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs @@ -1,6 +1,6 @@ use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool}; use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE; -use crate::geometry::{Collider, SAPProxyIndex, AABB}; +use crate::geometry::{SAPProxyIndex, AABB}; use crate::math::{Point, Real}; use parry::utils::hashmap::{Entry, HashMap}; @@ -213,12 +213,11 @@ impl SAPLayer { pub fn preupdate_collider( &mut self, - collider: &Collider, + proxy_id: u32, aabb: &AABB, proxies: &mut SAPProxies, pool: &mut SAPRegionPool, ) { - let proxy_id = collider.proxy_index; let start = super::point_key(aabb.mins, self.region_width); let end = super::point_key(aabb.maxs, self.region_width); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 2b08a96..edf1d88 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,231 +1,160 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle}; -use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags}; +use crate::geometry::{ + ColliderBroadPhaseData, ColliderChanges, ColliderGroups, ColliderMassProperties, + ColliderMaterial, ColliderParent, ColliderPosition, ColliderShape, ColliderType, + InteractionGroups, SharedShape, SolverFlags, +}; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM}; use crate::parry::transformation::vhacd::VHACDParameters; use na::Unit; use parry::bounding_volume::{BoundingVolume, AABB}; use parry::shape::Shape; -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags affecting the behavior of the constraints solver for a given contact manifold. - pub(crate) struct ColliderFlags: u8 { - const SENSOR = 1 << 0; - const FRICTION_COMBINE_RULE_01 = 1 << 1; - const FRICTION_COMBINE_RULE_10 = 1 << 2; - const RESTITUTION_COMBINE_RULE_01 = 1 << 3; - const RESTITUTION_COMBINE_RULE_10 = 1 << 4; - } -} - -impl ColliderFlags { - pub fn is_sensor(self) -> bool { - self.contains(ColliderFlags::SENSOR) - } - - pub fn friction_combine_rule_value(self) -> u8 { - (self.bits & 0b0000_0110) >> 1 - } - - pub fn restitution_combine_rule_value(self) -> u8 { - (self.bits & 0b0001_1000) >> 3 - } - - pub fn with_friction_combine_rule(mut self, rule: CoefficientCombineRule) -> Self { - self.bits = (self.bits & !0b0000_0110) | ((rule as u8) << 1); - self - } - - pub fn with_restitution_combine_rule(mut self, rule: CoefficientCombineRule) -> Self { - self.bits = (self.bits & !0b0001_1000) | ((rule as u8) << 3); - self - } -} - -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -enum MassInfo { - /// `MassProperties` are computed with the help of [`SharedShape::mass_properties`]. - Density(Real), - MassProperties(Box), -} - -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags describing how the collider has been modified by the user. - pub(crate) struct ColliderChanges: u32 { - const MODIFIED = 1 << 0; - const POSITION_WRT_PARENT = 1 << 1; // => BF & NF updates. - const POSITION = 1 << 2; // => BF & NF updates. - const COLLISION_GROUPS = 1 << 3; // => NF update. - const SOLVER_GROUPS = 1 << 4; // => NF update. - const SHAPE = 1 << 5; // => BF & NF update. NF pair workspace invalidation. - const SENSOR = 1 << 6; // => NF update. NF pair invalidation. - } -} - -impl ColliderChanges { - pub fn needs_broad_phase_update(self) -> bool { - self.intersects( - ColliderChanges::POSITION_WRT_PARENT - | ColliderChanges::POSITION - | ColliderChanges::SHAPE, - ) - } - - pub fn needs_narrow_phase_update(self) -> bool { - self.bits() > 1 - } -} - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] /// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries. /// /// To build a new collider, use the `ColliderBuilder` structure. pub struct Collider { - shape: SharedShape, - mass_info: MassInfo, - pub(crate) flags: ColliderFlags, - pub(crate) solver_flags: SolverFlags, - pub(crate) changes: ColliderChanges, - pub(crate) parent: RigidBodyHandle, - pub(crate) delta: Isometry, - pub(crate) position: Isometry, - /// The friction coefficient of this collider. - pub friction: Real, - /// The restitution coefficient of this collider. - pub restitution: Real, - pub(crate) collision_groups: InteractionGroups, - pub(crate) solver_groups: InteractionGroups, - pub(crate) proxy_index: SAPProxyIndex, + pub co_type: ColliderType, + pub co_shape: ColliderShape, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_mprops: ColliderMassProperties, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_changes: ColliderChanges, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_parent: ColliderParent, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_pos: ColliderPosition, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_material: ColliderMaterial, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_groups: ColliderGroups, // TODO ECS: this is public only for our bevy_rapier experiments. + pub co_bf_data: ColliderBroadPhaseData, // TODO ECS: this is public only for our bevy_rapier experiments. /// User-defined data associated to this rigid-body. pub user_data: u128, } impl Collider { - pub(crate) fn reset_internal_references(&mut self) { - self.parent = RigidBodyHandle::invalid(); - self.proxy_index = crate::INVALID_U32; - self.changes = ColliderChanges::empty(); + // TODO ECS: exists only for our bevy_ecs tests. + pub fn reset_internal_references(&mut self) { + self.co_parent.handle = RigidBodyHandle::invalid(); + self.co_bf_data.proxy_index = crate::INVALID_U32; + self.co_changes = ColliderChanges::all(); } /// The rigid body this collider is attached to. pub fn parent(&self) -> RigidBodyHandle { - self.parent + self.co_parent.handle } /// Is this collider a sensor? pub fn is_sensor(&self) -> bool { - self.flags.is_sensor() + self.co_type.is_sensor() } /// The combine rule used by this collider to combine its friction /// coefficient with the friction coefficient of the other collider it /// is in contact with. pub fn friction_combine_rule(&self) -> CoefficientCombineRule { - CoefficientCombineRule::from_value(self.flags.friction_combine_rule_value()) + self.co_material.friction_combine_rule } /// Sets the combine rule used by this collider to combine its friction /// coefficient with the friction coefficient of the other collider it /// is in contact with. pub fn set_friction_combine_rule(&mut self, rule: CoefficientCombineRule) { - self.flags = self.flags.with_friction_combine_rule(rule); + self.co_material.friction_combine_rule = rule; } /// The combine rule used by this collider to combine its restitution /// coefficient with the restitution coefficient of the other collider it /// is in contact with. pub fn restitution_combine_rule(&self) -> CoefficientCombineRule { - CoefficientCombineRule::from_value(self.flags.restitution_combine_rule_value()) + self.co_material.restitution_combine_rule } /// Sets the combine rule used by this collider to combine its restitution /// coefficient with the restitution coefficient of the other collider it /// is in contact with. pub fn set_restitution_combine_rule(&mut self, rule: CoefficientCombineRule) { - self.flags = self.flags.with_restitution_combine_rule(rule) + self.co_material.restitution_combine_rule = rule; } /// Sets whether or not this is a sensor collider. pub fn set_sensor(&mut self, is_sensor: bool) { if is_sensor != self.is_sensor() { - self.changes.insert(ColliderChanges::SENSOR); - self.flags.set(ColliderFlags::SENSOR, is_sensor); + self.co_changes.insert(ColliderChanges::TYPE); + self.co_type = if is_sensor { + ColliderType::Sensor + } else { + ColliderType::Solid + }; } } #[doc(hidden)] pub fn set_position_debug(&mut self, position: Isometry) { - self.position = position; + self.co_pos.0 = position; } /// The position of this collider expressed in the local-space of the rigid-body it is attached to. #[deprecated(note = "use `.position_wrt_parent()` instead.")] pub fn delta(&self) -> &Isometry { - &self.delta + &self.co_parent.pos_wrt_parent } /// The world-space position of this collider. pub fn position(&self) -> &Isometry { - &self.position - } - - /// Sets the position of this collider wrt. its parent rigid-body. - pub(crate) fn set_position(&mut self, position: Isometry) { - self.changes.insert(ColliderChanges::POSITION); - self.position = position; + &self.co_pos } /// The position of this collider wrt the body it is attached to. pub fn position_wrt_parent(&self) -> &Isometry { - &self.delta + &self.co_parent.pos_wrt_parent } /// Sets the position of this collider wrt. its parent rigid-body. pub fn set_position_wrt_parent(&mut self, position: Isometry) { - self.changes.insert(ColliderChanges::POSITION_WRT_PARENT); - self.delta = position; + self.co_changes.insert(ColliderChanges::PARENT); + self.co_parent.pos_wrt_parent = position; } /// The collision groups used by this collider. pub fn collision_groups(&self) -> InteractionGroups { - self.collision_groups + self.co_groups.collision_groups } /// Sets the collision groups of this collider. pub fn set_collision_groups(&mut self, groups: InteractionGroups) { - if self.collision_groups != groups { - self.changes.insert(ColliderChanges::COLLISION_GROUPS); - self.collision_groups = groups; + if self.co_groups.collision_groups != groups { + self.co_changes.insert(ColliderChanges::GROUPS); + self.co_groups.collision_groups = groups; } } /// The solver groups used by this collider. pub fn solver_groups(&self) -> InteractionGroups { - self.solver_groups + self.co_groups.solver_groups } /// Sets the solver groups of this collider. pub fn set_solver_groups(&mut self, groups: InteractionGroups) { - if self.solver_groups != groups { - self.changes.insert(ColliderChanges::SOLVER_GROUPS); - self.solver_groups = groups; + if self.co_groups.solver_groups != groups { + self.co_changes.insert(ColliderChanges::GROUPS); + self.co_groups.solver_groups = groups; } } + pub fn material(&self) -> &ColliderMaterial { + &self.co_material + } + /// The density of this collider, if set. pub fn density(&self) -> Option { - match &self.mass_info { - MassInfo::Density(density) => Some(*density), - MassInfo::MassProperties(_) => None, + match &self.co_mprops { + ColliderMassProperties::Density(density) => Some(*density), + ColliderMassProperties::MassProperties(_) => None, } } /// The geometric shape of this collider. pub fn shape(&self) -> &dyn Shape { - &*self.shape.0 + self.co_shape.as_ref() } /// A mutable reference to the geometric shape of this collider. @@ -234,33 +163,33 @@ impl Collider { /// cloned first so that `self` contains a unique copy of that /// shape that you can modify. pub fn shape_mut(&mut self) -> &mut dyn Shape { - self.changes.insert(ColliderChanges::SHAPE); - self.shape.make_mut() + self.co_changes.insert(ColliderChanges::SHAPE); + self.co_shape.make_mut() } /// Sets the shape of this collider. pub fn set_shape(&mut self, shape: SharedShape) { - self.changes.insert(ColliderChanges::SHAPE); - self.shape = shape; + self.co_changes.insert(ColliderChanges::SHAPE); + self.co_shape = shape; } /// Compute the axis-aligned bounding box of this collider. pub fn compute_aabb(&self) -> AABB { - self.shape.compute_aabb(&self.position) + self.co_shape.compute_aabb(&self.co_pos) } /// Compute the axis-aligned bounding box of this collider. pub fn compute_swept_aabb(&self, next_position: &Isometry) -> AABB { - let aabb1 = self.shape.compute_aabb(&self.position); - let aabb2 = self.shape.compute_aabb(next_position); + let aabb1 = self.co_shape.compute_aabb(&self.co_pos); + let aabb2 = self.co_shape.compute_aabb(next_position); aabb1.merged(&aabb2) } /// Compute the local-space mass properties of this collider. pub fn mass_properties(&self) -> MassProperties { - match &self.mass_info { - MassInfo::Density(density) => self.shape.mass_properties(*density), - MassInfo::MassProperties(mass_properties) => **mass_properties, + match &self.co_mprops { + ColliderMassProperties::Density(density) => self.co_shape.mass_properties(*density), + ColliderMassProperties::MassProperties(mass_properties) => **mass_properties, } } } @@ -272,10 +201,10 @@ pub struct ColliderBuilder { /// The shape of the collider to be built. pub shape: SharedShape, /// The uniform density of the collider to be built. - density: Option, + pub density: Option, /// Overrides automatic computation of `MassProperties`. /// If None, it will be computed based on shape and density. - mass_properties: Option, + pub mass_properties: Option, /// The friction coefficient of the collider to be built. pub friction: Real, /// The rule used to combine two friction coefficients. @@ -285,7 +214,7 @@ pub struct ColliderBuilder { /// The rule used to combine two restitution coefficients. pub restitution_combine_rule: CoefficientCombineRule, /// The position of this collider relative to the local frame of the rigid-body it is attached to. - pub delta: Isometry, + pub pos_wrt_parent: Isometry, /// Is this collider a sensor? pub is_sensor: bool, /// Do we have to always call the contact modifier @@ -308,7 +237,7 @@ impl ColliderBuilder { mass_properties: None, friction: Self::default_friction(), restitution: 0.0, - delta: Isometry::identity(), + pos_wrt_parent: Isometry::identity(), is_sensor: false, user_data: 0, collision_groups: InteractionGroups::all(), @@ -646,8 +575,8 @@ impl ColliderBuilder { /// relative to the rigid-body it is attached to. #[cfg(feature = "dim2")] pub fn translation(mut self, x: Real, y: Real) -> Self { - self.delta.translation.x = x; - self.delta.translation.y = y; + self.pos_wrt_parent.translation.x = x; + self.pos_wrt_parent.translation.y = y; self } @@ -655,23 +584,23 @@ impl ColliderBuilder { /// relative to the rigid-body it is attached to. #[cfg(feature = "dim3")] pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self { - self.delta.translation.x = x; - self.delta.translation.y = y; - self.delta.translation.z = z; + self.pos_wrt_parent.translation.x = x; + self.pos_wrt_parent.translation.y = y; + self.pos_wrt_parent.translation.z = z; self } /// Sets the initial orientation of the collider to be created, /// relative to the rigid-body it is attached to. pub fn rotation(mut self, angle: AngVector) -> Self { - self.delta.rotation = Rotation::new(angle); + self.pos_wrt_parent.rotation = Rotation::new(angle); self } /// Sets the initial position (translation and orientation) of the collider to be created, /// relative to the rigid-body it is attached to. pub fn position_wrt_parent(mut self, pos: Isometry) -> Self { - self.delta = pos; + self.pos_wrt_parent = pos; self } @@ -679,53 +608,97 @@ impl ColliderBuilder { /// relative to the rigid-body it is attached to. #[deprecated(note = "Use `.position_wrt_parent` instead.")] pub fn position(mut self, pos: Isometry) -> Self { - self.delta = pos; + self.pos_wrt_parent = pos; self } /// Set the position of this collider in the local-space of the rigid-body it is attached to. - #[deprecated(note = "Use `.position` instead.")] + #[deprecated(note = "Use `.position_wrt_parent` instead.")] pub fn delta(mut self, delta: Isometry) -> Self { - self.delta = delta; + self.pos_wrt_parent = delta; self } /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { + let (co_changes, co_pos, co_bf_data, co_shape, co_type, co_groups, co_material, co_mprops) = + self.components(); + let co_parent = ColliderParent { + pos_wrt_parent: co_pos.0, + handle: RigidBodyHandle::invalid(), + }; + Collider { + co_shape, + co_mprops, + co_material, + co_parent, + co_changes, + co_pos, + co_bf_data, + co_groups, + co_type, + user_data: self.user_data, + } + } + + /// Builds all the components required by a collider. + pub fn components( + &self, + ) -> ( + ColliderChanges, + ColliderPosition, + ColliderBroadPhaseData, + ColliderShape, + ColliderType, + ColliderGroups, + ColliderMaterial, + ColliderMassProperties, + ) { let mass_info = if let Some(mp) = self.mass_properties { - MassInfo::MassProperties(Box::new(mp)) + ColliderMassProperties::MassProperties(Box::new(mp)) } else { let default_density = if self.is_sensor { 0.0 } else { 1.0 }; let density = self.density.unwrap_or(default_density); - MassInfo::Density(density) + ColliderMassProperties::Density(density) }; - let mut flags = ColliderFlags::empty(); - flags.set(ColliderFlags::SENSOR, self.is_sensor); - flags = flags - .with_friction_combine_rule(self.friction_combine_rule) - .with_restitution_combine_rule(self.restitution_combine_rule); let mut solver_flags = SolverFlags::default(); solver_flags.set( SolverFlags::MODIFY_SOLVER_CONTACTS, self.modify_solver_contacts, ); - Collider { - shape: self.shape.clone(), - mass_info, + let co_shape = self.shape.clone(); + let co_mprops = mass_info; + let co_material = ColliderMaterial { friction: self.friction, restitution: self.restitution, - delta: self.delta, - flags, + friction_combine_rule: self.friction_combine_rule, + restitution_combine_rule: self.restitution_combine_rule, solver_flags, - changes: ColliderChanges::all(), - parent: RigidBodyHandle::invalid(), - position: Isometry::identity(), - proxy_index: crate::INVALID_U32, + }; + let co_changes = ColliderChanges::all(); + let co_pos = ColliderPosition(self.pos_wrt_parent); + let co_bf_data = ColliderBroadPhaseData::default(); + let co_groups = ColliderGroups { collision_groups: self.collision_groups, solver_groups: self.solver_groups, - user_data: self.user_data, - } + }; + let co_type = if self.is_sensor { + ColliderType::Sensor + } else { + ColliderType::Solid + }; + + ( + co_changes, + co_pos, + co_bf_data, + co_shape, + co_type, + co_groups, + co_material, + co_mprops, + ) } } diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs new file mode 100644 index 0000000..1aa760a --- /dev/null +++ b/src/geometry/collider_components.rs @@ -0,0 +1,220 @@ +use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle}; +use crate::geometry::{InteractionGroups, SAPProxyIndex, Shape, SharedShape, SolverFlags}; +use crate::math::{Isometry, Real}; +use crate::parry::partitioning::IndexedData; +use std::ops::Deref; + +/// The unique identifier of a collider added to a collider set. +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[repr(transparent)] +pub struct ColliderHandle(pub crate::data::arena::Index); + +impl ColliderHandle { + /// Converts this handle into its (index, generation) components. + pub fn into_raw_parts(self) -> (u32, u32) { + self.0.into_raw_parts() + } + + /// Reconstructs an handle from its (index, generation) components. + pub fn from_raw_parts(id: u32, generation: u32) -> Self { + Self(crate::data::arena::Index::from_raw_parts(id, generation)) + } + + /// An always-invalid collider handle. + pub fn invalid() -> Self { + Self(crate::data::arena::Index::from_raw_parts( + crate::INVALID_U32, + crate::INVALID_U32, + )) + } +} + +impl IndexedData for ColliderHandle { + fn default() -> Self { + Self(IndexedData::default()) + } + + fn index(&self) -> usize { + self.0.index() + } +} + +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + /// Flags describing how the collider has been modified by the user. + pub struct ColliderChanges: u32 { + const MODIFIED = 1 << 0; + const PARENT = 1 << 1; // => BF & NF updates. + const POSITION = 1 << 2; // => BF & NF updates. + const GROUPS = 1 << 3; // => NF update. + const SHAPE = 1 << 4; // => BF & NF update. NF pair workspace invalidation. + const TYPE = 1 << 5; // => NF update. NF pair invalidation. + } +} + +impl Default for ColliderChanges { + fn default() -> Self { + ColliderChanges::empty() + } +} + +impl ColliderChanges { + pub fn needs_broad_phase_update(self) -> bool { + self.intersects( + ColliderChanges::PARENT | ColliderChanges::POSITION | ColliderChanges::SHAPE, + ) + } + + pub fn needs_narrow_phase_update(self) -> bool { + self.bits() > 1 + } +} + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum ColliderType { + Solid, + Sensor, +} + +impl ColliderType { + pub fn is_sensor(self) -> bool { + self == ColliderType::Sensor + } +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderBroadPhaseData { + pub(crate) proxy_index: SAPProxyIndex, +} + +impl Default for ColliderBroadPhaseData { + fn default() -> Self { + ColliderBroadPhaseData { + proxy_index: crate::INVALID_U32, + } + } +} + +pub type ColliderShape = SharedShape; + +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum ColliderMassProperties { + /// `MassProperties` are computed with the help of [`SharedShape::mass_properties`]. + Density(Real), + MassProperties(Box), +} + +impl Default for ColliderMassProperties { + fn default() -> Self { + ColliderMassProperties::Density(1.0) + } +} + +impl ColliderMassProperties { + pub fn mass_properties(&self, shape: &dyn Shape) -> MassProperties { + match self { + Self::Density(density) => shape.mass_properties(*density), + Self::MassProperties(mprops) => **mprops, + } + } +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderParent { + pub handle: RigidBodyHandle, + pub pos_wrt_parent: Isometry, +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderPosition(pub Isometry); + +impl AsRef> for ColliderPosition { + #[inline] + fn as_ref(&self) -> &Isometry { + &self.0 + } +} + +impl Deref for ColliderPosition { + type Target = Isometry; + #[inline] + fn deref(&self) -> &Isometry { + &self.0 + } +} + +impl Default for ColliderPosition { + fn default() -> Self { + Self::identity() + } +} + +impl ColliderPosition { + #[must_use] + fn identity() -> Self { + ColliderPosition(Isometry::identity()) + } +} + +impl From for ColliderPosition +where + Isometry: From, +{ + fn from(position: T) -> Self { + Self(position.into()) + } +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderGroups { + pub collision_groups: InteractionGroups, + pub solver_groups: InteractionGroups, +} + +impl Default for ColliderGroups { + fn default() -> Self { + Self { + collision_groups: InteractionGroups::default(), + solver_groups: InteractionGroups::default(), + } + } +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderMaterial { + pub friction: Real, + pub restitution: Real, + pub friction_combine_rule: CoefficientCombineRule, + pub restitution_combine_rule: CoefficientCombineRule, + pub solver_flags: SolverFlags, +} + +impl ColliderMaterial { + pub fn new(friction: Real, restitution: Real) -> Self { + Self { + friction, + restitution, + ..Default::default() + } + } +} + +impl Default for ColliderMaterial { + fn default() -> Self { + Self { + friction: 1.0, + restitution: 0.0, + friction_combine_rule: CoefficientCombineRule::default(), + restitution_combine_rule: CoefficientCombineRule::default(), + solver_flags: SolverFlags::default(), + } + } +} diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 7b453a3..1becc8c 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,78 +1,93 @@ use crate::data::arena::Arena; -use crate::data::pubsub::PubSub; -use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::collider::ColliderChanges; -use crate::geometry::{Collider, SAPProxyIndex}; -use parry::partitioning::IndexedData; +use crate::data::{ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ + Collider, ColliderBroadPhaseData, ColliderGroups, ColliderMassProperties, ColliderMaterial, + ColliderParent, ColliderPosition, ColliderShape, ColliderType, +}; +use crate::geometry::{ColliderChanges, ColliderHandle}; use std::ops::{Index, IndexMut}; -/// The unique identifier of a collider added to a collider set. -#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[repr(transparent)] -pub struct ColliderHandle(pub(crate) crate::data::arena::Index); - -impl ColliderHandle { - /// Converts this handle into its (index, generation) components. - pub fn into_raw_parts(self) -> (usize, u64) { - self.0.into_raw_parts() - } - - /// Reconstructs an handle from its (index, generation) components. - pub fn from_raw_parts(id: usize, generation: u64) -> Self { - Self(crate::data::arena::Index::from_raw_parts(id, generation)) - } - - /// An always-invalid collider handle. - pub fn invalid() -> Self { - Self(crate::data::arena::Index::from_raw_parts( - crate::INVALID_USIZE, - crate::INVALID_U64, - )) - } -} - -impl IndexedData for ColliderHandle { - fn default() -> Self { - Self(IndexedData::default()) - } - - fn index(&self) -> usize { - self.0.index() - } -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub(crate) struct RemovedCollider { - pub handle: ColliderHandle, - pub(crate) proxy_index: SAPProxyIndex, -} - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] /// A set of colliders that can be handled by a physics `World`. pub struct ColliderSet { - pub(crate) removed_colliders: PubSub, pub(crate) colliders: Arena, pub(crate) modified_colliders: Vec, - pub(crate) modified_all_colliders: bool, + pub(crate) removed_colliders: Vec, } +macro_rules! impl_field_component_set( + ($T: ty, $field: ident) => { + impl ComponentSetOption<$T> for ColliderSet { + fn get(&self, handle: crate::data::Index) -> Option<&$T> { + self.get(ColliderHandle(handle)).map(|b| &b.$field) + } + } + + impl ComponentSet<$T> for ColliderSet { + fn size_hint(&self) -> usize { + self.len() + } + + #[inline(always)] + fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) { + for (handle, body) in self.colliders.iter() { + f(handle, &body.$field) + } + } + } + + impl ComponentSetMut<$T> for ColliderSet { + fn set_internal(&mut self, handle: crate::data::Index, val: $T) { + if let Some(rb) = self.get_mut_internal(ColliderHandle(handle)) { + rb.$field = val; + } + } + + #[inline(always)] + fn map_mut_internal( + &mut self, + handle: crate::data::Index, + f: impl FnOnce(&mut $T) -> Result, + ) -> Option { + self.get_mut_internal(ColliderHandle(handle)).map(|rb| f(&mut rb.$field)) + } + } + } +); + +impl_field_component_set!(ColliderType, co_type); +impl_field_component_set!(ColliderShape, co_shape); +impl_field_component_set!(ColliderMassProperties, co_mprops); +impl_field_component_set!(ColliderChanges, co_changes); +impl_field_component_set!(ColliderParent, co_parent); +impl_field_component_set!(ColliderPosition, co_pos); +impl_field_component_set!(ColliderMaterial, co_material); +impl_field_component_set!(ColliderGroups, co_groups); +impl_field_component_set!(ColliderBroadPhaseData, co_bf_data); + impl ColliderSet { /// Create a new empty set of colliders. pub fn new() -> Self { ColliderSet { - removed_colliders: PubSub::new(), colliders: Arena::new(), modified_colliders: Vec::new(), - modified_all_colliders: false, + removed_colliders: Vec::new(), } } + pub(crate) fn take_modified(&mut self) -> Vec { + std::mem::replace(&mut self.modified_colliders, vec![]) + } + + pub(crate) fn take_removed(&mut self) -> Vec { + std::mem::replace(&mut self.removed_colliders, vec![]) + } + /// An always-invalid collider handle. pub fn invalid_handle() -> ColliderHandle { - ColliderHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + ColliderHandle::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32) } /// Iterate through all the colliders on this set. @@ -84,31 +99,11 @@ impl ColliderSet { #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn iter_mut(&mut self) -> impl Iterator { self.modified_colliders.clear(); - self.modified_all_colliders = true; - self.colliders - .iter_mut() - .map(|(h, b)| (ColliderHandle(h), b)) - } - - #[inline(always)] - pub(crate) fn foreach_modified_colliders(&self, mut f: impl FnMut(ColliderHandle, &Collider)) { - for handle in &self.modified_colliders { - if let Some(rb) = self.colliders.get(handle.0) { - f(*handle, rb) - } - } - } - - #[inline(always)] - pub(crate) fn foreach_modified_colliders_mut_internal( - &mut self, - mut f: impl FnMut(ColliderHandle, &mut Collider), - ) { - for handle in &self.modified_colliders { - if let Some(rb) = self.colliders.get_mut(handle.0) { - f(*handle, rb) - } - } + let modified_colliders = &mut self.modified_colliders; + self.colliders.iter_mut().map(move |(h, b)| { + modified_colliders.push(ColliderHandle(h)); + (ColliderHandle(h), b) + }) } /// The number of colliders on this set. @@ -126,29 +121,6 @@ impl ColliderSet { self.colliders.contains(handle.0) } - pub(crate) fn contains_any_modified_collider(&self) -> bool { - self.modified_all_colliders || !self.modified_colliders.is_empty() - } - - pub(crate) fn clear_modified_colliders(&mut self) { - if self.modified_all_colliders { - for collider in self.colliders.iter_mut() { - collider.1.changes = ColliderChanges::empty(); - } - self.modified_colliders.clear(); - self.modified_all_colliders = false; - } else { - for handle in self.modified_colliders.drain(..) { - // NOTE: if the collider was added, then removed from this set before - // a an update, then it will no longer exist in `self.colliders` - // so we need to do this `if let`. - if let Some(co) = self.colliders.get_mut(handle.0) { - co.changes = ColliderChanges::empty(); - } - } - } - } - /// Inserts a new collider to this set and retrieve its handle. pub fn insert( &mut self, @@ -159,20 +131,24 @@ 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.parent = parent_handle; + coll.co_parent.handle = parent_handle; // NOTE: we use `get_mut` instead of `get_mut_internal` so that the // modification flag is updated properly. let parent = bodies .get_mut_internal_with_modification_tracking(parent_handle) .expect("Parent rigid body not found."); - coll.position = parent.position * coll.delta; let handle = ColliderHandle(self.colliders.insert(coll)); self.modified_colliders.push(handle); - let coll = self.colliders.get(handle.0).unwrap(); - parent.add_collider(handle, &coll); + let coll = self.colliders.get_mut(handle.0).unwrap(); + parent.add_collider( + handle, + &mut coll.co_parent, + &mut coll.co_pos, + &coll.co_shape, + &coll.co_mprops, + ); handle } @@ -183,6 +159,7 @@ impl ColliderSet { pub fn remove( &mut self, handle: ColliderHandle, + islands: &mut IslandManager, bodies: &mut RigidBodySet, wake_up: bool, ) -> Option { @@ -191,25 +168,22 @@ impl ColliderSet { /* * Delete the collider from its parent body. */ - // NOTE: we use `get_mut` instead of `get_mut_internal` so that the + // 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(parent) = bodies.get_mut_internal_with_modification_tracking(collider.parent) { + if let Some(parent) = + bodies.get_mut_internal_with_modification_tracking(collider.co_parent.handle) + { parent.remove_collider_internal(handle, &collider); if wake_up { - bodies.wake_up(collider.parent, true); + islands.wake_up(bodies, collider.co_parent.handle, true); } } /* * Publish removal. */ - let message = RemovedCollider { - handle, - proxy_index: collider.proxy_index, - }; - - self.removed_colliders.publish(message); + self.removed_colliders.push(handle); Some(collider) } @@ -242,12 +216,7 @@ impl ColliderSet { pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> { let (collider, handle) = self.colliders.get_unknown_gen_mut(i)?; let handle = ColliderHandle(handle); - Self::mark_as_modified( - handle, - collider, - &mut self.modified_colliders, - self.modified_all_colliders, - ); + Self::mark_as_modified(handle, collider, &mut self.modified_colliders); Some((collider, handle)) } @@ -260,10 +229,9 @@ impl ColliderSet { handle: ColliderHandle, collider: &mut Collider, modified_colliders: &mut Vec, - modified_all_colliders: bool, ) { - if !modified_all_colliders && !collider.changes.contains(ColliderChanges::MODIFIED) { - collider.changes = ColliderChanges::MODIFIED; + if !collider.co_changes.contains(ColliderChanges::MODIFIED) { + collider.co_changes = ColliderChanges::MODIFIED; modified_colliders.push(handle); } } @@ -272,62 +240,20 @@ impl ColliderSet { #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { let result = self.colliders.get_mut(handle.0)?; - Self::mark_as_modified( - handle, - result, - &mut self.modified_colliders, - self.modified_all_colliders, - ); + Self::mark_as_modified(handle, result, &mut self.modified_colliders); Some(result) } pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { self.colliders.get_mut(handle.0) } +} - // Just a very long name instead of `.get_mut` to make sure - // this is really the method we wanted to use instead of `get_mut_internal`. - pub(crate) fn get_mut_internal_with_modification_tracking( - &mut self, - handle: ColliderHandle, - ) -> Option<&mut Collider> { - let result = self.colliders.get_mut(handle.0)?; - Self::mark_as_modified( - handle, - result, - &mut self.modified_colliders, - self.modified_all_colliders, - ); - Some(result) - } +impl Index for ColliderSet { + type Output = Collider; - // Utility function to avoid some borrowing issue in the `maintain` method. - fn maintain_one(bodies: &mut RigidBodySet, collider: &mut Collider) { - if collider - .changes - .contains(ColliderChanges::POSITION_WRT_PARENT) - { - if let Some(parent) = bodies.get_mut_internal(collider.parent()) { - let position = parent.position * collider.position_wrt_parent(); - // NOTE: the set_position method will add the ColliderChanges::POSITION flag, - // which is needed for the broad-phase/narrow-phase to detect the change. - collider.set_position(position); - } - } - } - - pub(crate) fn handle_user_changes(&mut self, bodies: &mut RigidBodySet) { - if self.modified_all_colliders { - for (_, rb) in self.colliders.iter_mut() { - Self::maintain_one(bodies, rb) - } - } else { - for handle in self.modified_colliders.iter() { - if let Some(rb) = self.colliders.get_mut(handle.0) { - Self::maintain_one(bodies, rb) - } - } - } + fn index(&self, index: crate::data::Index) -> &Collider { + &self.colliders[index] } } @@ -343,12 +269,7 @@ impl Index for ColliderSet { impl IndexMut for ColliderSet { fn index_mut(&mut self, handle: ColliderHandle) -> &mut Collider { let collider = &mut self.colliders[handle.0]; - Self::mark_as_modified( - handle, - collider, - &mut self.modified_colliders, - self.modified_all_colliders, - ); + Self::mark_as_modified(handle, collider, &mut self.modified_colliders); collider } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index d4e8ac4..5a568fa 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{BodyPair, RigidBodyHandle}; +use crate::dynamics::RigidBodyHandle; use crate::geometry::{ColliderPair, Contact, ContactManifold}; use crate::math::{Point, Real, Vector}; use parry::query::ContactManifoldsWorkspace; @@ -115,8 +115,10 @@ impl ContactPair { /// part of the same contact manifold share the same contact normal and contact kinematics. pub struct ContactManifoldData { // The following are set by the narrow-phase. - /// The pair of body involved in this contact manifold. - pub body_pair: BodyPair, + /// The first rigid-body involved in this contact manifold. + pub rigid_body1: Option, + /// The second rigid-body involved in this contact manifold. + pub rigid_body2: Option, pub(crate) warmstart_multiplier: Real, // The two following are set by the constraints solver. #[cfg_attr(feature = "serde-serialize", serde(skip))] @@ -207,17 +209,19 @@ impl SolverContact { impl Default for ContactManifoldData { fn default() -> Self { - Self::new( - BodyPair::new(RigidBodyHandle::invalid(), RigidBodyHandle::invalid()), - SolverFlags::empty(), - ) + Self::new(None, None, SolverFlags::empty()) } } impl ContactManifoldData { - pub(crate) fn new(body_pair: BodyPair, solver_flags: SolverFlags) -> ContactManifoldData { + pub(crate) fn new( + rigid_body1: Option, + rigid_body2: Option, + solver_flags: SolverFlags, + ) -> ContactManifoldData { Self { - body_pair, + rigid_body1, + rigid_body2, warmstart_multiplier: Self::min_warmstart_multiplier(), constraint_index: 0, position_constraint_index: 0, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 1c83232..9835bee 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,8 +1,7 @@ //! Structures related to geometry: colliders, shapes, etc. pub use self::broad_phase_multi_sap::BroadPhase; -pub use self::collider::{Collider, ColliderBuilder}; -pub use self::collider_set::{ColliderHandle, ColliderSet}; +pub use self::collider_components::*; pub use self::contact_pair::{ContactData, ContactManifoldData}; pub use self::contact_pair::{ContactPair, SolverContact, SolverFlags}; pub use self::interaction_graph::{ @@ -11,6 +10,11 @@ pub use self::interaction_graph::{ pub use self::interaction_groups::InteractionGroups; pub use self::narrow_phase::NarrowPhase; +#[cfg(feature = "default-sets")] +pub use self::collider::{Collider, ColliderBuilder}; +#[cfg(feature = "default-sets")] +pub use self::collider_set::ColliderSet; + pub use parry::query::TrackedContact; /// A contact between two colliders. @@ -85,7 +89,6 @@ impl IntersectionEvent { } pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair, SAPProxyIndex}; -pub(crate) use self::collider_set::RemovedCollider; pub(crate) use self::narrow_phase::ContactManifoldIndex; pub(crate) use parry::partitioning::SimdQuadTree; pub use parry::shape::*; @@ -102,9 +105,13 @@ pub(crate) fn default_query_dispatcher() -> std::sync::Arc, intersection_graph: InteractionGraph, graph_indices: Coarena, - removed_colliders: Option>, } pub(crate) type ContactManifoldIndex = usize; @@ -75,7 +76,6 @@ impl NarrowPhase { contact_graph: InteractionGraph::new(), intersection_graph: InteractionGraph::new(), graph_indices: Coarena::new(), - removed_colliders: None, } } @@ -172,75 +172,79 @@ impl NarrowPhase { // } /// Maintain the narrow-phase internal state by taking collider removal into account. - pub fn handle_user_changes( + pub fn handle_user_changes( &mut self, - colliders: &mut ColliderSet, - bodies: &mut RigidBodySet, + islands: &mut IslandManager, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + colliders: &mut Colliders, + bodies: &mut Bodies, events: &dyn EventHandler, - ) { - // Ensure we already subscribed. - if self.removed_colliders.is_none() { - self.removed_colliders = Some(colliders.removed_colliders.subscribe()); - } - - let cursor = self.removed_colliders.take().unwrap(); - + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + Colliders: ComponentSet + + ComponentSetOption + + ComponentSet, + { // TODO: avoid these hash-maps. // They are necessary to handle the swap-remove done internally // by the contact/intersection graphs when a node is removed. let mut prox_id_remap = HashMap::new(); let mut contact_id_remap = HashMap::new(); - let mut i = 0; - while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) { + for collider in removed_colliders { // NOTE: if the collider does not have any graph indices currently, there is nothing // to remove in the narrow-phase for this collider. - if let Some(graph_idx) = self.graph_indices.get(collider.handle.0) { + if let Some(graph_idx) = self.graph_indices.get(collider.0) { let intersection_graph_id = prox_id_remap - .get(&collider.handle) + .get(collider) .copied() .unwrap_or(graph_idx.intersection_graph_index); let contact_graph_id = contact_id_remap - .get(&collider.handle) + .get(collider) .copied() .unwrap_or(graph_idx.contact_graph_index); self.remove_collider( intersection_graph_id, contact_graph_id, + islands, colliders, bodies, &mut prox_id_remap, &mut contact_id_remap, ); } - - i += 1; } - colliders.removed_colliders.ack(&cursor); - self.removed_colliders = Some(cursor); - - self.handle_modified_colliders(colliders, bodies, events); + self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events); } - pub(crate) fn remove_collider( + pub(crate) fn remove_collider( &mut self, intersection_graph_id: ColliderGraphIndex, contact_graph_id: ColliderGraphIndex, - colliders: &mut ColliderSet, - bodies: &mut RigidBodySet, + islands: &mut IslandManager, + colliders: &mut Colliders, + bodies: &mut Bodies, prox_id_remap: &mut HashMap, contact_id_remap: &mut HashMap, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + Colliders: ComponentSetOption, + { // Wake up every body in contact with the deleted collider. for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) { - if let Some(parent) = colliders.get(a).map(|c| c.parent) { - bodies.wake_up(parent, true) + if let Some(parent) = colliders.get(a.0).map(|c| c.handle) { + islands.wake_up(bodies, parent, true) } - if let Some(parent) = colliders.get(b).map(|c| c.parent) { - bodies.wake_up(parent, true) + if let Some(parent) = colliders.get(b.0).map(|c| c.handle) { + islands.wake_up(bodies, parent, true) } } @@ -263,78 +267,104 @@ impl NarrowPhase { } } - pub(crate) fn handle_modified_colliders( + pub(crate) fn handle_modified_colliders( &mut self, - colliders: &mut ColliderSet, - bodies: &mut RigidBodySet, + islands: &mut IslandManager, + modified_colliders: &[ColliderHandle], + colliders: &Colliders, + bodies: &mut Bodies, events: &dyn EventHandler, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + Colliders: ComponentSet + + ComponentSetOption + + ComponentSet, + { let mut pairs_to_remove = vec![]; - colliders.foreach_modified_colliders(|handle, collider| { - if collider.changes.needs_narrow_phase_update() { - // No flag relevant to the narrow-phase is enabled for this collider. - return; - } + 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(gid) = self.graph_indices.get(handle.0) { - // For each modified colliders, we need to wake-up the bodies it is in contact with - // 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 static or kinematic body which don't propagate the wake-up state. - bodies.wake_up(collider.parent, true); - - for inter in self - .contact_graph - .interactions_with(gid.contact_graph_index) - { - let other_handle = if handle == inter.0 { inter.1 } else { inter.0 }; - if let Some(other_collider) = colliders.get(other_handle) { - bodies.wake_up(other_collider.parent, true); - } + if let Some(co_changes) = co_changes { + if co_changes.needs_narrow_phase_update() { + // No flag relevant to the narrow-phase is enabled for this collider. + return; } - // For each collider which had their sensor status modified, we need - // 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 collider.changes.contains(ColliderChanges::SENSOR) { - if collider.is_sensor() { - // Find the contact pairs for this collider and - // push them to `pairs_to_remove`. - for inter in self - .contact_graph - .interactions_with(gid.contact_graph_index) - { - pairs_to_remove.push(( - ColliderPair::new(inter.0, inter.1), - PairRemovalMode::FromContactGraph, - )); + if let Some(gid) = self.graph_indices.get(handle.0) { + // For each modified colliders, we need to wake-up the bodies it is in contact with + // 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 static 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(co_parent) = co_parent { + islands.wake_up(bodies, co_parent.handle, true); + } + + for inter in self + .contact_graph + .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); + + if let Some(other_parent) = other_parent { + islands.wake_up(bodies, other_parent.handle, true); } - } else { - // Find the contact pairs for this collider and - // push them to `pairs_to_remove` if both involved - // colliders are not sensors. - for inter in self - .intersection_graph - .interactions_with(gid.intersection_graph_index) - .filter(|(h1, h2, _)| { - !colliders[*h1].is_sensor() && !colliders[*h2].is_sensor() - }) - { - pairs_to_remove.push(( - ColliderPair::new(inter.0, inter.1), - PairRemovalMode::FromIntersectionGraph, - )); + } + + // For each collider which had their sensor status modified, we need + // 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() { + // Find the contact pairs for this collider and + // push them to `pairs_to_remove`. + for inter in self + .contact_graph + .interactions_with(gid.contact_graph_index) + { + pairs_to_remove.push(( + ColliderPair::new(inter.0, inter.1), + PairRemovalMode::FromContactGraph, + )); + } + } else { + // Find the contact pairs for this collider and + // push them to `pairs_to_remove` if both involved + // colliders are not sensors. + for inter in self + .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() + }) + { + pairs_to_remove.push(( + ColliderPair::new(inter.0, inter.1), + PairRemovalMode::FromIntersectionGraph, + )); + } } } } } - }); + } // Remove the pair from the relevant graph. for pair in &pairs_to_remove { - self.remove_pair(colliders, bodies, &pair.0, events, pair.1); + self.remove_pair(islands, colliders, bodies, &pair.0, events, pair.1); } // Add the paid removed pair to the relevant graph. @@ -343,17 +373,24 @@ impl NarrowPhase { } } - fn remove_pair( + fn remove_pair( &mut self, - colliders: &mut ColliderSet, - bodies: &mut RigidBodySet, + islands: &mut IslandManager, + colliders: &Colliders, + bodies: &mut Bodies, pair: &ColliderPair, events: &dyn EventHandler, mode: PairRemovalMode, - ) { - if let (Some(co1), Some(co2)) = - (colliders.get(pair.collider1), colliders.get(pair.collider2)) - { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + Colliders: ComponentSet + ComponentSetOption, + { + 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) { // 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)) = ( @@ -361,7 +398,8 @@ impl NarrowPhase { self.graph_indices.get(pair.collider2.0), ) { if mode == PairRemovalMode::FromIntersectionGraph - || (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor())) + || (mode == PairRemovalMode::Auto + && (co_type1.is_sensor() || co_type2.is_sensor())) { let was_intersecting = self .intersection_graph @@ -382,8 +420,18 @@ impl NarrowPhase { // Also wake up the dynamic bodies that were in contact. if let Some(ctct) = contact_pair { if ctct.has_any_active_contact { - bodies.wake_up(co1.parent, true); - bodies.wake_up(co2.parent, true); + let co_parent1: Option<&ColliderParent> = + colliders.get(pair.collider1.0); + let co_parent2: Option<&ColliderParent> = + colliders.get(pair.collider2.0); + + if let Some(co_parent1) = co_parent1 { + islands.wake_up(bodies, co_parent1.handle, true); + } + + if let Some(co_parent2) = co_parent2 { + islands.wake_up(bodies, co_parent2.handle, true); + } events.handle_contact_event(ContactEvent::Stopped( pair.collider1, @@ -396,11 +444,18 @@ impl NarrowPhase { } } - fn add_pair(&mut self, colliders: &mut ColliderSet, pair: &ColliderPair) { - if let (Some(co1), Some(co2)) = - (colliders.get(pair.collider1), colliders.get(pair.collider2)) - { - if co1.parent == co2.parent { + fn add_pair(&mut self, colliders: &Colliders, pair: &ColliderPair) + where + Colliders: ComponentSet + ComponentSetOption, + { + 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) { // Same parents. Ignore collisions. return; } @@ -411,7 +466,7 @@ impl NarrowPhase { ColliderGraphIndices::invalid(), ); - if co1.is_sensor() || co2.is_sensor() { + if co_type1.is_sensor() || co_type2.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) @@ -469,33 +524,56 @@ impl NarrowPhase { } } - pub(crate) fn register_pairs( + pub(crate) fn register_pairs( &mut self, - colliders: &mut ColliderSet, - bodies: &mut RigidBodySet, + islands: &mut IslandManager, + colliders: &Colliders, + bodies: &mut Bodies, broad_phase_events: &[BroadPhasePairEvent], events: &dyn EventHandler, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut, + Colliders: ComponentSet + ComponentSetOption, + { for event in broad_phase_events { match event { BroadPhasePairEvent::AddPair(pair) => { self.add_pair(colliders, pair); } BroadPhasePairEvent::DeletePair(pair) => { - self.remove_pair(colliders, bodies, pair, events, PairRemovalMode::Auto); + self.remove_pair( + islands, + colliders, + bodies, + pair, + events, + PairRemovalMode::Auto, + ); } } } } - pub(crate) fn compute_intersections( + pub(crate) fn compute_intersections( &mut self, - bodies: &RigidBodySet, - colliders: &ColliderSet, - hooks: &dyn PhysicsHooks, + bodies: &Bodies, + colliders: &Colliders, + modified_colliders: &[ColliderHandle], + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, - ) { - if !colliders.contains_any_modified_collider() { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSet + + ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { + if modified_colliders.is_empty() { return; } @@ -507,35 +585,66 @@ impl NarrowPhase { par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| { let handle1 = nodes[edge.source().index()].weight; let handle2 = nodes[edge.target().index()].weight; - let co1 = &colliders[handle1]; - let co2 = &colliders[handle2]; - if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update() + let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0); + let (co_changes1, co_groups1, co_shape1, co_pos1): ( + &ColliderChanges, + &ColliderGroups, + &ColliderShape, + &ColliderPosition, + ) = colliders.index_bundle(handle1.0); + + let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0); + let (co_changes2, co_groups2, co_shape2, co_pos2): ( + &ColliderChanges, + &ColliderGroups, + &ColliderShape, + &ColliderPosition, + ) = colliders.index_bundle(handle2.0); + + if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update() { // No update needed for these colliders. return; } // TODO: avoid lookup into bodies. - let rb1 = &bodies[co1.parent]; - let rb2 = &bodies[co2.parent]; + let (mut sleeping1, mut status1) = (true, RigidBodyType::Static); + let (mut sleeping2, mut status2) = (true, RigidBodyType::Static); - if (rb1.is_sleeping() && rb2.is_static()) - || (rb2.is_sleeping() && rb1.is_static()) - || (rb1.is_sleeping() && rb2.is_sleeping()) + if let Some(co_parent1) = co_parent1 { + let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) = + bodies.index_bundle(co_parent1.handle.0); + status1 = *rb_type1; + sleeping1 = rb_activation1.sleeping; + } + + if let Some(co_parent2) = co_parent2 { + let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) = + bodies.index_bundle(co_parent2.handle.0); + status2 = *rb_type2; + sleeping2 = rb_activation2.sleeping; + } + + if (sleeping1 && status2.is_static()) + || (sleeping2 && status1.is_static()) + || (sleeping1 && sleeping2) { // No need to update this intersection because nothing moved. return; } - if !co1.collision_groups.test(co2.collision_groups) { + if !co_groups1 + .collision_groups + .test(co_groups2.collision_groups) + { // The intersection is not allowed. return; } if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) - && !rb1.is_dynamic() - && !rb2.is_dynamic() + && !status1.is_dynamic() + && !status2.is_dynamic() { // Default filtering rule: no intersection between two non-dynamic bodies. return; @@ -543,12 +652,12 @@ impl NarrowPhase { if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) { let context = PairFilterContext { - rigid_body1: rb1, - rigid_body2: rb2, - collider_handle1: handle1, - collider_handle2: handle2, - collider1: co1, - collider2: co2, + bodies, + colliders, + rigid_body1: co_parent1.map(|p| p.handle), + rigid_body2: co_parent2.map(|p| p.handle), + collider1: handle1, + collider2: handle2, }; if !hooks.filter_intersection_pair(&context) { @@ -557,10 +666,10 @@ impl NarrowPhase { } } - let pos12 = co1.position().inv_mul(co2.position()); + let pos12 = co_pos1.inv_mul(co_pos2); if let Ok(intersection) = - query_dispatcher.intersection_test(&pos12, co1.shape(), co2.shape()) + query_dispatcher.intersection_test(&pos12, &**co_shape1, &**co_shape2) { if intersection != edge.weight { edge.weight = intersection; @@ -574,15 +683,26 @@ impl NarrowPhase { }); } - pub(crate) fn compute_contacts( + pub(crate) fn compute_contacts( &mut self, prediction_distance: Real, - bodies: &RigidBodySet, - colliders: &ColliderSet, - hooks: &dyn PhysicsHooks, + bodies: &Bodies, + colliders: &Colliders, + modified_colliders: &[ColliderHandle], + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, - ) { - if !colliders.contains_any_modified_collider() { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSet + + ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + { + if modified_colliders.is_empty() { return; } @@ -592,35 +712,68 @@ impl NarrowPhase { // TODO: don't iterate on all the edges. par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { let pair = &mut edge.weight; - let co1 = &colliders[pair.pair.collider1]; - let co2 = &colliders[pair.pair.collider2]; - if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update() + let co_parent1: Option<&ColliderParent> = colliders.get(pair.pair.collider1.0); + let (co_changes1, co_groups1, co_shape1, co_pos1, co_material1): ( + &ColliderChanges, + &ColliderGroups, + &ColliderShape, + &ColliderPosition, + &ColliderMaterial, + ) = colliders.index_bundle(pair.pair.collider1.0); + + let co_parent2: Option<&ColliderParent> = colliders.get(pair.pair.collider2.0); + let (co_changes2, co_groups2, co_shape2, co_pos2, co_material2): ( + &ColliderChanges, + &ColliderGroups, + &ColliderShape, + &ColliderPosition, + &ColliderMaterial, + ) = colliders.index_bundle(pair.pair.collider2.0); + + if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update() { // No update needed for these colliders. return; } // TODO: avoid lookup into bodies. - let rb1 = &bodies[co1.parent]; - let rb2 = &bodies[co2.parent]; + let (mut sleeping1, mut status1) = (true, RigidBodyType::Static); + let (mut sleeping2, mut status2) = (true, RigidBodyType::Static); - if (rb1.is_sleeping() && rb2.is_static()) - || (rb2.is_sleeping() && rb1.is_static()) - || (rb1.is_sleeping() && rb2.is_sleeping()) + if let Some(co_parent1) = co_parent1 { + let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) = + bodies.index_bundle(co_parent1.handle.0); + status1 = *rb_type1; + sleeping1 = rb_activation1.sleeping; + } + + if let Some(co_parent2) = co_parent2 { + let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) = + bodies.index_bundle(co_parent2.handle.0); + status2 = *rb_type2; + sleeping2 = rb_activation2.sleeping; + } + + if (sleeping1 && status2.is_static()) + || (sleeping2 && status1.is_static()) + || (sleeping1 && sleeping2) { - // No need to update this contact because nothing moved. + // No need to update this intersection because nothing moved. return; } - if !co1.collision_groups.test(co2.collision_groups) { + if !co_groups1 + .collision_groups + .test(co_groups2.collision_groups) + { // The collision is not allowed. return; } if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR) - && !rb1.is_dynamic() - && !rb2.is_dynamic() + && !status1.is_dynamic() + && !status2.is_dynamic() { // Default filtering rule: no contact between two non-dynamic bodies. return; @@ -629,12 +782,12 @@ impl NarrowPhase { let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR) { let context = PairFilterContext { - rigid_body1: rb1, - rigid_body2: rb2, - collider_handle1: pair.pair.collider1, - collider_handle2: pair.pair.collider2, - collider1: co1, - collider2: co2, + bodies, + colliders, + rigid_body1: co_parent1.map(|p| p.handle), + rigid_body2: co_parent2.map(|p| p.handle), + collider1: pair.pair.collider1, + collider2: pair.pair.collider2, }; if let Some(solver_flags) = hooks.filter_contact_pair(&context) { @@ -644,25 +797,25 @@ impl NarrowPhase { return; } } else { - co1.solver_flags | co2.solver_flags + co_material1.solver_flags | co_material2.solver_flags }; - if !co1.solver_groups.test(co2.solver_groups) { + if !co_groups1.solver_groups.test(co_groups2.solver_groups) { solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); } - if co1.changes.contains(ColliderChanges::SHAPE) - || co2.changes.contains(ColliderChanges::SHAPE) + if co_changes1.contains(ColliderChanges::SHAPE) + || co_changes2.contains(ColliderChanges::SHAPE) { // The shape changed so the workspace is no longer valid. pair.workspace = None; } - let pos12 = co1.position().inv_mul(co2.position()); + let pos12 = co_pos1.inv_mul(co_pos2); let _ = query_dispatcher.contact_manifolds( &pos12, - co1.shape(), - co2.shape(), + &**co_shape1, + &**co_shape2, prediction_distance, &mut pair.manifolds, &mut pair.workspace, @@ -671,25 +824,34 @@ impl NarrowPhase { let mut has_any_active_contact = false; let friction = CoefficientCombineRule::combine( - co1.friction, - co2.friction, - co1.flags.friction_combine_rule_value(), - co2.flags.friction_combine_rule_value(), + co_material1.friction, + co_material2.friction, + co_material1.friction_combine_rule as u8, + co_material2.friction_combine_rule as u8, ); let restitution = CoefficientCombineRule::combine( - co1.restitution, - co2.restitution, - co1.flags.restitution_combine_rule_value(), - co2.flags.restitution_combine_rule_value(), + co_material1.restitution, + co_material2.restitution, + co_material1.restitution_combine_rule as u8, + co_material2.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)) + .unwrap_or(zero); + let dominance2 = co_parent2 + .map(|p2| *bodies.index(p2.handle.0)) + .unwrap_or(zero); + for manifold in &mut pair.manifolds { - let world_pos1 = manifold.subshape_pos1.prepend_to(co1.position()); + let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1); manifold.data.solver_contacts.clear(); - manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent()); + manifold.data.rigid_body1 = co_parent1.map(|p| p.handle); + manifold.data.rigid_body2 = co_parent2.map(|p| p.handle); manifold.data.solver_flags = solver_flags; manifold.data.relative_dominance = - rb1.effective_dominance_group() - rb2.effective_dominance_group(); + dominance1.effective_group(&status1) - dominance2.effective_group(&status2); manifold.data.normal = world_pos1 * manifold.local_n1; // Generate solver contacts. @@ -732,12 +894,12 @@ impl NarrowPhase { let mut modifiable_normal = manifold.data.normal; let mut context = ContactModificationContext { - rigid_body1: rb1, - rigid_body2: rb2, - collider_handle1: pair.pair.collider1, - collider_handle2: pair.pair.collider2, - collider1: co1, - collider2: co2, + bodies, + colliders, + rigid_body1: co_parent1.map(|p| p.handle), + rigid_body2: co_parent2.map(|p| p.handle), + collider1: pair.pair.collider1, + collider2: pair.pair.collider2, manifold, solver_contacts: &mut modifiable_solver_contacts, normal: &mut modifiable_normal, @@ -772,38 +934,61 @@ impl NarrowPhase { /// Retrieve all the interactions with at least one contact point, happening between two active bodies. // NOTE: this is very similar to the code from JointSet::select_active_interactions. - pub(crate) fn select_active_contacts<'a>( + pub(crate) fn select_active_contacts<'a, Bodies>( &'a mut self, - bodies: &RigidBodySet, + islands: &IslandManager, + bodies: &Bodies, out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut Vec>, - ) { - for out_island in &mut out[..bodies.num_islands()] { + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet, + { + for out_island in &mut out[..islands.num_islands()] { out_island.clear(); } // TODO: don't iterate through all the interactions. for inter in self.contact_graph.graph.edges.iter_mut() { for manifold in &mut inter.weight.manifolds { - let rb1 = &bodies[manifold.data.body_pair.body1]; - let rb2 = &bodies[manifold.data.body_pair.body2]; if manifold .data .solver_flags .contains(SolverFlags::COMPUTE_IMPULSES) && manifold.data.num_active_contacts() != 0 - && (rb1.is_dynamic() || rb2.is_dynamic()) - && (!rb1.is_dynamic() || !rb1.is_sleeping()) - && (!rb2.is_dynamic() || !rb2.is_sleeping()) { - let island_index = if !rb1.is_dynamic() { - rb2.active_island_id - } else { - rb1.active_island_id - }; + let (active_island_id1, status1, 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) + } else { + (0, RigidBodyType::Static, true) + }; - out[island_index].push(out_manifolds.len()); - out_manifolds.push(manifold); + let (active_island_id2, status2, 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) + } else { + (0, RigidBodyType::Static, true) + }; + + if (status1.is_dynamic() || status2.is_dynamic()) + && (!status1.is_dynamic() || !sleeping1) + && (!status2.is_dynamic() || !sleeping2) + { + let island_index = if !status1.is_dynamic() { + active_island_id2 + } else { + active_island_id1 + }; + + out[island_index].push(out_manifolds.len()); + out_manifolds.push(manifold); + } } } } diff --git a/src/lib.rs b/src/lib.rs index 564a758..46f6c6e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -8,7 +8,8 @@ //! - The ability to run a perfectly deterministic simulation on different machine, as long as they //! are compliant with the IEEE 754-2008 floating point standard. -#![warn(missing_docs)] +#![deny(bare_trait_objects)] +// #![warn(missing_docs)] // TODO: re-enable this #[cfg(all(feature = "dim2", feature = "f32"))] pub extern crate parry2d as parry; @@ -49,8 +50,8 @@ macro_rules! enable_flush_to_zero( ); #[cfg(feature = "simd-is-enabled")] -macro_rules! array( - ($callback: expr; SIMD_WIDTH) => { +macro_rules! gather( + ($callback: expr) => { { #[inline(always)] #[allow(dead_code)] @@ -122,7 +123,6 @@ macro_rules! try_ret { // } pub(crate) const INVALID_U32: u32 = u32::MAX; -pub(crate) const INVALID_U64: u64 = u64::MAX; pub(crate) const INVALID_USIZE: usize = INVALID_U32 as usize; /// The string version of Rapier. diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index da572f3..074ba75 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,7 +1,11 @@ //! Physics pipeline structures. -use crate::dynamics::{JointSet, RigidBodySet}; -use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; +use crate::data::{ComponentSet, ComponentSetMut}; +use crate::dynamics::{ + IslandManager, JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, + RigidBodyIds, RigidBodyType, RigidBodyVelocity, +}; +use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderShape, NarrowPhase}; use crate::math::Real; use crate::pipeline::{EventHandler, PhysicsHooks}; @@ -34,46 +38,25 @@ impl CollisionPipeline { } /// Executes one step of the collision detection. - pub fn step( + pub fn step( &mut self, - prediction_distance: Real, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - hooks: &dyn PhysicsHooks, - events: &dyn EventHandler, - ) { - colliders.handle_user_changes(bodies); - bodies.handle_user_changes(colliders); - self.broadphase_collider_pairs.clear(); - - self.broad_phase_events.clear(); - broad_phase.update(prediction_distance, colliders, &mut self.broad_phase_events); - - narrow_phase.handle_user_changes(colliders, bodies, events); - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events); - narrow_phase.compute_intersections(bodies, colliders, hooks, events); - - bodies.update_active_set_with_contacts( - colliders, - narrow_phase, - self.empty_joints.joint_graph(), - 128, - ); - - // Update colliders positions and kinematic bodies positions. - bodies.foreach_active_body_mut_internal(|_, rb| { - rb.position = rb.next_position; - rb.update_colliders_positions(colliders); - - for handle in &rb.colliders { - let collider = colliders.get_mut_internal(*handle).unwrap(); - collider.position = rb.position * collider.delta; - } - }); - - bodies.modified_inactive_set.clear(); + _prediction_distance: Real, + _broad_phase: &mut BroadPhase, + _narrow_phase: &mut NarrowPhase, + _islands: &mut IslandManager, + _bodies: &mut Bodies, + _colliders: &mut Colliders, + _hooks: &dyn PhysicsHooks, + _events: &dyn EventHandler, + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSetMut + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut, + { + unimplemented!() } } diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 5fad9bc..ad288d6 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -13,3 +13,4 @@ mod event_handler; mod physics_hooks; mod physics_pipeline; mod query_pipeline; +mod user_changes; diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index 72b635f..c4ef245 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -1,38 +1,38 @@ -use crate::dynamics::RigidBody; -use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags}; +use crate::dynamics::RigidBodyHandle; +use crate::geometry::{ColliderHandle, ContactManifold, SolverContact, SolverFlags}; use crate::math::{Real, Vector}; use na::ComplexField; /// Context given to custom collision filters to filter-out collisions. -pub struct PairFilterContext<'a> { - /// The first collider involved in the potential collision. - pub rigid_body1: &'a RigidBody, - /// The first collider involved in the potential collision. - pub rigid_body2: &'a RigidBody, - /// The first collider involved in the potential collision. - pub collider_handle1: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider_handle2: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider1: &'a Collider, - /// The first collider involved in the potential collision. - pub collider2: &'a Collider, +pub struct PairFilterContext<'a, Bodies, Colliders> { + /// The set of rigid-bodies. + pub bodies: &'a Bodies, + /// The set of colliders. + pub colliders: &'a Colliders, + /// The handle of the first collider involved in the potential collision. + pub collider1: ColliderHandle, + /// The handle of the first collider involved in the potential collision. + pub collider2: ColliderHandle, + /// The handle of the first body involved in the potential collision. + pub rigid_body1: Option, + /// The handle of the first body involved in the potential collision. + pub rigid_body2: Option, } /// Context given to custom contact modifiers to modify the contacts seen by the constraints solver. -pub struct ContactModificationContext<'a> { - /// The first collider involved in the potential collision. - pub rigid_body1: &'a RigidBody, - /// The first collider involved in the potential collision. - pub rigid_body2: &'a RigidBody, - /// The first collider involved in the potential collision. - pub collider_handle1: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider_handle2: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider1: &'a Collider, - /// The first collider involved in the potential collision. - pub collider2: &'a Collider, +pub struct ContactModificationContext<'a, Bodies, Colliders> { + /// The set of rigid-bodies. + pub bodies: &'a Bodies, + /// The set of colliders. + pub colliders: &'a Colliders, + /// The handle of the first collider involved in the potential collision. + pub collider1: ColliderHandle, + /// The handle of the first collider involved in the potential collision. + pub collider2: ColliderHandle, + /// The handle of the first body involved in the potential collision. + pub rigid_body1: Option, + /// The handle of the first body involved in the potential collision. + pub rigid_body2: Option, /// The contact manifold. pub manifold: &'a ContactManifold, /// The solver contacts that can be modified. @@ -45,7 +45,7 @@ pub struct ContactModificationContext<'a> { pub user_data: &'a mut u32, } -impl<'a> ContactModificationContext<'a> { +impl<'a, Bodies, Colliders> ContactModificationContext<'a, Bodies, Colliders> { /// Helper function to update `self` to emulate a oneway-platform. /// /// The "oneway" behavior will only allow contacts between two colliders @@ -127,9 +127,14 @@ bitflags::bitflags! { const MODIFY_SOLVER_CONTACTS = 0b0100; } } +impl Default for PhysicsHooksFlags { + fn default() -> Self { + PhysicsHooksFlags::empty() + } +} /// User-defined functions called by the physics engines during one timestep in order to customize its behavior. -pub trait PhysicsHooks: Send + Sync { +pub trait PhysicsHooks: Send + Sync { /// The sets of hooks that must be taken into account. fn active_hooks(&self) -> PhysicsHooksFlags; @@ -156,7 +161,10 @@ pub trait PhysicsHooks: Send + Sync { /// will be taken into account by the constraints solver. If this returns /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. - fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { + fn filter_contact_pair( + &self, + _context: &PairFilterContext, + ) -> Option { None } @@ -179,7 +187,7 @@ pub trait PhysicsHooks: Send + Sync { /// not compute any intersection information for it. /// If this return `true` then the narrow-phase will compute intersection /// information for this pair. - fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { false } @@ -207,21 +215,22 @@ pub trait PhysicsHooks: Send + Sync { /// as 0 and can be modified in `context.user_data`. /// /// The world-space contact normal can be modified in `context.normal`. - fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {} + fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) { + } } -impl PhysicsHooks for () { +impl PhysicsHooks for () { fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::empty() } - fn filter_contact_pair(&self, _: &PairFilterContext) -> Option { + fn filter_contact_pair(&self, _: &PairFilterContext) -> Option { None } - fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { false } - fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} + fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f4ac531..5a7a827 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -1,17 +1,28 @@ //! Physics pipeline structures. use crate::counters::Counters; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; -use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; +use crate::dynamics::{ + CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyActivation, RigidBodyCcd, + RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces, + RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + RigidBodyVelocity, +}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups, + ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, + ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase, }; use crate::math::{Real, Vector}; use crate::pipeline::{EventHandler, PhysicsHooks}; +#[cfg(feature = "default-sets")] +use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; + /// The physics pipeline, responsible for stepping the whole physics simulation. /// /// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh @@ -58,17 +69,43 @@ impl PhysicsPipeline { } } - fn detect_collisions( + fn clear_modified_colliders( + &mut self, + colliders: &mut impl ComponentSetMut, + modified_colliders: &mut Vec, + ) { + for handle in modified_colliders.drain(..) { + colliders.set_internal(handle.0, ColliderChanges::empty()) + } + } + + fn detect_collisions( &mut self, integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - hooks: &dyn PhysicsHooks, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, handle_user_changes: bool, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { self.counters.stages.collision_detection_time.resume(); self.counters.cd.broad_phase_time.resume(); @@ -78,6 +115,8 @@ impl PhysicsPipeline { broad_phase.update( integration_parameters.prediction_distance, colliders, + modified_colliders, + removed_colliders, &mut self.broad_phase_events, ); @@ -86,37 +125,46 @@ impl PhysicsPipeline { // Update narrow-phase. if handle_user_changes { - narrow_phase.handle_user_changes(colliders, bodies, events); + narrow_phase.handle_user_changes( + islands, + modified_colliders, + removed_colliders, + colliders, + bodies, + events, + ); } - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); + narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events); narrow_phase.compute_contacts( integration_parameters.prediction_distance, bodies, colliders, + modified_colliders, hooks, events, ); - narrow_phase.compute_intersections(bodies, colliders, hooks, events); - - // Clear colliders modification flags. - colliders.clear_modified_colliders(); + narrow_phase.compute_intersections(bodies, colliders, modified_colliders, hooks, events); self.counters.cd.narrow_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); } - fn solve_position_constraints( + fn solve_position_constraints( &mut self, integration_parameters: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + islands: &IslandManager, + bodies: &mut Bodies, + ) where + Bodies: ComponentSet + ComponentSetMut, + { #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..bodies.num_islands() { + for island_id in 0..islands.num_islands() { self.solvers[island_id].solve_position_constraints( island_id, + islands, &mut self.counters, integration_parameters, bodies, @@ -129,7 +177,7 @@ impl PhysicsPipeline { use rayon::prelude::*; use std::sync::atomic::Ordering; - let num_islands = bodies.num_islands(); + let num_islands = ilands.num_islands(); let solvers = &mut self.solvers[..num_islands]; let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); @@ -140,7 +188,7 @@ impl PhysicsPipeline { .par_iter_mut() .enumerate() .for_each(|(island_id, solver)| { - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; solver.solve_position_constraints( @@ -154,17 +202,30 @@ impl PhysicsPipeline { } } - fn build_islands_and_solve_velocity_constraints( + fn build_islands_and_solve_velocity_constraints( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, + bodies: &mut Bodies, + colliders: &mut Colliders, joints: &mut JointSet, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption, + { self.counters.stages.island_construction_time.resume(); - bodies.update_active_set_with_contacts( + islands.update_active_set_with_contacts( + bodies, colliders, narrow_phase, joints.joint_graph(), @@ -172,42 +233,58 @@ impl PhysicsPipeline { ); self.counters.stages.island_construction_time.pause(); - if self.manifold_indices.len() < bodies.num_islands() { + if self.manifold_indices.len() < islands.num_islands() { self.manifold_indices - .resize(bodies.num_islands(), Vec::new()); + .resize(islands.num_islands(), Vec::new()); } - if self.joint_constraint_indices.len() < bodies.num_islands() { + if self.joint_constraint_indices.len() < islands.num_islands() { self.joint_constraint_indices - .resize(bodies.num_islands(), Vec::new()); + .resize(islands.num_islands(), Vec::new()); } let mut manifolds = Vec::new(); - narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices); - joints.select_active_interactions(bodies, &mut self.joint_constraint_indices); + narrow_phase.select_active_contacts( + islands, + bodies, + &mut manifolds, + &mut self.manifold_indices, + ); + joints.select_active_interactions(islands, bodies, &mut self.joint_constraint_indices); self.counters.stages.update_time.resume(); - bodies.foreach_active_dynamic_body_mut_internal(|_, b| { - b.update_world_mass_properties(); - b.add_gravity(*gravity) - }); + 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.add_linear_acceleration(&gravity, effective_inv_mass) + }); + } self.counters.stages.update_time.pause(); self.counters.stages.solver_time.resume(); - if self.solvers.len() < bodies.num_islands() { + if self.solvers.len() < islands.num_islands() { self.solvers - .resize_with(bodies.num_islands(), IslandSolver::new); + .resize_with(islands.num_islands(), IslandSolver::new); } #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..bodies.num_islands() { + for island_id in 0..islands.num_islands() { self.solvers[island_id].init_constraints_and_solve_velocity_constraints( island_id, &mut self.counters, integration_parameters, + islands, bodies, &mut manifolds[..], &self.manifold_indices[island_id], @@ -238,7 +315,7 @@ impl PhysicsPipeline { .par_iter_mut() .enumerate() .for_each(|(island_id, solver)| { - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let manifolds: &mut Vec<&mut ContactManifold> = unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; @@ -261,19 +338,32 @@ impl PhysicsPipeline { self.counters.stages.solver_time.pause(); } - fn run_ccd_motion_clamping( + fn run_ccd_motion_clamping( &mut self, integration_parameters: &IntegrationParameters, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, + islands: &IslandManager, + bodies: &mut Bodies, + colliders: &mut Colliders, narrow_phase: &NarrowPhase, ccd_solver: &mut CCDSolver, events: &dyn EventHandler, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { self.counters.ccd.toi_computation_time.start(); // Handle CCD let impacts = ccd_solver.predict_impacts_at_next_positions( integration_parameters.dt, + islands, bodies, colliders, narrow_phase, @@ -283,74 +373,176 @@ impl PhysicsPipeline { self.counters.ccd.toi_computation_time.pause(); } - fn advance_to_final_positions( + fn advance_to_final_positions( &mut self, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, + islands: &IslandManager, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_colliders: &mut Vec, clear_forces: bool, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetOption, + { // Set the rigid-bodies and kinematic bodies to their final position. - bodies.foreach_active_body_mut_internal(|_, rb| { - if rb.is_kinematic() { - rb.linvel = na::zero(); - rb.angvel = na::zero(); + for handle in islands.iter_active_bodies() { + let status: &RigidBodyType = bodies.index(handle.0); + if status.is_kinematic() { + bodies.set_internal(handle.0, RigidBodyVelocity::zero()); } if clear_forces { - rb.force = na::zero(); - rb.torque = na::zero(); + bodies.map_mut_internal(handle.0, |f: &mut RigidBodyForces| { + f.torque = na::zero(); + f.force = na::zero(); + }); } - rb.position = rb.next_position; - rb.update_colliders_positions(colliders); - }); + 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); + } } - fn interpolate_kinematic_velocities( + fn interpolate_kinematic_velocities( &mut self, integration_parameters: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + islands: &IslandManager, + bodies: &mut Bodies, + ) where + Bodies: ComponentSetMut + ComponentSet, + { // Update kinematic bodies velocities. // TODO: what is the best place for this? It should at least be // located before the island computation because we test the velocity // there to determine if this kinematic body should wake-up dynamic // bodies it is touching. - bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - body.compute_velocity_from_next_position(integration_parameters.inv_dt()); - }); + for handle in islands.active_kinematic_bodies() { + let ppos: &RigidBodyPosition = bodies.index(handle.0); + let new_vel = ppos.interpolate_velocity(integration_parameters.inv_dt()); + bodies.set_internal(handle.0, new_vel); + } } - /// Executes one timestep of the physics simulation. + #[cfg(feature = "default-sets")] pub fn step( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, ccd_solver: &mut CCDSolver, - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { - self.counters.reset(); - self.counters.step_started(); - colliders.handle_user_changes(bodies); - bodies.handle_user_changes(colliders); + let mut modified_bodies = bodies.take_modified(); + let mut modified_colliders = colliders.take_modified(); + let mut removed_colliders = colliders.take_removed(); - self.detect_collisions( + self.step_generic( + gravity, integration_parameters, + islands, broad_phase, narrow_phase, bodies, colliders, + &mut modified_bodies, + &mut modified_colliders, + &mut removed_colliders, + joints, + ccd_solver, + hooks, + events, + ); + } + + /// Executes one timestep of the physics simulation. + pub fn step_generic( + &mut self, + gravity: &Vector, + integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_bodies: &mut Vec, + modified_colliders: &mut Vec, + removed_colliders: &mut Vec, + joints: &mut JointSet, + ccd_solver: &mut CCDSolver, + hooks: &dyn PhysicsHooks, + events: &dyn EventHandler, + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { + self.counters.reset(); + self.counters.step_started(); + + super::user_changes::handle_user_changes_to_colliders( + bodies, + colliders, + &modified_colliders[..], + ); + super::user_changes::handle_user_changes_to_rigid_bodies( + islands, + bodies, + colliders, + &modified_bodies, + modified_colliders, + ); + + self.detect_collisions( + integration_parameters, + islands, + broad_phase, + narrow_phase, + bodies, + colliders, + &modified_colliders[..], + removed_colliders, hooks, events, true, ); + self.clear_modified_colliders(colliders, modified_colliders); + removed_colliders.clear(); + let mut remaining_time = integration_parameters.dt; let mut integration_parameters = *integration_parameters; @@ -375,9 +567,16 @@ impl PhysicsPipeline { if ccd_is_enabled && remaining_substeps > 1 { // NOTE: Take forces into account when updating the bodies CCD activation flags // these forces have not been integrated to the body's velocity yet. - let ccd_active = ccd_solver.update_ccd_active_flags(bodies, remaining_time, true); + let ccd_active = + ccd_solver.update_ccd_active_flags(islands, bodies, remaining_time, true); let first_impact = if ccd_active { - ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase) + ccd_solver.find_first_impact( + remaining_time, + islands, + bodies, + colliders, + narrow_phase, + ) } else { None }; @@ -414,10 +613,11 @@ impl PhysicsPipeline { self.counters.ccd.num_substeps += 1; - self.interpolate_kinematic_velocities(&integration_parameters, bodies); + self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies); self.build_islands_and_solve_velocity_constraints( gravity, &integration_parameters, + islands, narrow_phase, bodies, colliders, @@ -428,11 +628,16 @@ impl PhysicsPipeline { if ccd_is_enabled { // NOTE: don't the forces into account when updating the CCD active flags because // they have already been integrated into the velocities by the solver. - let ccd_active = - ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt, false); + let ccd_active = ccd_solver.update_ccd_active_flags( + islands, + bodies, + integration_parameters.dt, + false, + ); if ccd_active { self.run_ccd_motion_clamping( &integration_parameters, + islands, bodies, colliders, narrow_phase, @@ -449,22 +654,31 @@ impl PhysicsPipeline { // This happens because our CCD use the real rigid-body // velocities instead of just interpolating between // isometries. - self.solve_position_constraints(&integration_parameters, bodies); + self.solve_position_constraints(&integration_parameters, islands, bodies); let clear_forces = remaining_substeps == 0; - self.advance_to_final_positions(bodies, colliders, clear_forces); + self.advance_to_final_positions( + islands, + bodies, + colliders, + modified_colliders, + clear_forces, + ); self.detect_collisions( &integration_parameters, + islands, broad_phase, narrow_phase, bodies, colliders, + modified_colliders, + removed_colliders, hooks, events, false, ); - bodies.modified_inactive_set.clear(); + self.clear_modified_colliders(colliders, modified_colliders); } self.counters.step_completed(); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index cb56141..3139a88 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,9 +1,14 @@ -use crate::dynamics::RigidBodySet; +use crate::data::{BundleSet, ComponentSet, ComponentSetOption}; +use crate::dynamics::{ + IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition, + RigidBodyVelocity, +}; use crate::geometry::{ - Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray, - RayIntersection, SimdQuadTree, AABB, + ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, + InteractionGroups, PointProjection, Ray, RayIntersection, SimdQuadTree, AABB, }; use crate::math::{Isometry, Point, Real, Vector}; +use parry::partitioning::SimdQuadtreeDataGenerator; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -32,11 +37,11 @@ pub struct QueryPipeline { dilation_factor: Real, } -struct QueryPipelineAsCompositeShape<'a> { +struct QueryPipelineAsCompositeShape<'a, Colliders> { query_pipeline: &'a QueryPipeline, - colliders: &'a ColliderSet, + colliders: &'a Colliders, query_groups: InteractionGroups, - filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>, + filter: Option<&'a dyn Fn(ColliderHandle) -> bool>, } /// Indicates how the colliders position should be taken into account when @@ -55,7 +60,12 @@ pub enum QueryPipelineMode { }, } -impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { +impl<'a, Colliders> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a, Colliders> +where + // TODO ECS: make everything optional but the shape? + Colliders: + ComponentSet + ComponentSet + ComponentSet, +{ type PartShape = dyn Shape; type PartId = ColliderHandle; @@ -64,11 +74,15 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { shape_id: Self::PartId, mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), ) { - if let Some(collider) = self.colliders.get(shape_id) { - if collider.collision_groups.test(self.query_groups) - && self.filter.map(|f| f(shape_id, collider)).unwrap_or(true) + let co_groups: Option<&ColliderGroups> = self.colliders.get(shape_id.0); + + if let Some(co_groups) = co_groups { + if co_groups.collision_groups.test(self.query_groups) + && self.filter.map(|f| f(shape_id)).unwrap_or(true) { - f(Some(collider.position()), collider.shape()) + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + self.colliders.index_bundle(shape_id.0); + f(Some(co_pos), &**co_shape) } } } @@ -98,12 +112,12 @@ impl QueryPipeline { Self::with_query_dispatcher(DefaultQueryDispatcher) } - fn as_composite_shape<'a>( + fn as_composite_shape<'a, Colliders>( &'a self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, query_groups: InteractionGroups, - filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> QueryPipelineAsCompositeShape<'a> { + filter: Option<&'a dyn Fn(ColliderHandle) -> bool>, + ) -> QueryPipelineAsCompositeShape<'a, Colliders> { QueryPipelineAsCompositeShape { query_pipeline: self, colliders, @@ -134,52 +148,140 @@ impl QueryPipeline { } /// Update the acceleration structure on the query pipeline. - pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { - self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition) + pub fn update( + &mut self, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSet + + ComponentSet + + ComponentSetOption, + { + self.update_with_mode( + islands, + bodies, + colliders, + QueryPipelineMode::CurrentPosition, + ) } /// Update the acceleration structure on the query pipeline. - pub fn update_with_mode( + pub fn update_with_mode( &mut self, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, mode: QueryPipelineMode, - ) { - if !self.tree_built { - match mode { - QueryPipelineMode::CurrentPosition => { - let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); - } - QueryPipelineMode::SweepTestWithNextPosition => { - let data = colliders.iter().map(|(h, c)| { - let next_position = - bodies[c.parent()].next_position * c.position_wrt_parent(); - (h, c.compute_swept_aabb(&next_position)) - }); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); - } - QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - let data = colliders.iter().map(|(h, c)| { - let next_position = bodies[c.parent()] - .predict_position_using_velocity_and_forces(dt) - * c.position_wrt_parent(); - (h, c.compute_swept_aabb(&next_position)) - }); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSet + + ComponentSet + + ComponentSetOption, + { + struct DataGenerator<'a, Bs, Cs> { + bodies: &'a Bs, + colliders: &'a Cs, + mode: QueryPipelineMode, + } + + impl<'a, Bs, Cs> SimdQuadtreeDataGenerator for DataGenerator<'a, Bs, Cs> + where + Bs: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Cs: ComponentSet + + ComponentSet + + ComponentSetOption, + { + fn size_hint(&self) -> usize { + ComponentSet::::size_hint(self.colliders) + } + + #[inline(always)] + 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)) + }) + } + 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), + ) + } else { + f(ColliderHandle(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_force_and_velocity(dt, forces, vels, mprops); + + let next_position = predicted_pos * co_parent.pos_wrt_parent; + f( + ColliderHandle(h), + co_shape.compute_swept_aabb(&co_pos, &next_position), + ) + } else { + f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) + } + }) + } } } + } + + if !self.tree_built { + let generator = DataGenerator { + bodies, + colliders, + mode, + }; + self.quadtree + .clear_and_rebuild(generator, self.dilation_factor); // FIXME: uncomment this once we handle insertion/removals properly. // self.tree_built = true; return; } - for (_, body) in bodies - .iter_active_dynamic() - .chain(bodies.iter_active_kinematic()) - { - for handle in &body.colliders { + for handle in islands.iter_active_bodies() { + let body_colliders: &RigidBodyColliders = bodies.index(handle.0); + for handle in &body_colliders.0 { self.quadtree.pre_update(*handle) } } @@ -187,17 +289,28 @@ impl QueryPipeline { match mode { QueryPipelineMode::CurrentPosition => { self.quadtree.update( - |handle| colliders[*handle].compute_aabb(), + |handle| { + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(handle.0); + co_shape.compute_aabb(&co_pos) + }, self.dilation_factor, ); } QueryPipelineMode::SweepTestWithNextPosition => { self.quadtree.update( |handle| { - let co = &colliders[*handle]; - let next_position = - bodies[co.parent()].next_position * co.position_wrt_parent(); - co.compute_swept_aabb(&next_position) + let co_parent: Option<&ColliderParent> = colliders.get(handle.0); + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(handle.0); + + if let Some(co_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; + co_shape.compute_swept_aabb(&co_pos, &next_position) + } else { + co_shape.compute_aabb(&co_pos) + } }, self.dilation_factor, ); @@ -205,11 +318,26 @@ impl QueryPipeline { QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { self.quadtree.update( |handle| { - let co = &colliders[*handle]; - let next_position = bodies[co.parent()] - .predict_position_using_velocity_and_forces(dt) - * co.position_wrt_parent(); - co.compute_swept_aabb(&next_position) + let co_parent: Option<&ColliderParent> = colliders.get(handle.0); + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(handle.0); + + if let Some(co_parent) = co_parent { + let (rb_pos, vels, forces, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + ) = bodies.index_bundle(co_parent.handle.0); + + let predicted_pos = + rb_pos.integrate_force_and_velocity(dt, forces, vels, mprops); + + let next_position = predicted_pos * co_parent.pos_wrt_parent; + co_shape.compute_swept_aabb(&co_pos, &next_position) + } else { + co_shape.compute_aabb(&co_pos) + } }, self.dilation_factor, ); @@ -232,15 +360,20 @@ impl QueryPipeline { /// - `filter`: a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_ray( + pub fn cast_ray( &self, - colliders: &ColliderSet, + colliders: &Colliders, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, Real)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, Real)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid); @@ -263,15 +396,20 @@ impl QueryPipeline { /// - `filter`: a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_ray_and_get_normal( + pub fn cast_ray_and_get_normal( &self, - colliders: &ColliderSet, + colliders: &Colliders, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, RayIntersection)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, RayIntersection)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( &pipeline_shape, @@ -301,26 +439,31 @@ 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>( + pub fn intersections_with_ray<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, - ) { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool, + ) where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(coll) = colliders.get(*handle) { - if coll.collision_groups.test(query_groups) - && filter.map(|f| f(*handle, coll)).unwrap_or(true) + let co_shape: Option<&ColliderShape> = colliders.get(handle.0); + if let Some(co_shape) = co_shape { + let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) = + colliders.index_bundle(handle.0); + if co_groups.collision_groups.test(query_groups) + && filter.map(|f| f(*handle)).unwrap_or(true) { - if let Some(hit) = - coll.shape() - .cast_ray_and_get_normal(coll.position(), ray, max_toi, solid) + if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid) { - return callback(*handle, coll, hit); + return callback(*handle, hit); } } } @@ -343,14 +486,19 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn intersection_with_shape( + pub fn intersection_with_shape( &self, - colliders: &ColliderSet, + colliders: &Colliders, shape_pos: &Isometry, shape: &dyn Shape, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, @@ -379,14 +527,19 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn project_point( + pub fn project_point( &self, - colliders: &ColliderSet, + colliders: &Colliders, point: &Point, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, PointProjection)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, PointProjection)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid); @@ -408,21 +561,30 @@ 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>( + pub fn intersections_with_point<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, point: &Point, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool, - ) { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + mut callback: impl FnMut(ColliderHandle) -> bool, + ) where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(coll) = colliders.get(*handle) { - if coll.collision_groups.test(query_groups) - && filter.map(|f| f(*handle, coll)).unwrap_or(true) - && coll.shape().contains_point(coll.position(), point) + let co_shape: Option<&ColliderShape> = colliders.get(handle.0); + + if let Some(co_shape) = co_shape { + let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) = + colliders.index_bundle(handle.0); + + if co_groups.collision_groups.test(query_groups) + && filter.map(|f| f(*handle)).unwrap_or(true) + && co_shape.contains_point(co_pos, point) { - return callback(*handle, coll); + return callback(*handle); } } @@ -451,13 +613,18 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn project_point_and_get_feature( + pub fn project_point_and_get_feature( &self, - colliders: &ColliderSet, + colliders: &Colliders, point: &Point, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, PointProjection, FeatureId)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false); @@ -493,16 +660,21 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_shape<'a>( + pub fn cast_shape<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, shape_pos: &Isometry, shape_vel: &Vector, shape: &dyn Shape, max_toi: Real, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, TOI)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, TOI)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, @@ -535,17 +707,22 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn nonlinear_cast_shape( + pub fn nonlinear_cast_shape( &self, - colliders: &ColliderSet, + colliders: &Colliders, shape_motion: &NonlinearRigidMotion, shape: &dyn Shape, start_time: Real, end_time: Real, stop_at_penetration: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, TOI)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, TOI)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let pipeline_motion = NonlinearRigidMotion::identity(); let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( @@ -574,27 +751,36 @@ 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>( + pub fn intersections_with_shape<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, shape_pos: &Isometry, shape: &dyn Shape, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool, - ) { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + mut callback: impl FnMut(ColliderHandle) -> bool, + ) where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let dispatcher = &*self.query_dispatcher; let inv_shape_pos = shape_pos.inverse(); let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(coll) = colliders.get(*handle) { - if coll.collision_groups.test(query_groups) - && filter.map(|f| f(*handle, coll)).unwrap_or(true) - { - let pos12 = inv_shape_pos * coll.position(); + let co_shape: Option<&ColliderShape> = colliders.get(handle.0); - if dispatcher.intersection_test(&pos12, shape, coll.shape()) == Ok(true) { - return callback(*handle, coll); + if let Some(co_shape) = co_shape { + let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) = + colliders.index_bundle(handle.0); + + if co_groups.collision_groups.test(query_groups) + && filter.map(|f| f(*handle)).unwrap_or(true) + { + let pos12 = inv_shape_pos * co_pos.as_ref(); + + 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 new file mode 100644 index 0000000..354ff3d --- /dev/null +++ b/src/pipeline/user_changes.rs @@ -0,0 +1,156 @@ +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{ + IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle, + RigidBodyIds, RigidBodyPosition, RigidBodyType, +}; +use crate::geometry::{ColliderChanges, ColliderHandle, ColliderParent, ColliderPosition}; + +pub(crate) fn handle_user_changes_to_colliders( + bodies: &mut impl ComponentSet, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], +) where + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetOption, +{ + 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.contains(ColliderChanges::PARENT) { + let co_parent: Option<&ColliderParent> = colliders.get(handle.0); + + if let Some(co_parent) = co_parent { + let parent_pos = bodies.index(co_parent.handle.0); + + 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); + } + } + } + } +} + +pub(crate) fn handle_user_changes_to_rigid_bodies( + islands: &mut IslandManager, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_bodies: &[RigidBodyHandle], + modified_colliders: &mut Vec, +) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetOption, +{ + enum FinalAction { + UpdateActiveKinematicSetId, + UpdateActiveDynamicSetId, + } + + for handle in modified_bodies { + let mut final_action = None; + + let mut changes: RigidBodyChanges = *bodies.index(handle.0); + let mut ids: RigidBodyIds = *bodies.index(handle.0); + let mut activation: RigidBodyActivation = *bodies.index(handle.0); + let (status, rb_colliders, poss): ( + &RigidBodyType, + &RigidBodyColliders, + &RigidBodyPosition, + ) = bodies.index_bundle(handle.0); + + { + // The body's status changed. We need to make sure + // it is on the correct active set. + if changes.contains(RigidBodyChanges::TYPE) { + match status { + RigidBodyType::Dynamic => { + // Remove from the active kinematic set if it was there. + if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { + islands.active_kinematic_set.swap_remove(ids.active_set_id); + final_action = + Some((FinalAction::UpdateActiveKinematicSetId, ids.active_set_id)); + } + + // Add to the active dynamic set. + activation.wake_up(true); + // Make sure the sleep change flag is set (even if for some + // reasons the rigid-body was already awake) to make + // sure the code handling sleeping change adds the body to + // the active_dynamic_set. + changes.set(RigidBodyChanges::SLEEP, true); + } + RigidBodyType::Kinematic => { + // Remove from the active dynamic set if it was there. + if islands.active_dynamic_set.get(ids.active_set_id) == Some(&handle) { + islands.active_dynamic_set.swap_remove(ids.active_set_id); + final_action = + Some((FinalAction::UpdateActiveDynamicSetId, ids.active_set_id)); + } + + // Add to the active kinematic set. + if islands.active_kinematic_set.get(ids.active_set_id) != Some(&handle) { + ids.active_set_id = islands.active_kinematic_set.len(); + islands.active_kinematic_set.push(*handle); + } + } + RigidBodyType::Static => {} + } + } + + // Update the positions of the colliders. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) + { + rb_colliders.update_positions(colliders, modified_colliders, &poss.position); + + if status.is_kinematic() + && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) + { + ids.active_set_id = islands.active_kinematic_set.len(); + islands.active_kinematic_set.push(*handle); + } + } + + // Push the body to the active set if it is not + // sleeping and if it is not already inside of the active set. + if changes.contains(RigidBodyChanges::SLEEP) + && !activation.sleeping // May happen if the body was put to sleep manually. + && status.is_dynamic() // Only dynamic bodies are in the active dynamic set. + && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) + { + ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. + islands.active_dynamic_set.push(*handle); + } + + bodies.set_internal(handle.0, RigidBodyChanges::empty()); + bodies.set_internal(handle.0, ids); + bodies.set_internal(handle.0, activation); + } + + // Adjust some ids, if needed. + if let Some((action, id)) = final_action { + let active_set = match action { + FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set, + FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set, + }; + + if id < active_set.len() { + bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| { + ids2.active_set_id = id; + }); + } + } + } +} diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index df31c95..db0f846 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -158,8 +158,8 @@ impl Box2dWorld { let center = na_vec_to_b2_vec(collider.position_wrt_parent().translation.vector); let mut fixture_def = b2::FixtureDef::new(); - fixture_def.restitution = collider.restitution; - fixture_def.friction = collider.friction; + fixture_def.restitution = collider.material().restitution; + fixture_def.friction = collider.material().friction; fixture_def.density = collider.density().unwrap_or(1.0); fixture_def.is_sensor = collider.is_sensor(); fixture_def.filter = b2::Filter::new(); diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 5fcc45c..8447fd0 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -4,7 +4,7 @@ use crate::{ }; use kiss3d::window::Window; use plugin::HarnessPlugin; -use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; +use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet}; use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase}; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline}; @@ -120,7 +120,7 @@ impl Harness { colliders: ColliderSet, joints: JointSet, gravity: Vector, - hooks: impl PhysicsHooks + 'static, + hooks: impl PhysicsHooks + 'static, ) { // println!("Num bodies: {}", bodies.len()); // println!("Num joints: {}", joints.len()); @@ -130,6 +130,7 @@ impl Harness { self.physics.joints = joints; self.physics.hooks = Box::new(hooks); + self.physics.islands = IslandManager::new(); self.physics.broad_phase = BroadPhase::new(); self.physics.narrow_phase = NarrowPhase::new(); self.state.timestep_id = 0; @@ -191,6 +192,7 @@ impl Harness { self.physics.pipeline.step( &self.physics.gravity, &self.physics.integration_parameters, + &mut self.physics.islands, &mut self.physics.broad_phase, &mut self.physics.narrow_phase, &mut self.physics.bodies, @@ -201,9 +203,11 @@ impl Harness { &self.event_handler, ); - self.physics - .query_pipeline - .update(&self.physics.bodies, &self.physics.colliders); + self.physics.query_pipeline.update( + &self.physics.islands, + &self.physics.bodies, + &self.physics.colliders, + ); for plugin in &mut self.plugins { plugin.step(&mut self.physics, &self.state) diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs index b89f9c8..29e9a46 100644 --- a/src_testbed/physics/mod.rs +++ b/src_testbed/physics/mod.rs @@ -1,5 +1,5 @@ use crossbeam::channel::Receiver; -use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; +use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet}; use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase}; use rapier::math::Vector; use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline}; @@ -68,6 +68,7 @@ impl PhysicsSnapshot { } pub struct PhysicsState { + pub islands: IslandManager, pub broad_phase: BroadPhase, pub narrow_phase: NarrowPhase, pub bodies: RigidBodySet, @@ -78,12 +79,13 @@ pub struct PhysicsState { pub query_pipeline: QueryPipeline, pub integration_parameters: IntegrationParameters, pub gravity: Vector, - pub hooks: Box, + pub hooks: Box>, } impl PhysicsState { pub fn new() -> Self { Self { + islands: IslandManager::new(), broad_phase: BroadPhase::new(), narrow_phase: NarrowPhase::new(), bodies: RigidBodySet::new(), diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 8a3b155..6e68917 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -520,9 +520,9 @@ fn physx_collider_from_rapier_collider( }; let mut material = physics .create_material( - collider.friction, - collider.friction, - collider.restitution, + collider.co_material.friction, + collider.co_material.friction, + collider.co_material.restitution, (), ) .unwrap(); diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 0e04de6..d885c8a 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -19,7 +19,7 @@ use kiss3d::text::Font; use kiss3d::window::{State, Window}; use na::{self, Point2, Point3, Vector3}; use rapier::dynamics::{ - ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet, + IntegrationParameters, JointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet, }; use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase}; #[cfg(feature = "dim3")] @@ -245,7 +245,7 @@ impl Testbed { colliders: ColliderSet, joints: JointSet, gravity: Vector, - hooks: impl PhysicsHooks + 'static, + hooks: impl PhysicsHooks + 'static, ) { self.harness .set_world_with_params(bodies, colliders, joints, gravity, hooks); @@ -586,6 +586,7 @@ impl Testbed { for to_delete in &colliders[..num_to_delete] { self.harness.physics.colliders.remove( to_delete[0], + &mut self.harness.physics.islands, &mut self.harness.physics.bodies, true, ); @@ -605,6 +606,7 @@ impl Testbed { for to_delete in &dynamic_bodies[..num_to_delete] { self.harness.physics.bodies.remove( *to_delete, + &mut self.harness.physics.islands, &mut self.harness.physics.colliders, &mut self.harness.physics.joints, ); @@ -617,6 +619,7 @@ impl Testbed { for to_delete in &joints[..num_to_delete] { self.harness.physics.joints.remove( *to_delete, + &mut self.harness.physics.islands, &mut self.harness.physics.bodies, true, ); @@ -1205,13 +1208,13 @@ impl State for Testbed { != self.state.flags.contains(TestbedStateFlags::SLEEP) { if self.state.flags.contains(TestbedStateFlags::SLEEP) { - for (_, mut body) in self.harness.physics.bodies.iter_mut() { - body.activation.threshold = ActivationStatus::default_threshold(); + for (_, body) in self.harness.physics.bodies.iter_mut() { + body.activation_mut().threshold = RigidBodyActivation::default_threshold(); } } else { - for (_, mut body) in self.harness.physics.bodies.iter_mut() { + for (_, body) in self.harness.physics.bodies.iter_mut() { body.wake_up(true); - body.activation.threshold = -1.0; + body.activation_mut().threshold = -1.0; } } }