nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

216
src/geometry/broad_phase_multi_sap/broad_phase.rs
View File

@@ -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<SAPLayer>,
smallest_layer: u8,
largest_layer: u8,
removed_colliders: Option<Subscription<RemovedCollider>>,
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<ColliderHandle, SAPProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
region_pool: SAPRegionPool, // To avoid repeated allocations.
// We could think serializing this workspace is useless.
@@ -107,13 +120,13 @@ impl BroadPhase {
/// Create a new empty broad-phase.
pub fn new() -> Self {
BroadPhase {
removed_colliders: None,
proxies: SAPProxies::new(),
layers: Vec::new(),
smallest_layer: 0,
largest_layer: 0,
region_pool: Vec::new(),
reporting: HashMap::default(),
colliders_proxy_ids: HashMap::default(),
deleted_any: false,
}
}
@@ -123,26 +136,13 @@ impl BroadPhase {
/// For each colliders marked as removed, we make their containing layer mark
/// its proxy as pre-deleted. The actual proxy removal will happen at the end
/// of the `BroadPhase::update`.
fn handle_removed_colliders(&mut self, colliders: &mut ColliderSet) {
// Ensure we already subscribed the collider-removed events.
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
// For each removed collider, remove the corresponding proxy.
for removed in removed_colliders {
if let Some(proxy_id) = self.colliders_proxy_ids.get(removed).copied() {
self.predelete_proxy(proxy_id);
}
}
// Extract the cursor to avoid borrowing issues.
let cursor = self.removed_colliders.take().unwrap();
// Read all the collider-removed events, and remove the corresponding proxy.
for collider in colliders.removed_colliders.read(&cursor) {
self.predelete_proxy(collider.proxy_index);
}
// NOTE: We don't acknowledge the cursor just yet because we need
// to traverse the set of removed colliders one more time after
// the broad-phase update.
// Re-insert the cursor we extracted to avoid borrowing issues.
self.removed_colliders = Some(cursor);
}
/// Pre-deletes a proxy from this broad-phase.
@@ -173,7 +173,7 @@ impl BroadPhase {
/// This method will actually remove from the proxy list all the proxies
/// marked as deletable by `self.predelete_proxy`, making their proxy
/// handles re-usable by new proxies.
fn complete_removals(&mut self, colliders: &mut ColliderSet) {
fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) {
// If there is no layer, there is nothing to remove.
if self.layers.is_empty() {
return;
@@ -215,13 +215,13 @@ impl BroadPhase {
/*
* Actually remove the colliders proxies.
*/
let cursor = self.removed_colliders.as_ref().unwrap();
for collider in colliders.removed_colliders.read(&cursor) {
if collider.proxy_index != crate::INVALID_U32 {
self.proxies.remove(collider.proxy_index);
for removed in removed_colliders {
if let Some(proxy_id) = self.colliders_proxy_ids.remove(removed) {
if proxy_id != crate::INVALID_U32 {
self.proxies.remove(proxy_id);
}
}
}
colliders.removed_colliders.ack(&cursor);
}
/// Finalize the insertion of the layer identified by `layer_id`.
@@ -336,67 +336,127 @@ impl BroadPhase {
}
}
/// Updates the broad-phase, taking into account the new collider positions.
pub fn update(
fn handle_modified_collider(
&mut self,
prediction_distance: Real,
colliders: &mut ColliderSet,
handle: ColliderHandle,
proxy_index: &mut u32,
collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
) -> bool {
let (co_pos, co_shape, co_changes) = collider;
let mut aabb = co_shape
.compute_aabb(co_pos)
.loosened(prediction_distance / 2.0);
aabb.mins = super::clamp_point(aabb.mins);
aabb.maxs = super::clamp_point(aabb.maxs);
let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) {
let mut layer_id = proxy.layer_id;
proxy.aabb = aabb;
if co_changes.contains(ColliderChanges::SHAPE) {
// If the shape was changed, then we need to see if this proxy should be
// migrated to a larger layer. Indeed, if the shape was replaced by
// a much larger shape, we need to promote the proxy to a bigger layer
// to avoid the O(n²) discretization problem.
let new_layer_depth = super::layer_containing_aabb(&aabb);
if new_layer_depth > proxy.layer_depth {
self.layers[proxy.layer_id as usize]
.proper_proxy_moved_to_bigger_layer(&mut self.proxies, *proxy_index);
// We need to promote the proxy to the bigger layer.
layer_id = self.ensure_layer_exists(new_layer_depth);
self.proxies[*proxy_index].layer_id = layer_id;
}
}
layer_id
} else {
let layer_depth = super::layer_containing_aabb(&aabb);
let layer_id = self.ensure_layer_exists(layer_depth);
// Create the proxy.
let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
*proxy_index = self.proxies.insert(proxy);
layer_id
};
let layer = &mut self.layers[layer_id as usize];
// Preupdate the collider in the layer.
layer.preupdate_collider(
*proxy_index,
&aabb,
&mut self.proxies,
&mut self.region_pool,
);
let need_region_propagation = !layer.created_regions.is_empty();
need_region_propagation
}
/// Updates the broad-phase, taking into account the new collider positions.
pub fn update<Colliders>(
&mut self,
prediction_distance: Real,
colliders: &mut Colliders,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
) where
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSet<ColliderChanges>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(colliders);
self.handle_removed_colliders(removed_colliders);
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
colliders.foreach_modified_colliders_mut_internal(|handle, collider| {
if !collider.changes.needs_broad_phase_update() {
return;
}
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
aabb.mins = super::clamp_point(aabb.mins);
aabb.maxs = super::clamp_point(aabb.maxs);
if let Some(co_changes) = co_changes {
let (co_bf_data, co_pos, co_shape): (
&ColliderBroadPhaseData,
&ColliderPosition,
&ColliderShape,
) = colliders.index_bundle(handle.0);
let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
let mut layer_id = proxy.layer_id;
proxy.aabb = aabb;
if !co_changes.needs_broad_phase_update() {
return;
}
let mut new_proxy_id = co_bf_data.proxy_index;
if collider.changes.contains(ColliderChanges::SHAPE) {
// If the shape was changed, then we need to see if this proxy should be
// migrated to a larger layer. Indeed, if the shape was replaced by
// a much larger shape, we need to promote the proxy to a bigger layer
// to avoid the O(n²) discretization problem.
let new_layer_depth = super::layer_containing_aabb(&aabb);
if new_layer_depth > proxy.layer_depth {
self.layers[proxy.layer_id as usize].proper_proxy_moved_to_bigger_layer(
&mut self.proxies,
collider.proxy_index,
);
// We need to promote the proxy to the bigger layer.
layer_id = self.ensure_layer_exists(new_layer_depth);
self.proxies[collider.proxy_index].layer_id = layer_id;
}
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
(co_pos, co_shape, co_changes),
) {
need_region_propagation = true;
}
layer_id
} else {
let layer_depth = super::layer_containing_aabb(&aabb);
let layer_id = self.ensure_layer_exists(layer_depth);
if co_bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Create the proxy.
let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
collider.proxy_index = self.proxies.insert(proxy);
layer_id
};
let layer = &mut self.layers[layer_id as usize];
// Preupdate the collider in the layer.
layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool);
need_region_propagation = need_region_propagation || !layer.created_regions.is_empty();
});
// Make sure we have the new proxy index in case
// the collider was added for the first time.
colliders.set_internal(
handle.0,
ColliderBroadPhaseData {
proxy_index: new_proxy_id,
},
);
}
}
}
// Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
if need_region_propagation {
@@ -408,7 +468,7 @@ impl BroadPhase {
// Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
// layers to possible remove regions from larger layers that would become empty that way.
self.complete_removals(colliders);
self.complete_removals(removed_colliders);
}
/// Propagate regions from the smallest layers up to the larger layers.

5
src/geometry/broad_phase_multi_sap/sap_layer.rs
View File

@@ -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);

305
src/geometry/collider.rs
View File

@@ -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<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.
///
/// 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<Real>,
pub(crate) position: Isometry<Real>,
/// 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<Real>) {
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<Real> {
&self.delta
&self.co_parent.pos_wrt_parent
}
/// The world-space position of this collider.
pub fn position(&self) -> &Isometry<Real> {
&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;
&self.co_pos
}
/// The position of this collider wrt the body it is attached to.
pub fn position_wrt_parent(&self) -> &Isometry<Real> {
&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<Real>) {
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<Real> {
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<Real>) -> 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<Real>,
pub density: Option<Real>,
/// Overrides automatic computation of `MassProperties`.
/// If None, it will be computed based on shape and density.
mass_properties: Option<MassProperties>,
pub mass_properties: Option<MassProperties>,
/// 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<Real>,
pub pos_wrt_parent: Isometry<Real>,
/// 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<Real>) -> 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<Real>) -> 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<Real>) -> 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<Real>) -> 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,
)
}
}

