aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/dynamics/coefficient_combine_rule.rs10
-rw-r--r--src/dynamics/rigid_body.rs14
-rw-r--r--src/dynamics/rigid_body_set.rs49
-rw-r--r--src/geometry/broad_phase_multi_sap/broad_phase.rs80
-rw-r--r--src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs8
-rw-r--r--src/geometry/broad_phase_multi_sap/sap_layer.rs21
-rw-r--r--src/geometry/broad_phase_multi_sap/sap_region.rs19
-rw-r--r--src/geometry/collider.rs110
-rw-r--r--src/geometry/collider_set.rs174
-rw-r--r--src/geometry/narrow_phase.rs391
-rw-r--r--src/pipeline/collision_pipeline.rs22
-rw-r--r--src/pipeline/physics_pipeline.rs75
12 files changed, 727 insertions, 246 deletions
diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs
index 2c66888..1bef022 100644
--- a/src/dynamics/coefficient_combine_rule.rs
+++ b/src/dynamics/coefficient_combine_rule.rs
@@ -21,6 +21,16 @@ pub enum CoefficientCombineRule {
}
impl CoefficientCombineRule {
+ pub fn from_value(val: u8) -> Self {
+ match val {
+ 0 => CoefficientCombineRule::Average,
+ 1 => CoefficientCombineRule::Min,
+ 2 => CoefficientCombineRule::Multiply,
+ 3 => CoefficientCombineRule::Max,
+ _ => panic!("Invalid coefficient combine rule."),
+ }
+ }
+
pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real {
let effective_rule = rule_value1.max(rule_value2);
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index e557df0..3c6bd65 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -42,7 +42,7 @@ bitflags::bitflags! {
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
- /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+ /// Flags describing how the rigid-body has been modified by the user.
pub(crate) struct RigidBodyChanges: u32 {
const MODIFIED = 1 << 0;
const POSITION = 1 << 1;
@@ -204,7 +204,7 @@ impl RigidBody {
self.flags.contains(RigidBodyFlags::CCD_ENABLED)
}
- pub fn is_moving_fast(&self, dt: Real) -> bool {
+ pub(crate) fn is_moving_fast(&self, dt: Real) -> bool {
self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
}
@@ -302,9 +302,13 @@ impl RigidBody {
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
- let collider = &mut colliders[*handle];
- collider.prev_position = self.position;
- collider.position = self.position * collider.delta;
+ // NOTE: we don't use `get_mut_internal` here because we want to
+ // benefit from the modification tracking to know the colliders
+ // we need to update the broad-phase and narrow-phase for.
+ let collider = colliders
+ .get_mut_internal_with_modification_tracking(*handle)
+ .unwrap();
+ collider.set_position(self.position * collider.delta);
}
}
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index f459d4f..2ae298f 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -220,13 +220,17 @@ impl RigidBodySet {
///
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not
/// suffer form the ABA problem.
+ #[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
- let result = self.bodies.get_unknown_gen_mut(i)?;
- if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
- result.0.changes = RigidBodyChanges::MODIFIED;
- self.modified_bodies.push(RigidBodyHandle(result.1));
- }
- Some((result.0, RigidBodyHandle(result.1)))
+ let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
+ let handle = RigidBodyHandle(handle);
+ Self::mark_as_modified(
+ handle,
+ rb,
+ &mut self.modified_bodies,
+ self.modified_all_bodies,
+ );
+ Some((rb, handle))
}
/// Gets the rigid-body with the given handle.
@@ -247,6 +251,7 @@ impl RigidBodySet {
}
/// Gets a mutable reference to the rigid-body with the given handle.
+ #[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle.0)?;
Self::mark_as_modified(
@@ -262,6 +267,22 @@ impl RigidBodySet {
self.bodies.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: RigidBodyHandle,
+ ) -> Option<&mut RigidBody> {
+ let result = self.bodies.get_mut(handle.0)?;
+ Self::mark_as_modified(
+ handle,
+ result,
+ &mut self.modified_bodies,
+ self.modified_all_bodies,
+ );
+ Some(result)
+ }
+
pub(crate) fn get2_mut_internal(
&mut self,
h1: RigidBodyHandle,
@@ -276,6 +297,7 @@ impl RigidBodySet {
}
/// Iterates mutably through all the rigid-bodies on this set.
+ #[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
self.modified_bodies.clear();
self.modified_all_bodies = true;
@@ -317,6 +339,7 @@ impl RigidBodySet {
/// Applies the given function on all the active dynamic rigid-bodies
/// contained by this set.
#[inline(always)]
+ #[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn foreach_active_dynamic_body_mut(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
@@ -467,7 +490,7 @@ impl RigidBodySet {
rb.changes = RigidBodyChanges::empty();
}
- pub(crate) fn maintain(&mut self, colliders: &mut ColliderSet) {
+ pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) {
if self.modified_all_bodies {
for (handle, rb) in self.bodies.iter_mut() {
Self::maintain_one(
@@ -651,8 +674,16 @@ impl Index<RigidBodyHandle> for RigidBodySet {
}
}
+#[cfg(not(feature = "dev-remove-slow-accessors"))]
impl IndexMut<RigidBodyHandle> for RigidBodySet {
- fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
- &mut self.bodies[index.0]
+ fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
+ let rb = &mut self.bodies[handle.0];
+ Self::mark_as_modified(
+ handle,
+ rb,
+ &mut self.modified_bodies,
+ self.modified_all_bodies,
+ );
+ rb
}
}
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index c97c737..e036ec0 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -2,8 +2,8 @@ use super::{
BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
};
use crate::data::pubsub::Subscription;
-use crate::dynamics::RigidBodySet;
use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
+use crate::geometry::collider::ColliderChanges;
use crate::geometry::{ColliderSet, RemovedCollider};
use crate::math::Real;
use crate::utils::IndexMut2;
@@ -340,7 +340,6 @@ impl BroadPhase {
pub fn update(
&mut self,
prediction_distance: Real,
- bodies: &RigidBodySet,
colliders: &mut ColliderSet,
events: &mut Vec<BroadPhasePairEvent>,
) {
@@ -350,39 +349,54 @@ impl BroadPhase {
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
- for body_handle in bodies
- .modified_inactive_set
- .iter()
- .chain(bodies.active_dynamic_set.iter())
- .chain(bodies.active_kinematic_set.iter())
- {
- for handle in &bodies[*body_handle].colliders {
- let collider = &mut colliders[*handle];
- 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) {
- proxy.aabb = aabb;
- proxy.layer_id
- } else {
- let layer_depth = super::layer_containing_aabb(&aabb);
- let layer_id = self.ensure_layer_exists(layer_depth);
+ colliders.foreach_modified_colliders_mut_internal(|handle, collider| {
+ if !collider.changes.needs_broad_phase_update() {
+ return;
+ }
- // Create the proxy.
- let proxy = SAPProxy::collider(*handle, aabb, layer_id, layer_depth);
- collider.proxy_index = self.proxies.insert(proxy);
- layer_id
- };
+ 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;
+ }
+ }
- let layer = &mut self.layers[layer_id as usize];
+ layer_id
+ } else {
+ let layer_depth = super::layer_containing_aabb(&aabb);
+ let layer_id = self.ensure_layer_exists(layer_depth);
- // 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();
- }
- }
+ // Create the proxy.
+ let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
+ collider.proxy_index = self.proxies.insert(proxy);
+ layer_id
+ };
+
+ let layer = &mut self.layers[layer_id as usize];
+
+ // Preupdate the collider in the layer.
+ layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool);
+ need_region_propagation = need_region_propagation || !layer.created_regions.is_empty();
+ });
// Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
if need_region_propagation {
@@ -527,7 +541,7 @@ mod test {
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
bodies.remove(hrb, &mut colliders, &mut joints);
- broad_phase.maintain(&mut colliders);
+ broad_phase.handle_user_changes(&mut colliders);
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
// Create another body.
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs b/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs
index c27917b..fdc9bfd 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs
@@ -15,14 +15,6 @@ impl ColliderPair {
}
}
- pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
- if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
- Self::new(collider1, collider2)
- } else {
- Self::new(collider2, collider1)
- }
- }
-
pub fn swap(self) -> Self {
Self::new(self.collider2, self.collider1)
}
diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs
index 1ee4468..9a9c296 100644
--- a/src/geometry/broad_phase_multi_sap/sap_layer.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs
@@ -294,7 +294,7 @@ impl SAPLayer {
proxies[*subregion_id].data.as_region_mut().mark_as_dirty();
}
- if region.subproper_proxy_count == 0 {
+ if !region.contains_subproper_proxies() {
self.regions_to_potentially_remove.push(*point);
}
@@ -325,7 +325,7 @@ impl SAPLayer {
region.update_after_subregion_removal(proxies, self.depth);
// Check if we can actually delete this region.
- if region.subproper_proxy_count == 0 {
+ if !region.contains_subproper_proxies() {
let region_id = region_id.remove();
// We can delete this region. So we need to tell the larger
@@ -352,4 +352,21 @@ impl SAPLayer {
}
}
}
+
+ pub fn proper_proxy_moved_to_bigger_layer(
+ &mut self,
+ proxies: &mut SAPProxies,
+ proxy_id: SAPProxyIndex,
+ ) {
+ for (point, region_id) in &self.regions {
+ let region = &mut proxies[*region_id].data.as_region_mut();
+ let region_contains_proxy = region.proper_proxy_moved_to_a_bigger_layer(proxy_id);
+
+ // If that proper proxy was the last one keeping that region
+ // alive, mark the region as potentially removable.
+ if region_contains_proxy && !region.contains_subproper_proxies() {
+ self.regions_to_potentially_remove.push(*point);
+ }
+ }
+ }
}
diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs
index 1c3036d..4cebdda 100644
--- a/src/geometry/broad_phase_multi_sap/sap_region.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_region.rs
@@ -21,7 +21,7 @@ pub struct SAPRegion {
// Number of proxies (added to this region) that originates
// from the layer at depth <= the depth of the layer containing
// this region.
- pub subproper_proxy_count: usize,
+ subproper_proxy_count: usize,
}
impl SAPRegion {
@@ -73,6 +73,23 @@ impl SAPRegion {
}
}
+ /// Does this region still contain endpoints of subproper proxies?
+ pub fn contains_subproper_proxies(&self) -> bool {
+ self.subproper_proxy_count > 0
+ }
+
+ /// If this region contains the given proxy, this will decrement this region's proxy count.
+ ///
+ /// Returns `true` if this region contained the proxy. Returns `false` otherwise.
+ pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: SAPProxyIndex) -> bool {
+ if self.existing_proxies[proxy_id as usize] {
+ self.subproper_proxy_count -= 1;
+ true
+ } else {
+ false
+ }
+ }
+
/// Deletes from the axes of this region all the endpoints that point
/// to a region.
pub fn delete_all_region_endpoints(&mut self, proxies: &SAPProxies) {
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 43f8294..e35603e 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -2,6 +2,7 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
use crate::geometry::{InteractionGroups, SAPProxyIndex, 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;
@@ -49,6 +50,34 @@ enum MassInfo {
MassProperties(Box<MassProperties>),
}
+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.
@@ -59,10 +88,10 @@ pub struct Collider {
mass_info: MassInfo,
pub(crate) flags: ColliderFlags,
pub(crate) solver_flags: SolverFlags,
+ pub(crate) changes: ColliderChanges,
pub(crate) parent: RigidBodyHandle,
pub(crate) delta: Isometry<Real>,
pub(crate) position: Isometry<Real>,
- pub(crate) prev_position: Isometry<Real>,
/// The friction coefficient of this collider.
pub friction: Real,
/// The restitution coefficient of this collider.
@@ -78,6 +107,7 @@ impl Collider {
pub(crate) fn reset_internal_references(&mut self) {
self.parent = RigidBodyHandle::invalid();
self.proxy_index = crate::INVALID_U32;
+ self.changes = ColliderChanges::empty();
}
/// The rigid body this collider is attached to.
@@ -90,6 +120,42 @@ impl Collider {
self.flags.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())
+ }
+
+ /// 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);
+ }
+
+ /// 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())
+ }
+
+ /// 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)
+ }
+
+ /// 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);
+ }
+ }
+
#[doc(hidden)]
pub fn set_position_debug(&mut self, position: Isometry<Real>) {
self.position = position;
@@ -106,21 +172,49 @@ impl Collider {
&self.position
}
+ /// Sets the position of this collider wrt. its parent rigid-body.
+ pub(crate) fn set_position(&mut self, position: Isometry<Real>) {
+ self.changes.insert(ColliderChanges::POSITION);
+ self.position = position;
+ }
+
/// The position of this collider wrt the body it is attached to.
pub fn position_wrt_parent(&self) -> &Isometry<Real> {
&self.delta
}
+ /// Sets the position of this collider wrt. its parent rigid-body.
+ pub fn set_position_wrt_parent(&mut self, position: Isometry<Real>) {
+ self.changes.insert(ColliderChanges::POSITION_WRT_PARENT);
+ self.delta = position;
+ }
+
/// The collision groups used by this collider.
pub fn collision_groups(&self) -> InteractionGroups {
self.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;
+ }
+ }
+
/// The solver groups used by this collider.
pub fn solver_groups(&self) -> InteractionGroups {
self.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;
+ }
+ }
+
/// The density of this collider, if set.
pub fn density(&self) -> Option<Real> {
match &self.mass_info {
@@ -134,6 +228,12 @@ impl Collider {
&*self.shape.0
}
+ /// Sets the shape of this collider.
+ pub fn set_shape(&mut self, shape: SharedShape) {
+ self.changes.insert(ColliderChanges::SHAPE);
+ self.shape = shape;
+ }
+
/// Compute the axis-aligned bounding box of this collider.
pub fn compute_aabb(&self) -> AABB {
self.shape.compute_aabb(&self.position)
@@ -219,6 +319,12 @@ impl ColliderBuilder {
Self::new(SharedShape::ball(radius))
}
+ /// Initialize a new collider build with a half-space shape defined by the outward normal
+ /// of its planar boundary.
+ pub fn halfspace(outward_normal: Unit<Vector<Real>>) -> Self {
+ Self::new(SharedShape::halfspace(outward_normal))
+ }
+
/// Initialize a new collider builder with a cylindrical shape defined by its half-height
/// (along along the y axis) and its radius.
#[cfg(feature = "dim3")]
@@ -595,8 +701,8 @@ impl ColliderBuilder {
delta: self.delta,
flags,
solver_flags,
+ changes: ColliderChanges::all(),
parent: RigidBodyHandle::invalid(),
- prev_position: Isometry::identity(),
position: Isometry::identity(),
proxy_index: crate::INVALID_U32,
collision_groups: self.collision_groups,
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index d84639c..a68aac7 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -1,6 +1,7 @@
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 std::ops::{Index, IndexMut};
@@ -54,6 +55,8 @@ pub(crate) struct RemovedCollider {
pub struct ColliderSet {
pub(crate) removed_colliders: PubSub<RemovedCollider>,
pub(crate) colliders: Arena<Collider>,
+ pub(crate) modified_colliders: Vec<ColliderHandle>,
+ pub(crate) modified_all_colliders: bool,
}
impl ColliderSet {
@@ -62,6 +65,8 @@ impl ColliderSet {
ColliderSet {
removed_colliders: PubSub::new(),
colliders: Arena::new(),
+ modified_colliders: Vec::new(),
+ modified_all_colliders: false,
}
}
@@ -75,6 +80,37 @@ impl ColliderSet {
self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c))
}
+ /// Iterates mutably through all the colliders on this set.
+ #[cfg(not(feature = "dev-remove-slow-accessors"))]
+ pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
+ 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)
+ }
+ }
+ }
+
/// The number of colliders on this set.
pub fn len(&self) -> usize {
self.colliders.len()
@@ -90,6 +126,24 @@ 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(..) {
+ self.colliders[handle.0].changes = ColliderChanges::empty();
+ }
+ }
+ }
+
/// Inserts a new collider to this set and retrieve its handle.
pub fn insert(
&mut self,
@@ -106,11 +160,12 @@ impl ColliderSet {
// NOTE: we use `get_mut` instead of `get_mut_internal` so that the
// modification flag is updated properly.
let parent = bodies
- .get_mut(parent_handle)
+ .get_mut_internal_with_modification_tracking(parent_handle)
.expect("Parent rigid body not found.");
- coll.prev_position = parent.position * coll.delta;
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);
handle
@@ -133,7 +188,7 @@ impl ColliderSet {
*/
// NOTE: we use `get_mut` instead of `get_mut_internal` so that the
// modification flag is updated properly.
- if let Some(parent) = bodies.get_mut(collider.parent) {
+ if let Some(parent) = bodies.get_mut_internal_with_modification_tracking(collider.parent) {
parent.remove_collider_internal(handle, &collider);
if wake_up {
@@ -178,10 +233,17 @@ impl ColliderSet {
///
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not
/// suffer form the ABA problem.
+ #[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> {
- self.colliders
- .get_unknown_gen_mut(i)
- .map(|(c, h)| (c, ColliderHandle(h)))
+ 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,
+ );
+ Some((collider, handle))
}
/// Get the collider with the given handle.
@@ -189,31 +251,79 @@ impl ColliderSet {
self.colliders.get(handle.0)
}
+ fn mark_as_modified(
+ handle: ColliderHandle,
+ collider: &mut Collider,
+ modified_colliders: &mut Vec<ColliderHandle>,
+ modified_all_colliders: bool,
+ ) {
+ if !modified_all_colliders && !collider.changes.contains(ColliderChanges::MODIFIED) {
+ collider.changes = ColliderChanges::MODIFIED;
+ modified_colliders.push(handle);
+ }
+ }
+
/// Gets a mutable reference to the collider with the given handle.
+ #[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,
+ );
+ Some(result)
+ }
+
+ pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
self.colliders.get_mut(handle.0)
}
- // pub(crate) fn get2_mut_internal(
- // &mut self,
- // h1: ColliderHandle,
- // h2: ColliderHandle,
- // ) -> (Option<&mut Collider>, Option<&mut Collider>) {
- // self.colliders.get2_mut(h1, h2)
- // }
-
- // pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, ColliderMut)> {
- // // let sender = &self.activation_channel_sender;
- // self.colliders.iter_mut().map(move |(h, rb)| {
- // (h, ColliderMut::new(h, rb /*sender.clone()*/))
- // })
- // }
-
- // pub(crate) fn iter_mut_internal(
- // &mut self,
- // ) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
- // self.colliders.iter_mut()
- // }
+ // 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);
+ }
+ }
+ }
+
+ 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.drain(..) {
+ if let Some(rb) = self.colliders.get_mut(handle.0) {
+ Self::maintain_one(bodies, rb)
+ }
+ }
+ }
+ }
}
impl Index<ColliderHandle> for ColliderSet {
@@ -224,8 +334,16 @@ impl Index<ColliderHandle> for ColliderSet {
}
}
+#[cfg(not(feature = "dev-remove-slow-accessors"))]
impl IndexMut<ColliderHandle> for ColliderSet {
- fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider {
- &mut self.colliders[index.0]
+ 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,
+ );
+ collider
}
}
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 7de0b26..de199ec 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -4,9 +4,10 @@ use rayon::prelude::*;
use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet};
+use crate::geometry::collider::ColliderChanges;
use crate::geometry::{
- BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderSet, ContactData,
- ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,<