From 46d976d97bc9334004a58a19bc9cab3ea78e9569 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 19 Nov 2022 16:05:46 +0100 Subject: Allow disabling colliders, rigid-bodies and impulse joints --- src/control/character_controller.rs | 2 +- src/dynamics/island_manager.rs | 7 +- src/dynamics/joint/generic_joint.rs | 38 ++++ .../joint/impulse_joint/impulse_joint_set.rs | 46 ++++- .../joint/multibody_joint/multibody_joint_set.rs | 17 +- src/dynamics/rigid_body.rs | 34 +++ src/dynamics/rigid_body_components.rs | 16 +- src/geometry/broad_phase_multi_sap/broad_phase.rs | 16 +- src/geometry/collider.rs | 41 ++++ src/geometry/collider_components.rs | 18 ++ src/geometry/collider_set.rs | 25 +++ src/pipeline/collision_pipeline.rs | 11 + src/pipeline/physics_pipeline.rs | 13 ++ src/pipeline/query_pipeline.rs | 6 +- src/pipeline/user_changes.rs | 227 +++++++++++++-------- 15 files changed, 409 insertions(+), 108 deletions(-) (limited to 'src') diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index ebed952..28ba475 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -133,7 +133,7 @@ impl Default for KinematicCharacterController { pub struct EffectiveCharacterMovement { /// The movement to apply. pub translation: Vector, - /// Is the character touching the ground after applying `EffictiveKineamticMovement::translation`? + /// Is the character touching the ground after applying `EffectiveKineamticMovement::translation`? pub grounded: bool, } diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 1295f14..edd2579 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -98,7 +98,8 @@ impl IslandManager { let rb = bodies.index_mut_internal(handle); rb.activation.wake_up(strong); - if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) { + if rb.is_enabled() && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) + { rb.ids.active_set_id = self.active_dynamic_set.len(); self.active_dynamic_set.push(handle); } @@ -256,12 +257,12 @@ impl IslandManager { // in contact or joined with this collider. push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); - for inter in impulse_joints.attached_joints(handle) { + for inter in impulse_joints.attached_enabled_joints(handle) { let other = crate::utils::select_other((inter.0, inter.1), handle); self.stack.push(other); } - for other in multibody_joints.attached_bodies(handle) { + for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { self.stack.push(other); } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index bb1598d..4f0a791 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -5,6 +5,7 @@ use crate::utils::{WBasis, WReal}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; +use crate::geometry::ColliderEnabled; #[cfg(feature = "dim3")] bitflags::bitflags! { @@ -182,6 +183,19 @@ impl JointMotor { } } +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// Enum indicating whether or not a joint is enabled. +pub enum JointEnabled { + /// The joint is enabled. + Enabled, + /// The joint wasn’t disabled by the user explicitly but it is attached to + /// a disabled rigid-body. + DisabledByAttachedBody, + /// The joint is disabled by the user explicitly. + Disabled, +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] /// A generic joint. @@ -208,6 +222,8 @@ pub struct GenericJoint { pub motors: [JointMotor; SPATIAL_DIM], /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, + /// Whether or not the joint is enabled. + pub enabled: JointEnabled, } impl Default for GenericJoint { @@ -222,6 +238,7 @@ impl Default for GenericJoint { limits: [JointLimits::default(); SPATIAL_DIM], motors: [JointMotor::default(); SPATIAL_DIM], contacts_enabled: true, + enabled: JointEnabled::Enabled, } } } @@ -260,6 +277,27 @@ impl GenericJoint { } } + /// Is this joint enabled? + pub fn is_enabled(&self) -> bool { + self.enabled == JointEnabled::Enabled + } + + /// Set whether this joint is enabled or not. + pub fn set_enabled(&mut self, enabled: bool) { + match self.enabled { + JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => { + if !enabled { + self.enabled = JointEnabled::Disabled; + } + } + JointEnabled::Disabled => { + if enabled { + self.enabled = JointEnabled::Enabled; + } + } + } + } + /// Add the specified axes to the set of axes locked by this joint. pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self { self.locked_axes |= axes; diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 7409172..f21f950 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -72,11 +72,11 @@ impl ImpulseJointSet { } /// Iterates through all the joints between two rigid-bodies. - pub fn joints_between<'a>( - &'a self, + pub fn joints_between( + &self, body1: RigidBodyHandle, body2: RigidBodyHandle, - ) -> impl Iterator { + ) -> impl Iterator { self.rb_graph_ids .get(body1.0) .zip(self.rb_graph_ids.get(body2.0)) @@ -86,15 +86,15 @@ impl ImpulseJointSet { } /// Iterates through all the impulse joints attached to the given rigid-body. - pub fn attached_joints<'a>( - &'a self, + pub fn attached_joints( + &self, body: RigidBodyHandle, ) -> impl Iterator< Item = ( RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, - &'a ImpulseJoint, + &ImpulseJoint, ), > { self.rb_graph_ids @@ -104,6 +104,35 @@ impl ImpulseJointSet { .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2)) } + /// Iterates through all the impulse joints attached to the given rigid-body. + pub fn map_attached_joints_mut<'a>( + &'a mut self, + body: RigidBodyHandle, + mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint), + ) { + self.rb_graph_ids.get(body.0).into_iter().for_each(|id| { + for inter in self.joint_graph.interactions_with_mut(*id) { + (f)(inter.0, inter.1, inter.3.handle, inter.3) + } + }) + } + + /// Iterates through all the enabled impulse joints attached to the given rigid-body. + pub fn attached_enabled_joints( + &self, + body: RigidBodyHandle, + ) -> impl Iterator< + Item = ( + RigidBodyHandle, + RigidBodyHandle, + ImpulseJointHandle, + &ImpulseJoint, + ), + > { + self.attached_joints(body) + .filter(|inter| inter.3.data.is_enabled()) + } + /// Is the given joint handle valid? pub fn contains(&self, handle: ImpulseJointHandle) -> bool { self.joint_ids.contains(handle.0) @@ -246,7 +275,7 @@ impl ImpulseJointSet { ImpulseJointHandle(handle) } - /// Retrieve all the impulse_joints happening between two active bodies. + /// Retrieve all the enabled impulse 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( &self, @@ -264,7 +293,8 @@ impl ImpulseJointSet { let rb1 = &bodies[joint.body1]; let rb2 = &bodies[joint.body2]; - if (rb1.is_dynamic() || rb2.is_dynamic()) + if joint.data.is_enabled() + && (rb1.is_dynamic() || rb2.is_dynamic()) && (!rb1.is_dynamic() || !rb1.is_sleeping()) && (!rb2.is_dynamic() || !rb2.is_sleeping()) { diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index c25de73..a4b338a 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -372,7 +372,7 @@ impl MultibodyJointSet { } /// Iterate through the handles of all the rigid-bodies attached to this rigid-body - /// by an multibody_joint. + /// by a multibody_joint. pub fn attached_bodies<'a>( &'a self, body: RigidBodyHandle, @@ -384,6 +384,21 @@ impl MultibodyJointSet { .map(move |inter| crate::utils::select_other((inter.0, inter.1), body)) } + /// Iterate through the handles of all the rigid-bodies attached to this rigid-body + /// by an enabled multibody_joint. + pub fn bodies_attached_with_enabled_joint<'a>( + &'a self, + body: RigidBodyHandle, + ) -> impl Iterator + 'a { + self.attached_bodies(body).filter(move |other| { + if let Some((_, _, link)) = self.joint_between(body, *other) { + link.joint.data.is_enabled() + } else { + false + } + }) + } + /// Iterates through all the multibodies on this set. pub fn multibodies(&self) -> impl Iterator { self.multibodies.iter().map(|e| e.1) diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9c38eee..a6384b7 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -35,6 +35,7 @@ pub struct RigidBody { pub(crate) body_type: RigidBodyType, /// The dominance group this rigid-body is part of. pub(crate) dominance: RigidBodyDominance, + pub(crate) enabled: bool, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -61,6 +62,7 @@ impl RigidBody { changes: RigidBodyChanges::all(), body_type: RigidBodyType::Dynamic, dominance: RigidBodyDominance::default(), + enabled: true, user_data: 0, } } @@ -81,6 +83,28 @@ impl RigidBody { &mut self.activation } + /// Is this rigid-body enabled? + pub fn is_enabled(&self) -> bool { + self.enabled + } + + /// Sets whether this rigid-body is enabled or not. + pub fn set_enabled(&mut self, enabled: bool) { + if enabled != self.enabled { + if enabled { + // NOTE: this is probably overkill, but it makes sure we don’t + // forget anything that needs to be updated because the rigid-body + // was basically interpreted as if it was removed while it was + // disabled. + self.changes = RigidBodyChanges::all(); + } else { + self.changes |= RigidBodyChanges::ENABLED_OR_DISABLED; + } + + self.enabled = enabled; + } + } + /// The linear damping coefficient of this rigid-body. #[inline] pub fn linear_damping(&self) -> Real { @@ -963,6 +987,8 @@ pub struct RigidBodyBuilder { pub ccd_enabled: bool, /// The dominance group of the rigid-body to be built. pub dominance_group: i8, + /// Will the rigid-body being built be enabled? + pub enabled: bool, /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. pub user_data: u128, } @@ -984,6 +1010,7 @@ impl RigidBodyBuilder { sleeping: false, ccd_enabled: false, dominance_group: 0, + enabled: true, user_data: 0, } } @@ -1230,6 +1257,12 @@ impl RigidBodyBuilder { self } + /// Enable or disable the rigid-body after its creation. + pub fn enabled(mut self, enabled: bool) -> Self { + self.enabled = enabled; + self + } + /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); @@ -1252,6 +1285,7 @@ impl RigidBodyBuilder { rb.damping.angular_damping = self.angular_damping; rb.forces.gravity_scale = self.gravity_scale; rb.dominance = RigidBodyDominance(self.dominance_group); + rb.enabled = self.enabled; rb.enable_ccd(self.ccd_enabled); if self.can_sleep && self.sleeping { diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index cabf6ca..eb94c6b 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -112,6 +112,8 @@ bitflags::bitflags! { const DOMINANCE = 1 << 5; /// Flag indicating that the local mass-properties of this rigid-body must be recomputed. const LOCAL_MASS_PROPERTIES = 1 << 6; + /// Flag indicating that the rigid-body was enabled or disabled. + const ENABLED_OR_DISABLED = 1 << 7; } } @@ -329,12 +331,14 @@ impl RigidBodyMassProps { for handle in &attached_colliders.0 { if let Some(co) = colliders.get(*handle) { - if let Some(co_parent) = co.parent { - let to_add = co - .mprops - .mass_properties(&*co.shape) - .transform_by(&co_parent.pos_wrt_parent); - self.local_mprops += to_add; + if co.is_enabled() { + if let Some(co_parent) = co.parent { + let to_add = co + .mprops + .mass_properties(&*co.shape) + .transform_by(&co_parent.pos_wrt_parent); + self.local_mprops += to_add; + } } } } diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index 915017d..0667184 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -176,7 +176,11 @@ 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, removed_colliders: &[ColliderHandle]) { + fn complete_removals( + &mut self, + colliders: &mut ColliderSet, + removed_colliders: &[ColliderHandle], + ) { // If there is no layer, there is nothing to remove. if self.layers.is_empty() { return; @@ -224,6 +228,11 @@ impl BroadPhase { self.proxies.remove(proxy_id); } } + + if let Some(co) = colliders.get_mut_internal(*removed) { + // Reset the proxy index. + co.bf_data.proxy_index = crate::INVALID_U32; + } } } @@ -460,9 +469,10 @@ impl BroadPhase { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. if let Some(co) = colliders.get_mut_internal(*handle) { - if !co.changes.needs_broad_phase_update() { + if !co.is_enabled() || !co.changes.needs_broad_phase_update() { continue; } + let mut new_proxy_id = co.bf_data.proxy_index; if self.handle_modified_collider( @@ -496,7 +506,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(removed_colliders); + self.complete_removals(colliders, removed_colliders); } /// Propagate regions from the smallest layers up to the larger layers. diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 95ae273..6759b81 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -7,6 +7,7 @@ use crate::geometry::{ use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM}; use crate::parry::transformation::vhacd::VHACDParameters; use crate::pipeline::{ActiveEvents, ActiveHooks}; +use crate::prelude::ColliderEnabled; use na::Unit; use parry::bounding_volume::Aabb; use parry::shape::{Shape, TriMeshFlags}; @@ -154,6 +155,32 @@ impl Collider { } } + /// Is this collider enabled? + pub fn is_enabled(&self) -> bool { + match self.flags.enabled { + ColliderEnabled::Enabled => true, + _ => false, + } + } + + /// Sets whether or not this collider is enabled. + pub fn set_enabled(&mut self, enabled: bool) { + match self.flags.enabled { + ColliderEnabled::Enabled | ColliderEnabled::DisabledByParent => { + if !enabled { + self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED); + self.flags.enabled = ColliderEnabled::Disabled; + } + } + ColliderEnabled::Disabled => { + if enabled { + self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED); + self.flags.enabled = ColliderEnabled::Enabled; + } + } + } + } + /// Sets the translational part of this collider's position. pub fn set_translation(&mut self, translation: Vector) { self.changes.insert(ColliderChanges::POSITION); @@ -402,6 +429,8 @@ pub struct ColliderBuilder { pub collision_groups: InteractionGroups, /// The solver groups for the collider being built. pub solver_groups: InteractionGroups, + /// Will the collider being built be enabled? + pub enabled: bool, /// The total force magnitude beyond which a contact force event can be emitted. pub contact_force_event_threshold: Real, } @@ -424,6 +453,7 @@ impl ColliderBuilder { active_collision_types: ActiveCollisionTypes::default(), active_hooks: ActiveHooks::empty(), active_events: ActiveEvents::empty(), + enabled: true, contact_force_event_threshold: 0.0, } } @@ -834,6 +864,12 @@ impl ColliderBuilder { self } + /// Enable or disable the collider after its creation. + pub fn enabled(mut self, enabled: bool) -> Self { + self.enabled = enabled; + self + } + /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { let shape = self.shape.clone(); @@ -849,6 +885,11 @@ impl ColliderBuilder { active_collision_types: self.active_collision_types, active_hooks: self.active_hooks, active_events: self.active_events, + enabled: if self.enabled { + ColliderEnabled::Enabled + } else { + ColliderEnabled::Disabled + }, }; let changes = ColliderChanges::all(); let pos = ColliderPosition(self.position); diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs index 29a24b5..0e65bac 100644 --- a/src/geometry/collider_components.rs +++ b/src/geometry/collider_components.rs @@ -64,6 +64,8 @@ bitflags::bitflags! { /// This flags is automatically set by the `PhysicsPipeline` when the `RigidBodyChanges::DOMINANCE` /// or `RigidBodyChanges::TYPE` of the parent rigid-body of this collider is detected. const PARENT_EFFECTIVE_DOMINANCE = 1 << 7; // NF update. + /// Flag indicating that whether or not the collider is enabled was changed. + const ENABLED_OR_DISABLED = 1 << 8; // BF & NF updates. } } @@ -372,6 +374,19 @@ impl Default for ActiveCollisionTypes { } } +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// Enum indicating whether or not a collider is enabled. +pub enum ColliderEnabled { + /// The collider is enabled. + Enabled, + /// The collider wasn’t disabled by the user explicitly but it is attached to + /// a disabled rigid-body. + DisabledByParent, + /// The collider is disabled by the user explicitly. + Disabled, +} + #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A set of flags for controlling collision/intersection filtering, modification, and events. @@ -389,6 +404,8 @@ pub struct ColliderFlags { pub active_hooks: ActiveHooks, /// The events enabled for this collider. pub active_events: ActiveEvents, + /// Whether or not the collider is enabled. + pub enabled: ColliderEnabled, } impl Default for ColliderFlags { @@ -399,6 +416,7 @@ impl Default for ColliderFlags { solver_groups: InteractionGroups::all(), active_hooks: ActiveHooks::empty(), active_events: ActiveEvents::empty(), + enabled: ColliderEnabled::Enabled, } } } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index b141445..1069f1b 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -41,6 +41,14 @@ impl ColliderSet { self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c)) } + /// Iterate through all the enabled colliders on this set. + pub fn iter_enabled(&self) -> impl Iterator { + self.colliders + .iter() + .map(|(h, c)| (ColliderHandle(h), c)) + .filter(|(_, c)| c.is_enabled()) + } + /// Iterates mutably through all the colliders on this set. #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn iter_mut(&mut self) -> impl Iterator { @@ -52,6 +60,12 @@ impl ColliderSet { }) } + /// Iterates mutably through all the enabled colliders on this set. + #[cfg(not(feature = "dev-remove-slow-accessors"))] + pub fn iter_enabled_mut(&mut self) -> impl Iterator { + self.iter_mut().filter(|(_, c)| c.is_enabled()) + } + /// The number of colliders on this set. pub fn len(&self) -> usize { self.colliders.len() @@ -268,6 +282,17 @@ impl ColliderSet { 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); + Some(result) + } } impl Index for ColliderSet { diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index ac92a04..631388b 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -127,9 +127,20 @@ impl CollisionPipeline { None, bodies, colliders, + &mut ImpulseJointSet::new(), + &mut MultibodyJointSet::new(), &modified_bodies, &mut modified_colliders, ); + + // Disabled colliders are treated as if they were removed. + removed_colliders.extend( + modified_colliders + .iter() + .copied() + .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), + ); + self.detect_collisions( prediction_distance, broad_phase, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 391b39a..f05a3f0 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -422,6 +422,7 @@ impl PhysicsPipeline { // Apply modifications. let mut modified_colliders = colliders.take_modified(); let mut removed_colliders = colliders.take_removed(); + super::user_changes::handle_user_changes_to_colliders( bodies, colliders, @@ -433,10 +434,22 @@ impl PhysicsPipeline { Some(islands), bodies, colliders, + impulse_joints, + multibody_joints, &modified_bodies, &mut modified_colliders, ); + // Disabled colliders are treated as if they were removed. + // NOTE: this must be called here, after handle_user_changes_to_rigid_bodies to take into + // account colliders disabled because of their parent rigid-body. + removed_colliders.extend( + modified_colliders + .iter() + .copied() + .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), + ); + // TODO: do this only on user-change. // TODO: do we want some kind of automatic inverse kinematics? for multibody in &mut multibody_joints.multibodies { diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 00825e2..6682370 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -358,12 +358,12 @@ impl QueryPipeline { fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { match self.mode { QueryPipelineMode::CurrentPosition => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { f(h, co.shape.compute_aabb(&co.pos)) } } QueryPipelineMode::SweepTestWithNextPosition => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { if let Some(co_parent) = co.parent { let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position; let next_position = rb_next_pos * co_parent.pos_wrt_parent; @@ -374,7 +374,7 @@ impl QueryPipeline { } } QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { if let Some(co_parent) = co.parent { let rb = &self.bodies[co_parent.handle]; let predicted_pos = rb.pos.integrate_forces_and_velocities( diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index e7da68a..adfd023 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -1,7 +1,10 @@ use crate::dynamics::{ - IslandManager, RigidBodyChanges, RigidBodyHandle, RigidBodySet, RigidBodyType, + ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges, + RigidBodyHandle, RigidBodySet, RigidBodyType, +}; +use crate::geometry::{ + ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet, }; -use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet}; pub(crate) fn handle_user_changes_to_colliders( bodies: &mut RigidBodySet, @@ -21,10 +24,12 @@ pub(crate) fn handle_user_changes_to_colliders( } } - if co - .changes - .intersects(ColliderChanges::SHAPE | ColliderChanges::LOCAL_MASS_PROPERTIES) - { + if co.changes.intersects( + ColliderChanges::SHAPE + | ColliderChanges::LOCAL_MASS_PROPERTIES + | ColliderChanges::ENABLED_OR_DISABLED + | ColliderChanges::PARENT, + ) { if let Some(rb) = co .parent .and_then(|p| bodies.get_mut_internal_with_modification_tracking(p.handle)) @@ -40,12 +45,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( mut islands: Option<&mut IslandManager>, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, modified_bodies: &[RigidBodyHandle], modified_colliders: &mut Vec, ) { enum FinalAction { - UpdateActiveKinematicSetId, - UpdateActiveDynamicSetId, + UpdateActiveKinematicSetId(usize), + UpdateActiveDynamicSetId(usize), + RemoveFromIsland, } for handle in modified_bodies { @@ -62,89 +70,95 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( let mut activation = rb.activation; { - // The body's status changed. We need to make sure - // it is on the correct active set. - if let Some(islands) = islands.as_deref_mut() { - if changes.contains(RigidBodyChanges::TYPE) { - match rb.body_type { - RigidBodyType::Dynamic => { - // Remove from the active kinematic set if it was there. - if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { - 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::KinematicVelocityBased - | RigidBodyType::KinematicPositionBased => { - // 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, - )); + if rb.is_enabled() { + // The body's status changed. We need to make sure + // it is on the correct active set. + if let Some(islands) = islands.as_deref_mut() { + if changes.contains(RigidBodyChanges::TYPE) { + match rb.body_type { + RigidBodyType::Dynamic => { + // Remove from the active kinematic set if it was there. + if islands.active_kinematic_set.get(ids.active_set_id) + == Some(handle) + { + 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); } - - // 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::KinematicVelocityBased + | RigidBodyType::KinematicPositionBased => { + // 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::Fixed => {} } - RigidBodyType::Fixed => {} } - } - // Update the positions of the colliders. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.colliders - .update_positions(colliders, modified_colliders, &rb.pos.position); + // Update the active kinematic set. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) + { + if rb.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); + } + } - if rb.is_kinematic() - && islands.active_kinematic_set.get(ids.active_set_id) != Some(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) + && rb.is_enabled() + && !activation.sleeping // May happen if the body was put to sleep manually. + && rb.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_kinematic_set.len(); - islands.active_kinematic_set.push(*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); } } + } - // 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. - && rb.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); - } - } else { - // We don't use islands. So just update the colliders' positions. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.colliders - .update_positions(colliders, modified_colliders, &rb.pos.position); - } + // Update the colliders' positions. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) + { + rb.colliders + .update_positions(colliders, modified_colliders, &rb.pos.position); } if changes.contains(RigidBodyChanges::DOMINANCE) || changes.contains(RigidBodyChanges::TYPE) { for handle in rb.colliders.0.iter() { + // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking` + // here because that would modify the `modified_colliders` inside of the `ColliderSet` + // instead of the one passed to this method. let co = colliders.index_mut_internal(*handle); if !co.changes.contains(ColliderChanges::MODIFIED) { modified_colliders.push(*handle); @@ -165,6 +179,43 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( ); } + if changes.contains(RigidBodyChanges::ENABLED_OR_DISABLED) { + // Propagate the rigid-body’s enabled/disable status to its colliders. + for handle in rb.colliders.0.iter() { + // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking` + // here because that would modify the `modified_colliders` inside of the `ColliderSet` + // instead of the one passed to this method. + let co = colliders.index_mut_internal(*handle); + if !co.changes.contains(ColliderChanges::MODIFIED) { + modified_colliders.push(*handle); + } + + if rb.enabled && co.flags.enabled == ColliderEnabled::DisabledByParent { + co.flags.enabled = ColliderEnabled::Enabled; + } else if !rb.enabled && co.flags.enabled == ColliderEnabled::Enabled { + co.flags.enabled = ColliderEnabled::DisabledByParent; + } + + co.changes |= ColliderChanges::MODIFIED | ColliderChanges::ENABLED_OR_DISABLED; + } + + // Propagate the rigid-body’s enabled/disable status to its attached impulse joints. + impulse_joints.map_attached_joints_mut(*handle, |_, _, _, joint| { + if rb.enabled && joint.data.enabled == JointEnabled::DisabledByAttachedBody { + joint.data.enabled = JointEnabled::Enabled; + } else if !rb.enabled && joint.data.enabled == JointEnabled::Enabled { + joint.data.enabled = JointEnabled::DisabledByAttachedBody; + } + }); + + // FIXME: Propagate the rigid-body’s enabled/disable status to its attached multibody joints. + + // Remove the rigid-body from the island manager. + if !rb.enabled { + final_action = Some(FinalAction::RemoveFromIsland); + } + } + rb.changes = RigidBodyChanges::empty(); rb.ids = ids; rb.activation = activation; @@ -172,15 +223,25 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // Adjust some ids, if needed. if let Some(islands) = islands.as_deref_mut() { - 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 let Some(action) = final_action { + match action { + FinalAction::RemoveFromIsland => { + let ids = rb.ids; + islands.rigid_body_removed(*handle, &ids, bodies); + } + FinalAction::UpdateActiveKinematicSetId(id) => { + let active_set = &mut islands.active_kinematic_set; + if id < active_set.len() { + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; + } + } + FinalAction::UpdateActiveDynamicSetId(id) => { + let active_set = &mut islands.active_dynamic_set; + if id < active_set.len() { + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; + } + } }; - - if id < active_set.len() { - bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; - } } } } -- cgit From 683baf6bf77cfb41227ea6ed4a42499d1e051cdf Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 26 Nov 2022 17:45:14 +0100 Subject: Allow the PhysicsPipeline and CollisionPipeline to update the QueryPipeline incrementally --- src/dynamics/ccd/ccd_solver.rs | 2 - src/dynamics/joint/generic_joint.rs | 1 - src/geometry/broad_phase_qbvh.rs | 88 +++++++++++++++++++++++++++ src/geometry/collider_set.rs | 1 + src/geometry/mod.rs | 6 +- src/pipeline/collision_pipeline.rs | 7 ++- src/pipeline/physics_pipeline.rs | 24 ++++++-- src/pipeline/query_pipeline.rs | 116 +++++++++++------------------------- src/pipeline/user_changes.rs | 2 +- 9 files changed, 156 insertions(+), 91 deletions(-) create mode 100644 src/geometry/broad_phase_qbvh.rs (limited to 'src') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 9e0ab8e..9b06a80 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -123,7 +123,6 @@ impl CCDSolver { ) -> Option { // Update the query pipeline. self.query_pipeline.update_with_mode( - islands, bodies, colliders, QueryPipelineMode::SweepTestWithPredictedPosition { dt }, @@ -245,7 +244,6 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( - islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 4f0a791..cbd9649 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -5,7 +5,6 @@ use crate::utils::{WBasis, WReal}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; -use crate::geometry::ColliderEnabled; #[cfg(feature = "dim3")] bitflags::bitflags! { diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs new file mode 100644 index 0000000..22ca562 --- /dev/null +++ b/src/geometry/broad_phase_qbvh.rs @@ -0,0 +1,88 @@ +use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet}; +use parry::bounding_volume::BoundingVolume; +use parry::math::Real; +use parry::partitioning::Qbvh; +use parry::partitioning::QbvhUpdateWorkspace; +use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct BroadPhase { + qbvh: Qbvh, + stack: Vec<(u32, u32)>, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + workspace: QbvhUpdateWorkspace, +} + +impl Default for BroadPhase { + fn default() -> Self { + Self::new() + } +} + +impl BroadPhase { + pub fn new() -> Self { + Self { + qbvh: Qbvh::new(), + stack: vec![], + workspace: QbvhUpdateWorkspace::default(), + } + } + + #[allow(dead_code)] // This broad-phase is just experimental right now. + pub fn update( + &mut self, + prediction_distance: Real, + colliders: &ColliderSet, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + events: &mut Vec, + ) { + let margin = 0.01; + + if modified_colliders.is_empty() { + return; + } + + // Visitor to find collision pairs. + let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new( + |co1: &ColliderHandle, co2: &ColliderHandle| { + events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2))); + true + }, + ); + + let full_rebuild = self.qbvh.raw_nodes().is_empty(); + + if full_rebuild { + self.qbvh.clear_and_rebuild( + colliders.iter().map(|(handle, collider)| { + ( + handle, + collider.compute_aabb().loosened(prediction_distance / 2.0), + ) + }), + margin, + ); + self.qbvh + .traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack); + } else { + for modified in modified_colliders { + self.qbvh.pre_update_or_insert(*modified); + } + + for removed in removed_colliders { + self.qbvh.remove(*removed); + } + + let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| { + colliders[*handle] + .compute_aabb() + .loosened(prediction_distance / 2.0) + }); + self.qbvh + .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack); + self.qbvh.rebalance(margin, &mut self.workspace); + } + } +} diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 1069f1b..4bc2a03 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -285,6 +285,7 @@ impl ColliderSet { // 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`. + #[allow(dead_code)] pub(crate) fn get_mut_internal_with_modification_tracking( &mut self, handle: ColliderHandle, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4f3ada8..a91c4b9 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,6 +1,9 @@ //! Structures related to geometry: colliders, shapes, etc. -pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair}; +pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; + +pub use self::broad_phase_multi_sap::BroadPhase; +// pub use self::broad_phase_qbvh::BroadPhase; pub use self::collider_components::*; pub use self::contact_pair::{ ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags, @@ -199,5 +202,6 @@ mod interaction_graph; mod interaction_groups; mod narrow_phase; +mod broad_phase_qbvh; mod collider; mod collider_set; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 631388b..d0beedd 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -5,7 +5,7 @@ use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase, }; use crate::math::Real; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline}; use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -111,6 +111,7 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + query_pipeline: Option<&mut QueryPipeline>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -154,6 +155,10 @@ impl CollisionPipeline { true, ); + if let Some(queries) = query_pipeline { + queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); removed_colliders.clear(); } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f05a3f0..7226063 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -14,7 +14,7 @@ use crate::geometry::{ ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex, }; use crate::math::{Real, Vector}; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline}; use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; /// The physics pipeline, responsible for stepping the whole physics simulation. @@ -404,6 +404,7 @@ impl PhysicsPipeline { impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, ccd_solver: &mut CCDSolver, + mut query_pipeline: Option<&mut QueryPipeline>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -468,13 +469,17 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, - &modified_colliders[..], - &mut removed_colliders, + &modified_colliders, + &removed_colliders, hooks, events, true, ); + if let Some(queries) = query_pipeline.as_deref_mut() { + queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); removed_colliders.clear(); @@ -595,13 +600,22 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, - &mut modified_colliders, - &mut removed_colliders, + &modified_colliders, + &[], hooks, events, false, ); + if let Some(queries) = query_pipeline.as_deref_mut() { + queries.update_incremental( + colliders, + &modified_colliders, + &[], + remaining_substeps == 0, + ); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 6682370..8a667de 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,10 +1,10 @@ -use crate::dynamics::{IslandManager, RigidBodyHandle}; +use crate::dynamics::RigidBodyHandle; use crate::geometry::{ Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection, }; use crate::math::{Isometry, Point, Real, Vector}; use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; -use parry::partitioning::QbvhDataGenerator; +use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace}; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -30,8 +30,9 @@ pub struct QueryPipeline { )] query_dispatcher: Arc, qbvh: Qbvh, - tree_built: bool, dilation_factor: Real, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + workspace: QbvhUpdateWorkspace, } struct QueryPipelineAsCompositeShape<'a> { @@ -310,8 +311,8 @@ impl QueryPipeline { Self { query_dispatcher: Arc::new(d), qbvh: Qbvh::new(), - tree_built: false, dilation_factor: 0.01, + workspace: QbvhUpdateWorkspace::default(), } } @@ -320,25 +321,39 @@ impl QueryPipeline { &*self.query_dispatcher } - /// Update the acceleration structure on the query pipeline. - pub fn update( + /// Update the query pipeline incrementally, avoiding a complete rebuild of its + /// internal data-structure. + pub fn update_incremental( &mut self, - islands: &IslandManager, - bodies: &RigidBodySet, colliders: &ColliderSet, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + refit_and_rebalance: bool, ) { - self.update_with_mode( - islands, - bodies, - colliders, - QueryPipelineMode::CurrentPosition, - ) + for modified in modified_colliders { + self.qbvh.pre_update_or_insert(*modified); + } + + for removed in removed_colliders { + self.qbvh.remove(*removed); + } + + if refit_and_rebalance { + let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { + colliders[*handle].compute_aabb() + }); + self.qbvh.rebalance(0.0, &mut self.workspace); + } + } + + /// 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) } /// Update the acceleration structure on the query pipeline. pub fn update_with_mode( &mut self, - islands: &IslandManager, bodies: &RigidBodySet, colliders: &ColliderSet, mode: QueryPipelineMode, @@ -392,71 +407,12 @@ impl QueryPipeline { } } - if !self.tree_built { - let generator = DataGenerator { - bodies, - colliders, - mode, - }; - self.qbvh.clear_and_rebuild(generator, self.dilation_factor); - - // FIXME: uncomment this once we handle insertion/removals properly. - // self.tree_built = true; - return; - } - - for handle in islands.iter_active_bodies() { - let rb = &bodies[handle]; - for handle in &rb.colliders.0 { - self.qbvh.pre_update(*handle) - } - } - - match mode { - QueryPipelineMode::CurrentPosition => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - co.shape.compute_aabb(&co.pos) - }, - self.dilation_factor, - ); - } - QueryPipelineMode::SweepTestWithNextPosition => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - if let Some(parent) = &co.parent { - let rb_next_pos = &bodies[parent.handle].pos.next_position; - let next_position = rb_next_pos * parent.pos_wrt_parent; - co.shape.compute_swept_aabb(&co.pos, &next_position) - } else { - co.shape.compute_aabb(&co.pos) - } - }, - self.dilation_factor, - ); - } - QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - if let Some(parent) = co.parent { - let rb = &bodies[parent.handle]; - let predicted_pos = rb.pos.integrate_forces_and_velocities( - dt, &rb.forces, &rb.vels, &rb.mprops, - ); - - let next_position = predicted_pos * parent.pos_wrt_parent; - co.shape.compute_swept_aabb(&co.pos, &next_position) - } else { - co.shape.compute_aabb(&co.pos) - } - }, - self.dilation_factor, - ); - } - } + let generator = DataGenerator { + bodies, + colliders, + mode, + }; + self.qbvh.clear_and_rebuild(generator, self.dilation_factor); } /// Find the closest intersection between a ray and a set of collider. diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index adfd023..00096de 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -46,7 +46,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, - multibody_joints: &mut MultibodyJointSet, + _multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies modified_bodies: &[RigidBodyHandle], modified_colliders: &mut Vec, ) { -- cgit From 7e95cba09dbaccdc60e9bc79a9d280577618843a Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 27 Nov 2022 12:13:08 +0100 Subject: Fix ABA problem in incremental query pipeline update --- src/pipeline/query_pipeline.rs | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 8a667de..a248426 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -330,14 +330,21 @@ impl QueryPipeline { removed_colliders: &[ColliderHandle], refit_and_rebalance: bool, ) { - for modified in modified_colliders { - self.qbvh.pre_update_or_insert(*modified); - } - + // We remove first. This is needed to avoid the ABA problem: if a collider was removed + // and another added right after with the same handle index, we can remove first, and + // then update the new one (but only if its actually exists, to address the case where + // a collider was added/modified and then removed during the same frame). for removed in removed_colliders { self.qbvh.remove(*removed); } + for modified in modified_colliders { + // Check that the collider still exists as it may have been removed. + if colliders.contains(*modified) { + self.qbvh.pre_update_or_insert(*modified); + } + } + if refit_and_rebalance { let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { colliders[*handle].compute_aabb() -- cgit From 849f398031c61b4894d5b8ca229037c026c9186d Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 4 Dec 2022 18:27:38 +0100 Subject: Add a dynamic ray-cast vehicle controller --- src/control/mod.rs | 6 + src/control/ray_cast_vehicle_controller.rs | 808 +++++++++++++++++++++++++++++ src/dynamics/rigid_body.rs | 6 + 3 files changed, 820 insertions(+) create mode 100644 src/control/ray_cast_vehicle_controller.rs (limited to 'src') diff --git a/src/control/mod.rs b/src/control/mod.rs index 0290f2c..3f7dea1 100644 --- a/src/control/mod.rs +++ b/src/control/mod.rs @@ -5,4 +5,10 @@ pub use self::character_controller::{ KinematicCharacterController, }; +#[cfg(feature = "dim3")] +pub use self::ray_cast_vehicle_controller::{DynamicRayCastVehicleController, Wheel, WheelTuning}; + mod character_controller; + +#[cfg(feature = "dim3")] +mod ray_cast_vehicle_controller; diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs new file mode 100644 index 0000000..42bb112 --- /dev/null +++ b/src/control/ray_cast_vehicle_controller.rs @@ -0,0 +1,808 @@ +//! A vehicle controller based on ray-casting, ported and modified from Bullet’s `btRaycastVehicle`. + +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderHandle, ColliderSet, Ray}; +use crate::math::{Point, Real, Rotation, Vector}; +use crate::pipeline::{QueryFilter, QueryPipeline}; +use crate::utils::{WCross, WDot}; + +/// A character controller to simulate vehicles using ray-casting for the wheels. +pub struct DynamicRayCastVehicleController { + wheels: Vec, + forward_ws: Vec>, + axle: Vec>, + /// The current forward speed of the vehicle. + pub current_vehicle_speed: Real, + + /// Handle of the vehicle’s chassis. + pub chassis: RigidBodyHandle, + /// The chassis’ local _up_ direction (`0 = x, 1 = y, 2 = z`) + pub index_up_axis: usize, + /// The chassis’ local _forward_ direction (`0 = x, 1 = y, 2 = z`) + pub index_forward_axis: usize, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +/// Parameters affecting the physical behavior of a wheel. +pub struct WheelTuning { + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub suspension_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub suspension_damping: Real, +