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

Merge branch 'master' into collider-builder-debug

Browse Source
This commit is contained in:
Thierry Berger
2024-06-03 15:20:24 +02:00
parent fe336b9b98 856675032e
commit e1ed90603e
116 changed files with 5370 additions and 1613 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
src/geometry/broad_phase.rs Normal file
View File

@@ -0,0 +1,50 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderSet};
use parry::math::Real;
/// An internal index stored in colliders by some broad-phase algorithms.
pub type BroadPhaseProxyIndex = u32;
/// Trait implemented by broad-phase algorithms supported by Rapier.
///
/// The task of a broad-phase algorithm is to detect potential collision pairs, usually based on
/// bounding volumes. The pairs must be conservative: it is OK to create a collision pair if
/// two objects don’t actually touch, but it is incorrect to remove a pair between two objects
/// that are still touching. In other words, it can have false-positive (though these induce
/// some computational overhead on the narrow-phase), but cannot have false-negative.
pub trait BroadPhase: Send + Sync + 'static {
/// Updates the broad-phase.
///
/// The results must be output through the `events` struct. The broad-phase algorithm is only
/// required to generate new events (i.e. no need to re-send an `AddPair` event if it was already
/// sent previously and no `RemovePair` happened since then). Sending redundant events is allowed
/// but can result in a slight computational overhead.
///
/// The `colliders` set is mutable only to provide access to
/// [`collider.set_internal_broad_phase_proxy_index`]. Other properties of the collider should
/// **not** be modified during the broad-phase update.
///
/// # Parameters
/// - `prediction_distance`: colliders that are not exactly touching, but closer to this
/// distance must form a collision pair.
/// - `colliders`: the set of colliders. Change detection with `collider.needs_broad_phase_update()`
/// can be relied on at this stage.
/// - `modified_colliders`: colliders that are know to be modified since the last update.
/// - `removed_colliders`: colliders that got removed since the last update. Any associated data
/// in the broad-phase should be removed by this call to `update`.
/// - `events`: the broad-phase’s output. They indicate what collision pairs need to be created
/// and what pairs need to be removed. It is OK to create pairs for colliders that don’t
/// actually collide (though this can increase computational overhead in the narrow-phase)
/// but it is important not to indicate removal of a collision pair if the underlying colliders
/// are still touching or closer than `prediction_distance`.
fn update(
&mut self,
dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
);
}

182
src/geometry/broad_phase_multi_sap/broad_phase.rs → src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
View File