220
src/geometry/collider_components.rs Normal file
View File

@@ -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<MassProperties>),
}
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<Real>,
}
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ColliderPosition(pub Isometry<Real>);
impl AsRef<Isometry<Real>> for ColliderPosition {
#[inline]
fn as_ref(&self) -> &Isometry<Real> {
&self.0
}
}
impl Deref for ColliderPosition {
type Target = Isometry<Real>;
#[inline]
fn deref(&self) -> &Isometry<Real> {
&self.0
}
}
impl Default for ColliderPosition {
fn default() -> Self {
Self::identity()
}
}
impl ColliderPosition {
#[must_use]
fn identity() -> Self {
ColliderPosition(Isometry::identity())
}
}
impl<T> From<T> for ColliderPosition
where
Isometry<Real>: From<T>,
{
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(),
}
}
}

277
src/geometry/collider_set.rs
View File

@@ -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<RemovedCollider>,
pub(crate) colliders: Arena<Collider>,
pub(crate) modified_colliders: Vec<ColliderHandle>,
pub(crate) modified_all_colliders: bool,
pub(crate) removed_colliders: Vec<ColliderHandle>,
}
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<Result>(
&mut self,
handle: crate::data::Index,
f: impl FnOnce(&mut $T) -> Result,
) -> Option<Result> {
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<ColliderHandle> {
std::mem::replace(&mut self.modified_colliders, vec![])
}
pub(crate) fn take_removed(&mut self) -> Vec<ColliderHandle> {
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<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)
}
}
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<Collider> {
@@ -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<ColliderHandle>,
modified_all_colliders: bool,
) {
if !modified_all_colliders && !collider.changes.contains(ColliderChanges::MODIFIED) {
collider.changes = ColliderChanges::MODIFIED;
if !collider.co_changes.contains(ColliderChanges::MODIFIED) {
collider.co_changes = ColliderChanges::MODIFIED;
modified_colliders.push(handle);
}
}
@@ -272,62 +240,20 @@ impl ColliderSet {
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
let result = self.colliders.get_mut(handle.0)?;
Self::mark_as_modified(
handle,
result,
&mut self.modified_colliders,
self.modified_all_colliders,
);
Self::mark_as_modified(handle, result, &mut self.modified_colliders);
Some(result)
}
pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
self.colliders.get_mut(handle.0)
}
}
// Just a very long name instead of `.get_mut` to make sure
// this is really the method we wanted to use instead of `get_mut_internal`.
pub(crate) fn get_mut_internal_with_modification_tracking(
&mut self,
handle: ColliderHandle,
) -> Option<&mut Collider> {
let result = self.colliders.get_mut(handle.0)?;
Self::mark_as_modified(
handle,
result,
&mut self.modified_colliders,
self.modified_all_colliders,
);
Some(result)
}
impl Index<crate::data::Index> for ColliderSet {
type Output = Collider;
// Utility function to avoid some borrowing issue in the `maintain` method.
fn maintain_one(bodies: &mut RigidBodySet, collider: &mut Collider) {
if collider
.changes
.contains(ColliderChanges::POSITION_WRT_PARENT)
{
if let Some(parent) = bodies.get_mut_internal(collider.parent()) {
let position = parent.position * collider.position_wrt_parent();
// NOTE: the set_position method will add the ColliderChanges::POSITION flag,
// which is needed for the broad-phase/narrow-phase to detect the change.
collider.set_position(position);
}
}
}
pub(crate) fn handle_user_changes(&mut self, bodies: &mut RigidBodySet) {
if self.modified_all_colliders {
for (_, rb) in self.colliders.iter_mut() {
Self::maintain_one(bodies, rb)
}
} else {
for handle in self.modified_colliders.iter() {
if let Some(rb) = self.colliders.get_mut(handle.0) {
Self::maintain_one(bodies, rb)
}
}
}
fn index(&self, index: crate::data::Index) -> &Collider {
&self.colliders[index]
}
}
@@ -343,12 +269,7 @@ impl Index<ColliderHandle> for ColliderSet {
impl IndexMut<ColliderHandle> 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
}
}

