From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- src/geometry/broad_phase_multi_sap/broad_phase.rs | 220 +++++--- src/geometry/broad_phase_multi_sap/sap_layer.rs | 5 +- src/geometry/collider.rs | 305 +++++------ src/geometry/collider_components.rs | 220 ++++++++ src/geometry/collider_set.rs | 277 ++++------ src/geometry/contact_pair.rs | 22 +- src/geometry/mod.rs | 17 +- src/geometry/narrow_phase.rs | 593 ++++++++++++++-------- 8 files changed, 1014 insertions(+), 645 deletions(-) create mode 100644 src/geometry/collider_components.rs (limited to 'src/geometry') 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()); - } - - // 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); + 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); + } } - - // 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 { } } + fn handle_modified_collider( + &mut self, + prediction_distance: Real, + 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( + pub fn update( &mut self, prediction_distance: Real, - colliders: &mut ColliderSet, + 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; - } - - 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); - - 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 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; - } + 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 { + let (co_bf_data, co_pos, co_shape): ( + &ColliderBroadPhaseData, + &ColliderPosition, + &ColliderShape, + ) = colliders.index_bundle(handle.0); + + if !co_changes.needs_broad_phase_update() { + return; + } + let mut new_proxy_id = co_bf_data.proxy_index; + + 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); - - // 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]; + if co_bf_data.proxy_index != new_proxy_id { + self.colliders_proxy_ids.insert(*handle, new_proxy_id); - // 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) - } - - // 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); - } - } - } +impl Index for ColliderSet { + type Output = Collider; - 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,