@@ -1,12 +1,12 @@
use super::{
BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
};
use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
use crate::geometry::{
ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet,
ColliderShape,
BroadPhaseProxyIndex, Collider, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
ColliderSet,
};
use crate::math::Real;
use crate::math::{Isometry, Real};
use crate::prelude::{BroadPhase, RigidBodySet};
use crate::utils::IndexMut2;
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::HashMap;
@@ -74,7 +74,7 @@ use parry::utils::hashmap::HashMap;
/// broad-phase, as well as the Aabbs of all the regions part of this broad-phase.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhase {
pub struct BroadPhaseMultiSap {
proxies: SAPProxies,
layers: Vec<SAPLayer>,
smallest_layer: u8,
@@ -90,7 +90,7 @@ pub struct BroadPhase {
// 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>,
colliders_proxy_ids: HashMap<ColliderHandle, BroadPhaseProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
region_pool: SAPRegionPool, // To avoid repeated allocations.
// We could think serializing this workspace is useless.
@@ -114,16 +114,16 @@ pub struct BroadPhase {
reporting: HashMap<(u32, u32), bool>, // Workspace
}
impl Default for BroadPhase {
impl Default for BroadPhaseMultiSap {
fn default() -> Self {
Self::new()
}
}
impl BroadPhase {
impl BroadPhaseMultiSap {
/// Create a new empty broad-phase.
pub fn new() -> Self {
BroadPhase {
BroadPhaseMultiSap {
proxies: SAPProxies::new(),
layers: Vec::new(),
smallest_layer: 0,
@@ -138,7 +138,7 @@ 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`.
/// of the `BroadPhaseMultiSap::update`.
fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
// For each removed collider, remove the corresponding proxy.
for removed in removed_colliders {
@@ -156,7 +156,7 @@ impl BroadPhase {
/// remove, the `complete_removal` method MUST be called to
/// complete the removal of these proxies, by actually removing them
/// from all the relevant layers/regions/axes.
fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) {
fn predelete_proxy(&mut self, proxy_index: BroadPhaseProxyIndex) {
if proxy_index == crate::INVALID_U32 {
// This collider has not been added to the broad-phase yet.
return;
@@ -353,13 +353,18 @@ impl BroadPhase {
prediction_distance: Real,
handle: ColliderHandle,
proxy_index: &mut u32,
collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
collider: &Collider,
next_position: Option<&Isometry<Real>>,
) -> bool {
let (co_pos, co_shape, co_changes) = collider;
let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
let mut aabb = co_shape
.compute_aabb(co_pos)
.loosened(prediction_distance / 2.0);
if let Some(next_position) = next_position {
let next_aabb = collider
.shape
.compute_aabb(next_position)
.loosened(collider.contact_skin() + prediction_distance / 2.0);
aabb.merge(&next_aabb);
}
if aabb.mins.coords.iter().any(|e| !e.is_finite())
|| aabb.maxs.coords.iter().any(|e| !e.is_finite())
@@ -378,7 +383,7 @@ impl BroadPhase {
prev_aabb = proxy.aabb;
proxy.aabb = aabb;
if co_changes.contains(ColliderChanges::SHAPE) {
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
@@ -449,65 +454,6 @@ impl BroadPhase {
!layer.created_regions.is_empty()
}
/// Updates the broad-phase, taking into account the new collider positions.
pub fn update(
&mut self,
prediction_distance: Real,
colliders: &mut ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
if let Some(co) = colliders.get_mut_internal(*handle) {
if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
continue;
}
let mut new_proxy_id = co.bf_data.proxy_index;
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
(&co.pos, &co.shape, &co.changes),
) {
need_region_propagation = true;
}
if co.bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Make sure we have the new proxy index in case
// the collider was added for the first time.
co.bf_data = 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 {
self.propagate_created_regions();
}
// Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
self.update_layers_and_find_pairs(events);
// 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, removed_colliders);
}
/// Propagate regions from the smallest layers up to the larger layers.
///
/// Whenever a region is created on a layer `n`, then its Aabb must be
@@ -618,16 +564,90 @@ impl BroadPhase {
}
}
impl BroadPhase for BroadPhaseMultiSap {
/// Updates the broad-phase, taking into account the new collider positions.
fn update(
&mut self,
dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
if let Some(co) = colliders.get_mut_internal(*handle) {
if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
continue;
}
let mut new_proxy_id = co.bf_data.proxy_index;
let next_pos = co.parent.and_then(|p| {
let parent = bodies.get(p.handle)?;
(parent.soft_ccd_prediction() > 0.0).then(|| {
parent.predict_position_using_velocity_and_forces_with_max_dist(
dt,
parent.soft_ccd_prediction(),
) * p.pos_wrt_parent
})
});
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
co,
next_pos.as_ref(),
) {
need_region_propagation = true;
}
if co.bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Make sure we have the new proxy index in case
// the collider was added for the first time.
co.bf_data = 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 {
self.propagate_created_regions();
}
// Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
self.update_layers_and_find_pairs(events);
// 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, removed_colliders);
}
}
#[cfg(test)]
mod test {
use crate::dynamics::{
ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyBuilder, RigidBodySet,
};
use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
use crate::geometry::{BroadPhase, BroadPhaseMultiSap, ColliderBuilder, ColliderSet};
#[test]
fn test_add_update_remove() {
let mut broad_phase = BroadPhase::new();
let mut broad_phase = BroadPhaseMultiSap::new();
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
@@ -640,7 +660,7 @@ mod test {
let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
let mut events = Vec::new();
broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
bodies.remove(
hrb,
@@ -650,7 +670,7 @@ mod test {
&mut multibody_joints,
true,
);
broad_phase.update(0.0, &mut colliders, &[], &[coh], &mut events);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[], &[coh], &mut events);
// Create another body.
let rb = RigidBodyBuilder::dynamic().build();
@@ -659,6 +679,6 @@ mod test {
let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
// Make sure the proxy handles is recycled properly.
broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
}
}

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

@@ -1,6 +1,5 @@
pub use self::broad_phase::BroadPhase;
pub use self::broad_phase_multi_sap::BroadPhaseMultiSap;
pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair};
pub use self::sap_proxy::SAPProxyIndex;
use self::sap_axis::*;
use self::sap_endpoint::*;
@@ -9,7 +8,7 @@ use self::sap_proxy::*;
use self::sap_region::*;
use self::sap_utils::*;
mod broad_phase;
mod broad_phase_multi_sap;
mod broad_phase_pair_event;
mod sap_axis;
mod sap_endpoint;

4
src/geometry/broad_phase_multi_sap/sap_axis.rs
View File

@@ -1,6 +1,6 @@
use super::{SAPEndpoint, SAPProxies, NUM_SENTINELS};
use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
use crate::geometry::SAPProxyIndex;
use crate::geometry::BroadPhaseProxyIndex;
use crate::math::Real;
use bit_vec::BitVec;
use parry::bounding_volume::BoundingVolume;
@@ -39,7 +39,7 @@ impl SAPAxis {
pub fn batch_insert(
&mut self,
dim: usize,
new_proxies: &[SAPProxyIndex],
new_proxies: &[BroadPhaseProxyIndex],
proxies: &SAPProxies,
reporting: Option<&mut HashMap<(u32, u32), bool>>,
) {

20
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::{Aabb, SAPProxyIndex};
use crate::geometry::{Aabb, BroadPhaseProxyIndex};
use crate::math::{Point, Real};
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::{Entry, HashMap};
@@ -13,11 +13,11 @@ pub(crate) struct SAPLayer {
pub smaller_layer: Option<u8>,
pub larger_layer: Option<u8>,
region_width: Real,
pub regions: HashMap<Point<i32>, SAPProxyIndex>,
pub regions: HashMap<Point<i32>, BroadPhaseProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
regions_to_potentially_remove: Vec<Point<i32>>, // Workspace
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub created_regions: Vec<SAPProxyIndex>,
pub created_regions: Vec<BroadPhaseProxyIndex>,
}
impl SAPLayer {
@@ -71,7 +71,7 @@ impl SAPLayer {
///
/// This method must be called in a bottom-up loop, propagating new regions from the
/// smallest layer, up to the largest layer. That loop is done by the Phase 3 of the
/// BroadPhase::update.
/// BroadPhaseMultiSap::update.
pub fn propagate_created_regions(
&mut self,
larger_layer: &mut Self,
@@ -103,7 +103,7 @@ impl SAPLayer {
/// one region on its parent "larger" layer.
fn register_subregion(
&mut self,
proxy_id: SAPProxyIndex,
proxy_id: BroadPhaseProxyIndex,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
@@ -140,7 +140,7 @@ impl SAPLayer {
fn unregister_subregion(
&mut self,
proxy_id: SAPProxyIndex,
proxy_id: BroadPhaseProxyIndex,
proxy_region: &SAPRegion,
proxies: &mut SAPProxies,
) {
@@ -182,7 +182,7 @@ impl SAPLayer {
/// If the region with the given region key does not exist yet, it is created.
/// When a region is created, it creates a new proxy for that region, and its
/// proxy ID is added to `self.created_region` so it can be propagated during
/// the Phase 3 of `BroadPhase::update`.
/// the Phase 3 of `BroadPhaseMultiSap::update`.
///
/// This returns the proxy ID of the already existing region if it existed, or
/// of the new region if it did not exist and has been created by this method.
@@ -191,7 +191,7 @@ impl SAPLayer {
region_key: Point<i32>,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) -> SAPProxyIndex {
) -> BroadPhaseProxyIndex {
match self.regions.entry(region_key) {
// Yay, the region already exists!
Entry::Occupied(occupied) => *occupied.get(),
@@ -266,7 +266,7 @@ impl SAPLayer {
}
}
pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: SAPProxyIndex) {
pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: BroadPhaseProxyIndex) {
// Discretize the Aabb to find the regions that need to be invalidated.
let proxy_aabb = &mut proxies[proxy_index].aabb;
let start = super::point_key(proxy_aabb.mins, self.region_width);
@@ -379,7 +379,7 @@ impl SAPLayer {
pub fn proper_proxy_moved_to_bigger_layer(
&mut self,
proxies: &mut SAPProxies,
proxy_id: SAPProxyIndex,
proxy_id: BroadPhaseProxyIndex,
) {
for (point, region_id) in &self.regions {
let region = &mut proxies[*region_id].data.as_region_mut();

24
src/geometry/broad_phase_multi_sap/sap_proxy.rs
View File

@@ -1,11 +1,9 @@
use super::NEXT_FREE_SENTINEL;
use crate::geometry::broad_phase_multi_sap::SAPRegion;
use crate::geometry::ColliderHandle;
use crate::geometry::{BroadPhaseProxyIndex, ColliderHandle};
use parry::bounding_volume::Aabb;
use std::ops::{Index, IndexMut};
pub type SAPProxyIndex = u32;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub enum SAPProxyData {
@@ -49,7 +47,7 @@ impl SAPProxyData {
pub struct SAPProxy {
pub data: SAPProxyData,
pub aabb: Aabb,
pub next_free: SAPProxyIndex,
pub next_free: BroadPhaseProxyIndex,
// TODO: pack the layer_id and layer_depth into a single u16?
pub layer_id: u8,
pub layer_depth: i8,
@@ -81,7 +79,7 @@ impl SAPProxy {
#[derive(Clone)]
pub struct SAPProxies {
pub elements: Vec<SAPProxy>,
pub first_free: SAPProxyIndex,
pub first_free: BroadPhaseProxyIndex,
}
impl Default for SAPProxies {
@@ -98,7 +96,7 @@ impl SAPProxies {
}
}
pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex {
pub fn insert(&mut self, proxy: SAPProxy) -> BroadPhaseProxyIndex {
if self.first_free != NEXT_FREE_SENTINEL {
let proxy_id = self.first_free;
self.first_free = self.elements[proxy_id as usize].next_free;
@@ -110,31 +108,31 @@ impl SAPProxies {
}
}
pub fn remove(&mut self, proxy_id: SAPProxyIndex) {
pub fn remove(&mut self, proxy_id: BroadPhaseProxyIndex) {
let proxy = &mut self.elements[proxy_id as usize];
proxy.next_free = self.first_free;
self.first_free = proxy_id;
}
// NOTE: this must not take holes into account.
pub fn get_mut(&mut self, i: SAPProxyIndex) -> Option<&mut SAPProxy> {
pub fn get_mut(&mut self, i: BroadPhaseProxyIndex) -> Option<&mut SAPProxy> {
self.elements.get_mut(i as usize)
}
// NOTE: this must not take holes into account.
pub fn get(&self, i: SAPProxyIndex) -> Option<&SAPProxy> {
pub fn get(&self, i: BroadPhaseProxyIndex) -> Option<&SAPProxy> {
self.elements.get(i as usize)
}
}
impl Index<SAPProxyIndex> for SAPProxies {
impl Index<BroadPhaseProxyIndex> for SAPProxies {
type Output = SAPProxy;
fn index(&self, i: SAPProxyIndex) -> &SAPProxy {
fn index(&self, i: BroadPhaseProxyIndex) -> &SAPProxy {
self.elements.index(i as usize)
}
}
impl IndexMut<SAPProxyIndex> for SAPProxies {
fn index_mut(&mut self, i: SAPProxyIndex) -> &mut SAPProxy {
impl IndexMut<BroadPhaseProxyIndex> for SAPProxies {
fn index_mut(&mut self, i: BroadPhaseProxyIndex) -> &mut SAPProxy {
self.elements.index_mut(i as usize)
}
}

18
src/geometry/broad_phase_multi_sap/sap_region.rs
View File

@@ -1,5 +1,5 @@
use super::{SAPAxis, SAPProxies};
use crate::geometry::SAPProxyIndex;
use crate::geometry::BroadPhaseProxyIndex;
use crate::math::DIM;
use bit_vec::BitVec;
use parry::bounding_volume::Aabb;
@@ -13,8 +13,8 @@ pub struct SAPRegion {
pub axes: [SAPAxis; DIM],
pub existing_proxies: BitVec,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub to_insert: Vec<SAPProxyIndex>, // Workspace
pub subregions: Vec<SAPProxyIndex>,
pub to_insert: Vec<BroadPhaseProxyIndex>, // Workspace
pub subregions: Vec<BroadPhaseProxyIndex>,
pub id_in_parent_subregion: u32,
pub update_count: u8,
pub needs_update_after_subregion_removal: bool,
@@ -90,7 +90,7 @@ impl SAPRegion {
/// 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 {
pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: BroadPhaseProxyIndex) -> bool {
if self.existing_proxies.get(proxy_id as usize) == Some(true) {
// NOTE: we are just registering the fact that that proxy isn't a
// subproper proxy anymore. But it is still part of this region
@@ -142,7 +142,7 @@ impl SAPRegion {
self.subproper_proxy_count -= num_deleted_subregion_endpoints[0] / 2;
}
pub fn predelete_proxy(&mut self, _proxy_id: SAPProxyIndex) {
pub fn predelete_proxy(&mut self, _proxy_id: BroadPhaseProxyIndex) {
// We keep the proxy_id as argument for uniformity with the "preupdate"
// method. However we don't actually need it because the deletion will be
// handled transparently during the next update.
@@ -153,14 +153,18 @@ impl SAPRegion {
self.update_count = self.update_count.max(1);
}
pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize {
pub fn register_subregion(&mut self, proxy_id: BroadPhaseProxyIndex) -> usize {
let subregion_index = self.subregions.len();
self.subregions.push(proxy_id);
self.preupdate_proxy(proxy_id, true);
subregion_index
}
pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool {
pub fn preupdate_proxy(
&mut self,
proxy_id: BroadPhaseProxyIndex,
is_subproper_proxy: bool,
) -> bool {
let mask_len = self.existing_proxies.len();
if proxy_id as usize >= mask_len {
self.existing_proxies

13
src/geometry/broad_phase_qbvh.rs
View File

@@ -1,5 +1,4 @@
use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
use parry::bounding_volume::BoundingVolume;
use parry::math::Real;
use parry::partitioning::Qbvh;
use parry::partitioning::QbvhUpdateWorkspace;
@@ -7,20 +6,20 @@ use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhase {
pub struct BroadPhaseQbvh {
qbvh: Qbvh<ColliderHandle>,
stack: Vec<(u32, u32)>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
workspace: QbvhUpdateWorkspace,
}
impl Default for BroadPhase {
impl Default for BroadPhaseQbvh {
fn default() -> Self {
Self::new()
}
}
impl BroadPhase {
impl BroadPhaseQbvh {
pub fn new() -> Self {
Self {
qbvh: Qbvh::new(),
@@ -59,7 +58,7 @@ impl BroadPhase {
colliders.iter().map(|(handle, collider)| {
(
handle,
collider.compute_aabb().loosened(prediction_distance / 2.0),
collider.compute_collision_aabb(prediction_distance / 2.0),
)
}),
margin,
@@ -76,9 +75,7 @@ impl BroadPhase {
}
let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
colliders[*handle]
.compute_aabb()
.loosened(prediction_distance / 2.0)
colliders[*handle].compute_collision_aabb(prediction_distance / 2.0)
});
self.qbvh
.traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);

143
src/geometry/collider.rs
View File

@@ -1,17 +1,20 @@
use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
use crate::geometry::{
ActiveCollisionTypes, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition, ColliderShape,
ColliderType, InteractionGroups, SharedShape,
ActiveCollisionTypes, BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges,
ColliderFlags, ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition,
ColliderShape, ColliderType, InteractionGroups, SharedShape,
};
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
use crate::parry::transformation::vhacd::VHACDParameters;
use crate::pipeline::{ActiveEvents, ActiveHooks};
use crate::prelude::ColliderEnabled;
use na::Unit;
use parry::bounding_volume::Aabb;
use parry::bounding_volume::{Aabb, BoundingVolume};
use parry::shape::{Shape, TriMeshFlags};
#[cfg(feature = "dim3")]
use crate::geometry::HeightFieldFlags;
#[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.
@@ -27,6 +30,7 @@ pub struct Collider {
pub(crate) material: ColliderMaterial,
pub(crate) flags: ColliderFlags,
pub(crate) bf_data: ColliderBroadPhaseData,
contact_skin: Real,
contact_force_event_threshold: Real,
/// User-defined data associated to this collider.
pub user_data: u128,
@@ -50,6 +54,21 @@ impl Collider {
}
}
/// An internal index associated to this collider by the broad-phase algorithm.
pub fn internal_broad_phase_proxy_index(&self) -> BroadPhaseProxyIndex {
self.bf_data.proxy_index
}
/// Sets the internal index associated to this collider by the broad-phase algorithm.
///
/// This must **not** be called, unless you are implementing your own custom broad-phase
/// that require storing an index in the collider struct.
/// Modifying that index outside of a custom broad-phase code will most certainly break
/// the physics engine.
pub fn set_internal_broad_phase_proxy_index(&mut self, id: BroadPhaseProxyIndex) {
self.bf_data.proxy_index = id;
}
/// The rigid body this collider is attached to.
pub fn parent(&self) -> Option<RigidBodyHandle> {
self.parent.map(|parent| parent.handle)
@@ -60,6 +79,55 @@ impl Collider {
self.coll_type.is_sensor()
}
/// Copy all the characteristics from `other` to `self`.
///
/// If you have a mutable reference to a collider `collider: &mut Collider`, attempting to
/// assign it a whole new collider instance, e.g., `*collider = ColliderBuilder::ball(0.5).build()`,
/// will crash due to some internal indices being overwritten. Instead, use
/// `collider.copy_from(&ColliderBuilder::ball(0.5).build())`.
///
/// This method will allow you to set most characteristics of this collider from another
/// collider instance without causing any breakage.
///
/// This method **cannot** be used for reparenting a collider. Therefore, the parent of the
/// `other` (if any), as well as its relative position to that parent will not be copied into
/// `self`.
///
/// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has
/// a parent, its position is directly controlled by the parent rigid-body).
pub fn copy_from(&mut self, other: &Collider) {
// NOTE: we deconstruct the collider struct to be sure we don’t forget to
// add some copies here if we add more field to Collider in the future.
let Collider {
coll_type,
shape,
mprops,
changes: _changes, // Will be set to ALL.
parent: _parent, // This function cannot be used to reparent the collider.
pos,
material,
flags,
bf_data: _bf_data, // Internal ids must not be overwritten.
contact_force_event_threshold,
user_data,
contact_skin,
} = other;
if self.parent.is_none() {
self.pos = *pos;
}
self.coll_type = *coll_type;
self.shape = shape.clone();
self.mprops = mprops.clone();
self.material = *material;
self.contact_force_event_threshold = *contact_force_event_threshold;
self.user_data = *user_data;
self.flags = *flags;
self.changes = ColliderChanges::all();
self.contact_skin = *contact_skin;
}
/// The physics hooks enabled for this collider.
pub fn active_hooks(&self) -> ActiveHooks {
self.flags.active_hooks
@@ -90,6 +158,20 @@ impl Collider {
self.flags.active_collision_types = active_collision_types;
}
/// The contact skin of this collider.
///
/// See the documentation of [`ColliderBuilder::contact_skin`] for details.
pub fn contact_skin(&self) -> Real {
self.contact_skin
}
/// Sets the contact skin of this collider.
///
/// See the documentation of [`ColliderBuilder::contact_skin`] for details.
pub fn set_contact_skin(&mut self, skin_thickness: Real) {
self.contact_skin = skin_thickness;
}
/// The friction coefficient of this collider.
pub fn friction(&self) -> Real {
self.material.friction
@@ -224,7 +306,7 @@ impl Collider {
}
}
/// Sets the rotational part of this collider's rotaiton relative to its parent rigid-body.
/// Sets the rotational part of this collider's rotation relative to its parent rigid-body.
pub fn set_rotation_wrt_parent(&mut self, rotation: AngVector<Real>) {
if let Some(parent) = self.parent.as_mut() {
self.changes.insert(ColliderChanges::PARENT);
@@ -372,10 +454,21 @@ impl Collider {
}
/// Compute the axis-aligned bounding box of this collider.
///
/// This AABB doesn’t take into account the collider’s contact skin.
/// [`Collider::contact_skin`].
pub fn compute_aabb(&self) -> Aabb {
self.shape.compute_aabb(&self.pos)
}
/// Compute the axis-aligned bounding box of this collider, taking into account the
/// [`Collider::contact_skin`] and prediction distance.
pub fn compute_collision_aabb(&self, prediction: Real) -> Aabb {
self.shape
.compute_aabb(&self.pos)
.loosened(self.contact_skin + prediction)
}
/// Compute the axis-aligned bounding box of this collider moving from its current position
/// to the given `next_position`
pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> Aabb {
@@ -430,6 +523,8 @@ pub struct ColliderBuilder {
pub enabled: bool,
/// The total force magnitude beyond which a contact force event can be emitted.
pub contact_force_event_threshold: Real,
/// An extra thickness around the collider shape to keep them further apart when colliding.
pub contact_skin: Real,
}
impl ColliderBuilder {
@@ -452,6 +547,7 @@ impl ColliderBuilder {
active_events: ActiveEvents::empty(),
enabled: true,
contact_force_event_threshold: 0.0,
contact_skin: 0.0,
}
}
@@ -518,6 +614,15 @@ impl ColliderBuilder {
Self::new(SharedShape::round_cuboid(hx, hy, border_radius))
}
/// Initialize a new collider builder with a capsule defined from its endpoints.
///
/// See also [`ColliderBuilder::capsule_x`], [`ColliderBuilder::capsule_y`], and
/// [`ColliderBuilder::capsule_z`], for a simpler way to build capsules with common
/// orientations.
pub fn capsule_from_endpoints(a: Point<Real>, b: Point<Real>, radius: Real) -> Self {
Self::new(SharedShape::capsule(a, b, radius))
}
/// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
pub fn capsule_x(half_height: Real, radius: Real) -> Self {
Self::new(SharedShape::capsule_x(half_height, radius))
@@ -698,6 +803,17 @@ impl ColliderBuilder {
Self::new(SharedShape::heightfield(heights, scale))
}
/// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
/// factor along each coordinate axis.
#[cfg(feature = "dim3")]
pub fn heightfield_with_flags(
heights: na::DMatrix<Real>,
scale: Vector<Real>,
flags: HeightFieldFlags,
) -> Self {
Self::new(SharedShape::heightfield_with_flags(heights, scale, flags))
}
/// The default friction coefficient used by the collider builder.
pub fn default_friction() -> Real {
0.5
@@ -705,7 +821,7 @@ impl ColliderBuilder {
/// The default density used by the collider builder.
pub fn default_density() -> Real {
1.0
100.0
}
/// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
@@ -861,6 +977,20 @@ impl ColliderBuilder {
self
}
/// Sets the contact skin of the collider.
///
/// The contact skin acts as if the collider was enlarged with a skin of width `skin_thickness`
/// around it, keeping objects further apart when colliding.
///
/// A non-zero contact skin can increase performance, and in some cases, stability. However
/// it creates a small gap between colliding object (equal to the sum of their skin). If the
/// skin is sufficiently small, this might not be visually significant or can be hidden by the
/// rendering assets.
pub fn contact_skin(mut self, skin_thickness: Real) -> Self {
self.contact_skin = skin_thickness;
self
}
/// Enable or disable the collider after its creation.
pub fn enabled(mut self, enabled: bool) -> Self {
self.enabled = enabled;
@@ -908,6 +1038,7 @@ impl ColliderBuilder {
flags,
coll_type,
contact_force_event_threshold: self.contact_force_event_threshold,
contact_skin: self.contact_skin,
user_data: self.user_data,
}
}

4
src/geometry/collider_components.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle, RigidBodyType};
use crate::geometry::{InteractionGroups, SAPProxyIndex, Shape, SharedShape};
use crate::geometry::{BroadPhaseProxyIndex, InteractionGroups, Shape, SharedShape};
use crate::math::{Isometry, Real};
use crate::parry::partitioning::IndexedData;
use crate::pipeline::{ActiveEvents, ActiveHooks};
@@ -118,7 +118,7 @@ impl ColliderType {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Data associated to a collider that takes part to a broad-phase algorithm.
pub struct ColliderBroadPhaseData {
pub(crate) proxy_index: SAPProxyIndex,
pub(crate) proxy_index: BroadPhaseProxyIndex,
}
impl Default for ColliderBroadPhaseData {

44
src/geometry/contact_pair.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold};
use crate::math::{Point, Real, Vector};
use crate::math::{Point, Real, TangentImpulse, Vector};
use crate::pipeline::EventHandler;
use crate::prelude::CollisionEventFlags;
use parry::query::ContactManifoldsWorkspace;
@@ -33,12 +33,11 @@ pub struct ContactData {
pub impulse: Real,
/// The friction impulse along the vector orthonormal to the contact normal, applied to the first
/// collider's rigid-body.
#[cfg(feature = "dim2")]
pub tangent_impulse: Real,
/// The friction impulses along the basis orthonormal to the contact normal, applied to the first
/// collider's rigid-body.
#[cfg(feature = "dim3")]
pub tangent_impulse: na::Vector2<Real>,
pub tangent_impulse: TangentImpulse<Real>,
/// The impulse retained for warmstarting the next simulation step.
pub warmstart_impulse: Real,
/// The friction impulse retained for warmstarting the next simulation step.
pub warmstart_tangent_impulse: TangentImpulse<Real>,
}
impl Default for ContactData {
@@ -46,6 +45,8 @@ impl Default for ContactData {
Self {
impulse: 0.0,
tangent_impulse: na::zero(),
warmstart_impulse: 0.0,
warmstart_tangent_impulse: na::zero(),
}
}
}
@@ -57,14 +58,14 @@ pub struct IntersectionPair {
/// Are the colliders intersecting?
pub intersecting: bool,
/// Was a `CollisionEvent::Started` emitted for this collider?
pub(crate) start_event_emited: bool,
pub(crate) start_event_emitted: bool,
}
impl IntersectionPair {
pub(crate) fn new() -> Self {
Self {
intersecting: false,
start_event_emited: false,
start_event_emitted: false,
}
}
@@ -76,7 +77,7 @@ impl IntersectionPair {
collider2: ColliderHandle,
events: &dyn EventHandler,
) {
self.start_event_emited = true;
self.start_event_emitted = true;
events.handle_collision_event(
bodies,
colliders,
@@ -93,7 +94,7 @@ impl IntersectionPair {
collider2: ColliderHandle,
events: &dyn EventHandler,
) {
self.start_event_emited = false;
self.start_event_emitted = false;
events.handle_collision_event(
bodies,
colliders,
@@ -114,11 +115,14 @@ pub struct ContactPair {
/// The set of contact manifolds between the two colliders.
///
/// All contact manifold contain themselves contact points between the colliders.
/// Note that contact points in the contact manifold do not take into account the
/// [`Collider::contact_skin`] which only affects the constraint solver and the
/// [`SolverContact`].
pub manifolds: Vec<ContactManifold>,
/// Is there any active contact in this contact pair?
pub has_any_active_contact: bool,
/// Was a `CollisionEvent::Started` emitted for this collider?
pub(crate) start_event_emited: bool,
pub(crate) start_event_emitted: bool,
pub(crate) workspace: Option<ContactManifoldsWorkspace>,
}
@@ -129,7 +133,7 @@ impl ContactPair {
collider2,
has_any_active_contact: false,
manifolds: Vec::new(),
start_event_emited: false,
start_event_emitted: false,
workspace: None,
}
}
@@ -206,7 +210,7 @@ impl ContactPair {
colliders: &ColliderSet,
events: &dyn EventHandler,
) {
self.start_event_emited = true;
self.start_event_emitted = true;
events.handle_collision_event(
bodies,
@@ -222,7 +226,7 @@ impl ContactPair {
colliders: &ColliderSet,
events: &dyn EventHandler,
) {
self.start_event_emited = false;
self.start_event_emitted = false;
events.handle_collision_event(
bodies,
@@ -299,6 +303,10 @@ pub struct SolverContact {
pub tangent_velocity: Vector<Real>,
/// Whether or not this contact existed during the last timestep.
pub is_new: bool,
/// Impulse used to warmstart the solve for the normal constraint.
pub warmstart_impulse: Real,
/// Impulse used to warmstart the solve for the friction constraints.
pub warmstart_tangent_impulse: TangentImpulse<Real>,
}
impl SolverContact {
@@ -351,16 +359,10 @@ impl ContactManifoldData {
pub trait ContactManifoldExt {
/// Computes the sum of all the impulses applied by contacts from this contact manifold.
fn total_impulse(&self) -> Real;
/// Computes the maximum impulse applied by contacts from this contact manifold.
fn max_impulse(&self) -> Real;
}
impl ContactManifoldExt for ContactManifold {
fn total_impulse(&self) -> Real {
self.points.iter().map(|pt| pt.data.impulse).sum()
}
fn max_impulse(&self) -> Real {
self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse))
}
}

17
src/geometry/mod.rs
View File

@@ -1,9 +1,7 @@
//! Structures related to geometry: colliders, shapes, etc.
pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
pub use self::broad_phase_multi_sap::BroadPhase;
// pub use self::broad_phase_qbvh::BroadPhase;
pub use self::broad_phase::BroadPhase;
pub use self::broad_phase_multi_sap::{BroadPhaseMultiSap, BroadPhasePairEvent, ColliderPair};
pub use self::collider_components::*;
pub use self::contact_pair::{
ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -51,10 +49,12 @@ pub type Aabb = parry::bounding_volume::Aabb;
pub type Ray = parry::query::Ray;
/// The intersection between a ray and a collider.
pub type RayIntersection = parry::query::RayIntersection;
/// The the projection of a point on a collider.
/// The projection of a point on a collider.
pub type PointProjection = parry::query::PointProjection;
/// The the time of impact between two shapes.
pub type TOI = parry::query::TOI;
/// The result of a shape-cast between two shapes.
pub type ShapeCastHit = parry::query::ShapeCastHit;
/// The default broad-phase implementation recommended for general-purpose usage.
pub type DefaultBroadPhase = BroadPhaseMultiSap;
bitflags::bitflags! {
/// Flags providing more information regarding a collision event.
@@ -180,7 +180,7 @@ impl ContactForceEvent {
}
}
pub(crate) use self::broad_phase_multi_sap::SAPProxyIndex;
pub(crate) use self::broad_phase::BroadPhaseProxyIndex;
pub(crate) use self::narrow_phase::ContactManifoldIndex;
pub(crate) use parry::partitioning::Qbvh;
pub use parry::shape::*;
@@ -203,6 +203,7 @@ mod interaction_graph;
mod interaction_groups;
mod narrow_phase;
mod broad_phase;
mod broad_phase_qbvh;
mod collider;
mod collider_set;

124
src/geometry/narrow_phase.rs
View File

@@ -8,9 +8,10 @@ use crate::dynamics::{
RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex,
BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle,
ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData,
ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
TemporaryInteractionIndex,
};
use crate::math::{Real, Vector};
use crate::pipeline::{
@@ -342,7 +343,7 @@ impl NarrowPhase {
islands.wake_up(bodies, parent.handle, true)
}
if pair.start_event_emited {
if pair.start_event_emitted {
events.handle_collision_event(
bodies,
colliders,
@@ -354,7 +355,7 @@ impl NarrowPhase {
} else {
// If there is no island, don’t wake-up bodies, but do send the Stopped collision event.
for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
if pair.start_event_emited {
if pair.start_event_emitted {
events.handle_collision_event(
bodies,
colliders,
@@ -370,7 +371,7 @@ impl NarrowPhase {
.intersection_graph
.interactions_with(intersection_graph_id)
{
if pair.start_event_emited {
if pair.start_event_emitted {
events.handle_collision_event(
bodies,
colliders,
@@ -709,7 +710,6 @@ impl NarrowPhase {
let co1 = &colliders[handle1];
let co2 = &colliders[handle2];
// TODO: remove the `loop` once labels on blocks is stabilized.
'emit_events: {
if !co1.changes.needs_narrow_phase_update()
&& !co2.changes.needs_narrow_phase_update()
@@ -767,7 +767,6 @@ impl NarrowPhase {
edge.weight.intersecting = query_dispatcher
.intersection_test(&pos12, &*co1.shape, &*co2.shape)
.unwrap_or(false);
break 'emit_events;
}
let active_events = co1.flags.active_events | co2.flags.active_events;
@@ -789,6 +788,7 @@ impl NarrowPhase {
pub(crate) fn compute_contacts(
&mut self,
prediction_distance: Real,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
@@ -810,7 +810,6 @@ impl NarrowPhase {
let co1 = &colliders[pair.collider1];
let co2 = &colliders[pair.collider2];
// TODO: remove the `loop` once labels on blocks are supported.
'emit_events: {
if !co1.changes.needs_narrow_phase_update()
&& !co2.changes.needs_narrow_phase_update()
@@ -819,17 +818,11 @@ impl NarrowPhase {
return;
}
// TODO: avoid lookup into bodies.
let mut rb_type1 = RigidBodyType::Fixed;
let mut rb_type2 = RigidBodyType::Fixed;
let rb1 = co1.parent.map(|co_parent1| &bodies[co_parent1.handle]);
let rb2 = co2.parent.map(|co_parent2| &bodies[co_parent2.handle]);
if let Some(co_parent1) = &co1.parent {
rb_type1 = bodies[co_parent1.handle].body_type;
}
if let Some(co_parent2) = &co2.parent {
rb_type2 = bodies[co_parent2.handle].body_type;
}
let rb_type1 = rb1.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed);
let rb_type2 = rb2.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed);
// Deal with contacts disabled between bodies attached by joints.
if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) {
@@ -901,11 +894,37 @@ impl NarrowPhase {
}
let pos12 = co1.pos.inv_mul(&co2.pos);
let contact_skin_sum = co1.contact_skin() + co2.contact_skin();
let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 {
let aabb1 = co1.compute_collision_aabb(0.0);
let aabb2 = co2.compute_collision_aabb(0.0);
let inv_dt = crate::utils::inv(dt);
let linvel1 = rb1.map(|rb| rb.linvel()
.cap_magnitude(soft_ccd_prediction1 * inv_dt)).unwrap_or_default();
let linvel2 = rb2.map(|rb| rb.linvel()
.cap_magnitude(soft_ccd_prediction2 * inv_dt)).unwrap_or_default();
if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) {
pair.clear();
break 'emit_events;
}
prediction_distance.max(
dt * (linvel1 - linvel2).norm()) + contact_skin_sum
} else {
prediction_distance + contact_skin_sum
};
let _ = query_dispatcher.contact_manifolds(
&pos12,
&*co1.shape,
&*co2.shape,
prediction_distance,
effective_prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
);
@@ -924,14 +943,8 @@ impl NarrowPhase {
);
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co1
.parent
.map(|p1| bodies[p1.handle].dominance)
.unwrap_or(zero);
let dominance2 = co2
.parent
.map(|p2| bodies[p2.handle].dominance)
.unwrap_or(zero);
let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero);
let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero);
pair.has_any_active_contact = false;
@@ -948,12 +961,22 @@ impl NarrowPhase {
// Generate solver contacts.
for (contact_id, contact) in manifold.points.iter().enumerate() {
assert!(
contact_id <= u8::MAX as usize,
"A contact manifold cannot contain more than 255 contacts currently."
);
if contact_id > u8::MAX as usize {
log::warn!("A contact manifold cannot contain more than 255 contacts currently, dropping contact in excess.");
break;
}
if contact.dist < prediction_distance {
let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin();
let keep_solver_contact = effective_contact_dist < prediction_distance || {
let world_pt1 = world_pos1 * contact.local_p1;
let world_pt2 = world_pos2 * contact.local_p2;
let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default();
let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default();
effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance
};
if keep_solver_contact {
// Generate the solver contact.
let world_pt1 = world_pos1 * contact.local_p1;
let world_pt2 = world_pos2 * contact.local_p2;
@@ -962,11 +985,13 @@ impl NarrowPhase {
let solver_contact = SolverContact {
contact_id: contact_id as u8,
point: effective_point,
dist: contact.dist,
dist: effective_contact_dist,
friction,
restitution,
tangent_velocity: Vector::zeros(),
is_new: contact.data.impulse == 0.0,
warmstart_impulse: contact.data.warmstart_impulse,
warmstart_tangent_impulse: contact.data.warmstart_tangent_impulse,
};
manifold.data.solver_contacts.push(solver_contact);
@@ -1000,9 +1025,36 @@ impl NarrowPhase {
manifold.data.normal = modifiable_normal;
manifold.data.user_data = modifiable_user_data;
}
}
break 'emit_events;
/*
* TODO: When using the block solver in 3D, I’d expect this sort to help, but
* it makes the domino demo worse. Needs more investigation.
fn sort_solver_contacts(mut contacts: &mut [SolverContact]) {
while contacts.len() > 2 {
let first = contacts[0];
let mut furthest_id = 1;
let mut furthest_dist = na::distance(&first.point, &contacts[1].point);
for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) {
let candidate_dist = na::distance(&first.point, &candidate.point);
if candidate_dist > furthest_dist {
furthest_dist = candidate_dist;
furthest_id = candidate_id;
}
}
if furthest_id > 1 {
contacts.swap(1, furthest_id);
}
contacts = &mut contacts[2..];
}
}
sort_solver_contacts(&mut manifold.data.solver_contacts);
*/
}
}
let active_events = co1.flags.active_events | co2.flags.active_events;