22
src/geometry/contact_pair.rs
View File

@@ -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<RigidBodyHandle>,
/// The second rigid-body involved in this contact manifold.
pub rigid_body2: Option<RigidBodyHandle>,
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<RigidBodyHandle>,
rigid_body2: Option<RigidBodyHandle>,
solver_flags: SolverFlags,
) -> ContactManifoldData {
Self {
body_pair,
rigid_body1,
rigid_body2,
warmstart_multiplier: Self::min_warmstart_multiplier(),
constraint_index: 0,
position_constraint_index: 0,

17
src/geometry/mod.rs
View File

@@ -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<dyn parry::query::Que
}
mod broad_phase_multi_sap;
mod collider;
mod collider_set;
mod collider_components;
mod contact_pair;
mod interaction_graph;
mod interaction_groups;
mod narrow_phase;
#[cfg(feature = "default-sets")]
mod collider;
#[cfg(feature = "default-sets")]
mod collider_set;

595
src/geometry/narrow_phase.rs
View File

@@ -1,14 +1,16 @@
#[cfg(feature = "parallel")]
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::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::CoefficientCombineRule;
use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet,
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderGroups, ColliderHandle,
ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
ContactData, ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
IntersectionEvent, RemovedCollider, SolverContact, SolverFlags,
IntersectionEvent, SolverContact, SolverFlags,
};
use crate::math::{Real, Vector};
use crate::pipeline::{
@@ -54,7 +56,6 @@ pub struct NarrowPhase {
contact_graph: InteractionGraph<ColliderHandle, ContactPair>,
intersection_graph: InteractionGraph<ColliderHandle, bool>,
graph_indices: Coarena<ColliderGraphIndices>,
removed_colliders: Option<Subscription<RemovedCollider>>,
}
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<Bodies, Colliders>(
&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<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>,
{
// TODO: avoid these hash-maps.
// They are necessary to handle the swap-remove done internally
// by the contact/intersection graphs when a node is removed.
let mut prox_id_remap = HashMap::new();
let mut contact_id_remap = HashMap::new();
let mut i = 0;
while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
for collider in removed_colliders {
// NOTE: if the collider does not have any graph indices currently, there is nothing
// to remove in the narrow-phase for this collider.
if let Some(graph_idx) = self.graph_indices.get(collider.handle.0) {
if let Some(graph_idx) = self.graph_indices.get(collider.0) {
let intersection_graph_id = prox_id_remap
.get(&collider.handle)
.get(collider)
.copied()
.unwrap_or(graph_idx.intersection_graph_index);
let contact_graph_id = contact_id_remap
.get(&collider.handle)
.get(collider)
.copied()
.unwrap_or(graph_idx.contact_graph_index);
self.remove_collider(
intersection_graph_id,
contact_graph_id,
islands,
colliders,
bodies,
&mut prox_id_remap,
&mut contact_id_remap,
);
}
i += 1;
}
colliders.removed_colliders.ack(&cursor);
self.removed_colliders = Some(cursor);
self.handle_modified_colliders(colliders, bodies, events);
self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
}
pub(crate) fn remove_collider(
pub(crate) fn remove_collider<Bodies, Colliders>(
&mut self,
intersection_graph_id: ColliderGraphIndex,
contact_graph_id: ColliderGraphIndex,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
colliders: &mut Colliders,
bodies: &mut Bodies,
prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSetOption<ColliderParent>,
{
// Wake up every body in contact with the deleted collider.
for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
if let Some(parent) = colliders.get(a).map(|c| c.parent) {
bodies.wake_up(parent, true)
if let Some(parent) = colliders.get(a.0).map(|c| c.handle) {
islands.wake_up(bodies, parent, true)
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent, true)
if let Some(parent) = colliders.get(b.0).map(|c| c.handle) {
islands.wake_up(bodies, parent, true)
}
}
@@ -263,78 +267,104 @@ impl NarrowPhase {
}
}
pub(crate) fn handle_modified_colliders(
pub(crate) fn handle_modified_colliders<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
modified_colliders: &[ColliderHandle],
colliders: &Colliders,
bodies: &mut Bodies,
events: &dyn EventHandler,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>,
{
let mut pairs_to_remove = vec![];
colliders.foreach_modified_colliders(|handle, collider| {
if collider.changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider.
return;
}
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
if let Some(gid) = self.graph_indices.get(handle.0) {
// For each modified colliders, we need to wake-up the bodies it is in contact with
// so that the narrow-phase properly takes into account the change in, e.g.,
// collision groups. Waking up the modified collider's parent isn't enough because
// it could be a static or kinematic body which don't propagate the wake-up state.
bodies.wake_up(collider.parent, true);
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
let other_handle = if handle == inter.0 { inter.1 } else { inter.0 };
if let Some(other_collider) = colliders.get(other_handle) {
bodies.wake_up(other_collider.parent, true);
}
if let Some(co_changes) = co_changes {
if co_changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider.
return;
}
// For each collider which had their sensor status modified, we need
// to transfer their contact/intersection graph edges to the intersection/contact graph.
// To achieve this we will remove the relevant contact/intersection pairs form the
// contact/intersection graphs, and then add them into the other graph.
if collider.changes.contains(ColliderChanges::SENSOR) {
if collider.is_sensor() {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove`.
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromContactGraph,
));
if let Some(gid) = self.graph_indices.get(handle.0) {
// For each modified colliders, we need to wake-up the bodies it is in contact with
// so that the narrow-phase properly takes into account the change in, e.g.,
// collision groups. Waking up the modified collider's parent isn't enough because
// it could be a static or kinematic body which don't propagate the wake-up state.
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
let (co_changes, co_type): (&ColliderChanges, &ColliderType) =
colliders.index_bundle(handle.0);
if let Some(co_parent) = co_parent {
islands.wake_up(bodies, co_parent.handle, true);
}
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 };
let other_parent: Option<&ColliderParent> = colliders.get(other_handle.0);
if let Some(other_parent) = other_parent {
islands.wake_up(bodies, other_parent.handle, true);
}
} else {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove` if both involved
// colliders are not sensors.
for inter in self
.intersection_graph
.interactions_with(gid.intersection_graph_index)
.filter(|(h1, h2, _)| {
!colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
})
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromIntersectionGraph,
));
}
// For each collider which had their sensor status modified, we need
// to transfer their contact/intersection graph edges to the intersection/contact graph.
// To achieve this we will remove the relevant contact/intersection pairs form the
// contact/intersection graphs, and then add them into the other graph.
if co_changes.contains(ColliderChanges::TYPE) {
if co_type.is_sensor() {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove`.
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromContactGraph,
));
}
} else {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove` if both involved
// colliders are not sensors.
for inter in self
.intersection_graph
.interactions_with(gid.intersection_graph_index)
.filter(|(h1, h2, _)| {
let co_type1: &ColliderType = colliders.index(h1.0);
let co_type2: &ColliderType = colliders.index(h2.0);
!co_type1.is_sensor() && !co_type2.is_sensor()
})
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromIntersectionGraph,
));
}
}
}
}
}
});
}
// Remove the pair from the relevant graph.
for pair in &pairs_to_remove {
self.remove_pair(colliders, bodies, &pair.0, events, pair.1);
self.remove_pair(islands, colliders, bodies, &pair.0, events, pair.1);
}
// Add the paid removed pair to the relevant graph.
@@ -343,17 +373,24 @@ impl NarrowPhase {
}
}
fn remove_pair(
fn remove_pair<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
colliders: &Colliders,
bodies: &mut Bodies,
pair: &ColliderPair,
events: &dyn EventHandler,
mode: PairRemovalMode,
) {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
// TODO: could we just unwrap here?
// Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
if let (Some(gid1), Some(gid2)) = (
@@ -361,7 +398,8 @@ impl NarrowPhase {
self.graph_indices.get(pair.collider2.0),
) {
if mode == PairRemovalMode::FromIntersectionGraph
|| (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
|| (mode == PairRemovalMode::Auto
&& (co_type1.is_sensor() || co_type2.is_sensor()))
{
let was_intersecting = self
.intersection_graph
@@ -382,8 +420,18 @@ impl NarrowPhase {
// Also wake up the dynamic bodies that were in contact.
if let Some(ctct) = contact_pair {
if ctct.has_any_active_contact {
bodies.wake_up(co1.parent, true);
bodies.wake_up(co2.parent, true);
let co_parent1: Option<&ColliderParent> =
colliders.get(pair.collider1.0);
let co_parent2: Option<&ColliderParent> =
colliders.get(pair.collider2.0);
if let Some(co_parent1) = co_parent1 {
islands.wake_up(bodies, co_parent1.handle, true);
}
if let Some(co_parent2) = co_parent2 {
islands.wake_up(bodies, co_parent2.handle, true);
}
events.handle_contact_event(ContactEvent::Stopped(
pair.collider1,
@@ -396,11 +444,18 @@ impl NarrowPhase {
}
}
fn add_pair(&mut self, colliders: &mut ColliderSet, pair: &ColliderPair) {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if co1.parent == co2.parent {
fn add_pair<Colliders>(&mut self, colliders: &Colliders, pair: &ColliderPair)
where
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
if co_parent1.map(|p| p.handle) == co_parent2.map(|p| p.handle) {
// Same parents. Ignore collisions.
return;
}
@@ -411,7 +466,7 @@ impl NarrowPhase {
ColliderGraphIndices::invalid(),
);
if co1.is_sensor() || co2.is_sensor() {
if co_type1.is_sensor() || co_type2.is_sensor() {
// NOTE: the collider won't have a graph index as long
// as it does not interact with anything.
if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index)
@@ -469,33 +524,56 @@ impl NarrowPhase {
}
}
pub(crate) fn register_pairs(
pub(crate) fn register_pairs<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
colliders: &Colliders,
bodies: &mut Bodies,
broad_phase_events: &[BroadPhasePairEvent],
events: &dyn EventHandler,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
for event in broad_phase_events {
match event {
BroadPhasePairEvent::AddPair(pair) => {
self.add_pair(colliders, pair);
}
BroadPhasePairEvent::DeletePair(pair) => {
self.remove_pair(colliders, bodies, pair, events, PairRemovalMode::Auto);
self.remove_pair(
islands,
colliders,
bodies,
pair,
events,
PairRemovalMode::Auto,
);
}
}
}
}
pub(crate) fn compute_intersections(
pub(crate) fn compute_intersections<Bodies, Colliders>(
&mut self,
bodies: &RigidBodySet,
colliders: &ColliderSet,
hooks: &dyn PhysicsHooks,
bodies: &Bodies,
colliders: &Colliders,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
events: &dyn EventHandler,
) {
if !colliders.contains_any_modified_collider() {
) where
Bodies: ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderGroups>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>,
{
if modified_colliders.is_empty() {
return;
}
@@ -507,35 +585,66 @@ impl NarrowPhase {
par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
let handle1 = nodes[edge.source().index()].weight;
let handle2 = nodes[edge.target().index()].weight;
let co1 = &colliders[handle1];
let co2 = &colliders[handle2];
if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
let (co_changes1, co_groups1, co_shape1, co_pos1): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
) = colliders.index_bundle(handle1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0);
let (co_changes2, co_groups2, co_shape2, co_pos2): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
) = colliders.index_bundle(handle2.0);
if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
{
// No update needed for these colliders.
return;
}
// TODO: avoid lookup into bodies.
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
let (mut sleeping1, mut status1) = (true, RigidBodyType::Static);
let (mut sleeping2, mut status2) = (true, RigidBodyType::Static);
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
if let Some(co_parent1) = co_parent1 {
let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent1.handle.0);
status1 = *rb_type1;
sleeping1 = rb_activation1.sleeping;
}
if let Some(co_parent2) = co_parent2 {
let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent2.handle.0);
status2 = *rb_type2;
sleeping2 = rb_activation2.sleeping;
}
if (sleeping1 && status2.is_static())
|| (sleeping2 && status1.is_static())
|| (sleeping1 && sleeping2)
{
// No need to update this intersection because nothing moved.
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
if !co_groups1
.collision_groups
.test(co_groups2.collision_groups)
{
// The intersection is not allowed.
return;
}
if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR)
&& !rb1.is_dynamic()
&& !rb2.is_dynamic()
&& !status1.is_dynamic()
&& !status2.is_dynamic()
{
// Default filtering rule: no intersection between two non-dynamic bodies.
return;
@@ -543,12 +652,12 @@ impl NarrowPhase {
if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: handle1,
collider_handle2: handle2,
collider1: co1,
collider2: co2,
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: handle1,
collider2: handle2,
};
if !hooks.filter_intersection_pair(&context) {
@@ -557,10 +666,10 @@ impl NarrowPhase {
}
}
let pos12 = co1.position().inv_mul(co2.position());
let pos12 = co_pos1.inv_mul(co_pos2);
if let Ok(intersection) =
query_dispatcher.intersection_test(&pos12, co1.shape(), co2.shape())
query_dispatcher.intersection_test(&pos12, &**co_shape1, &**co_shape2)
{
if intersection != edge.weight {
edge.weight = intersection;
@@ -574,15 +683,26 @@ impl NarrowPhase {
});
}
pub(crate) fn compute_contacts(
pub(crate) fn compute_contacts<Bodies, Colliders>(
&mut self,
prediction_distance: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
hooks: &dyn PhysicsHooks,
bodies: &Bodies,
colliders: &Colliders,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
events: &dyn EventHandler,
) {
if !colliders.contains_any_modified_collider() {
) where
Bodies: ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderGroups>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderMaterial>,
{
if modified_colliders.is_empty() {
return;
}
@@ -592,35 +712,68 @@ impl NarrowPhase {
// TODO: don't iterate on all the edges.
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
let pair = &mut edge.weight;
let co1 = &colliders[pair.pair.collider1];
let co2 = &colliders[pair.pair.collider2];
if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
let co_parent1: Option<&ColliderParent> = colliders.get(pair.pair.collider1.0);
let (co_changes1, co_groups1, co_shape1, co_pos1, co_material1): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
&ColliderMaterial,
) = colliders.index_bundle(pair.pair.collider1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(pair.pair.collider2.0);
let (co_changes2, co_groups2, co_shape2, co_pos2, co_material2): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
&ColliderMaterial,
) = colliders.index_bundle(pair.pair.collider2.0);
if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
{
// No update needed for these colliders.
return;
}
// TODO: avoid lookup into bodies.
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
let (mut sleeping1, mut status1) = (true, RigidBodyType::Static);
let (mut sleeping2, mut status2) = (true, RigidBodyType::Static);
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
if let Some(co_parent1) = co_parent1 {
let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent1.handle.0);
status1 = *rb_type1;
sleeping1 = rb_activation1.sleeping;
}
if let Some(co_parent2) = co_parent2 {
let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent2.handle.0);
status2 = *rb_type2;
sleeping2 = rb_activation2.sleeping;
}
if (sleeping1 && status2.is_static())
|| (sleeping2 && status1.is_static())
|| (sleeping1 && sleeping2)
{
// No need to update this contact because nothing moved.
// No need to update this intersection because nothing moved.
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
if !co_groups1
.collision_groups
.test(co_groups2.collision_groups)
{
// The collision is not allowed.
return;
}
if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
&& !rb1.is_dynamic()
&& !rb2.is_dynamic()
&& !status1.is_dynamic()
&& !status2.is_dynamic()
{
// Default filtering rule: no contact between two non-dynamic bodies.
return;
@@ -629,12 +782,12 @@ impl NarrowPhase {
let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
{
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: pair.pair.collider1,
collider_handle2: pair.pair.collider2,
collider1: co1,
collider2: co2,
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.pair.collider1,
collider2: pair.pair.collider2,
};
if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
@@ -644,25 +797,25 @@ impl NarrowPhase {
return;
}
} else {
co1.solver_flags | co2.solver_flags
co_material1.solver_flags | co_material2.solver_flags
};
if !co1.solver_groups.test(co2.solver_groups) {
if !co_groups1.solver_groups.test(co_groups2.solver_groups) {
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
}
if co1.changes.contains(ColliderChanges::SHAPE)
|| co2.changes.contains(ColliderChanges::SHAPE)
if co_changes1.contains(ColliderChanges::SHAPE)
|| co_changes2.contains(ColliderChanges::SHAPE)
{
// The shape changed so the workspace is no longer valid.
pair.workspace = None;
}
let pos12 = co1.position().inv_mul(co2.position());
let pos12 = co_pos1.inv_mul(co_pos2);
let _ = query_dispatcher.contact_manifolds(
&pos12,
co1.shape(),
co2.shape(),
&**co_shape1,
&**co_shape2,
prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
@@ -671,25 +824,34 @@ impl NarrowPhase {
let mut has_any_active_contact = false;
let friction = CoefficientCombineRule::combine(
co1.friction,
co2.friction,
co1.flags.friction_combine_rule_value(),
co2.flags.friction_combine_rule_value(),
co_material1.friction,
co_material2.friction,
co_material1.friction_combine_rule as u8,
co_material2.friction_combine_rule as u8,
);
let restitution = CoefficientCombineRule::combine(
co1.restitution,
co2.restitution,
co1.flags.restitution_combine_rule_value(),
co2.flags.restitution_combine_rule_value(),
co_material1.restitution,
co_material2.restitution,
co_material1.restitution_combine_rule as u8,
co_material2.restitution_combine_rule as u8,
);
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co_parent1
.map(|p1| *bodies.index(p1.handle.0))
.unwrap_or(zero);
let dominance2 = co_parent2
.map(|p2| *bodies.index(p2.handle.0))
.unwrap_or(zero);
for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(co1.position());
let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1);
manifold.data.solver_contacts.clear();
manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent());
manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
manifold.data.solver_flags = solver_flags;
manifold.data.relative_dominance =
rb1.effective_dominance_group() - rb2.effective_dominance_group();
dominance1.effective_group(&status1) - dominance2.effective_group(&status2);
manifold.data.normal = world_pos1 * manifold.local_n1;
// Generate solver contacts.
@@ -732,12 +894,12 @@ impl NarrowPhase {
let mut modifiable_normal = manifold.data.normal;
let mut context = ContactModificationContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: pair.pair.collider1,
collider_handle2: pair.pair.collider2,
collider1: co1,
collider2: co2,
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.pair.collider1,
collider2: pair.pair.collider2,
manifold,
solver_contacts: &mut modifiable_solver_contacts,
normal: &mut modifiable_normal,
@@ -772,38 +934,61 @@ impl NarrowPhase {
/// Retrieve all the interactions with at least one contact point, happening between two active bodies.
// NOTE: this is very similar to the code from JointSet::select_active_interactions.
pub(crate) fn select_active_contacts<'a>(
pub(crate) fn select_active_contacts<'a, Bodies>(
&'a mut self,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
out_manifolds: &mut Vec<&'a mut ContactManifold>,
out: &mut Vec<Vec<ContactManifoldIndex>>,
) {
for out_island in &mut out[..bodies.num_islands()] {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyActivation>,
{
for out_island in &mut out[..islands.num_islands()] {
out_island.clear();
}
// TODO: don't iterate through all the interactions.
for inter in self.contact_graph.graph.edges.iter_mut() {
for manifold in &mut inter.weight.manifolds {
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if manifold
.data
.solver_flags
.contains(SolverFlags::COMPUTE_IMPULSES)
&& manifold.data.num_active_contacts() != 0
&& (rb1.is_dynamic() || rb2.is_dynamic())
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
{
let island_index = if !rb1.is_dynamic() {
rb2.active_island_id
} else {
rb1.active_island_id
};
let (active_island_id1, status1, sleeping1) =
if let Some(handle1) = manifold.data.rigid_body1 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(handle1.0);
(data.0.active_island_id, *data.1, data.2.sleeping)
} else {
(0, RigidBodyType::Static, true)
};
out[island_index].push(out_manifolds.len());
out_manifolds.push(manifold);
let (active_island_id2, status2, sleeping2) =
if let Some(handle2) = manifold.data.rigid_body2 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(handle2.0);
(data.0.active_island_id, *data.1, data.2.sleeping)
} else {
(0, RigidBodyType::Static, true)
};
if (status1.is_dynamic() || status2.is_dynamic())
&& (!status1.is_dynamic() || !sleeping1)
&& (!status2.is_dynamic() || !sleeping2)
{
let island_index = if !status1.is_dynamic() {
active_island_id2
} else {
active_island_id1
};
out[island_index].push(out_manifolds.len());
out_manifolds.push(manifold);
}
}
}
}