From 754a48b7ff6d8c58b1ee08651e60112900b60455 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 25 Aug 2020 22:10:25 +0200 Subject: First public release of Rapier. --- src/geometry/ball.rs | 16 + src/geometry/broad_phase.rs | 255 ++++++++ src/geometry/broad_phase_multi_sap.rs | 645 +++++++++++++++++++++ src/geometry/capsule.rs | 168 ++++++ src/geometry/collider.rs | 373 ++++++++++++ src/geometry/collider_set.rs | 139 +++++ src/geometry/contact.rs | 485 ++++++++++++++++ .../ball_ball_contact_generator.rs | 98 ++++ .../ball_convex_contact_generator.rs | 85 +++ .../ball_polygon_contact_generator.rs | 0 .../capsule_capsule_contact_generator.rs | 200 +++++++ .../contact_generator/contact_dispatcher.rs | 127 ++++ .../contact_generator/contact_generator.rs | 222 +++++++ .../cuboid_capsule_contact_generator.rs | 188 ++++++ .../cuboid_cuboid_contact_generator.rs | 155 +++++ .../cuboid_polygon_contact_generator.rs | 0 .../cuboid_triangle_contact_generator.rs | 171 ++++++ .../heightfield_shape_contact_generator.rs | 189 ++++++ src/geometry/contact_generator/mod.rs | 71 +++ .../polygon_polygon_contact_generator.rs | 298 ++++++++++ .../trimesh_shape_contact_generator.rs | 194 +++++++ .../voxels_shape_contact_generator.rs | 0 src/geometry/cuboid.rs | 234 ++++++++ src/geometry/cuboid_feature2d.rs | 128 ++++ src/geometry/cuboid_feature3d.rs | 516 +++++++++++++++++ src/geometry/interaction_graph.rs | 259 +++++++++ src/geometry/mod.rs | 80 +++ src/geometry/narrow_phase.rs | 483 +++++++++++++++ src/geometry/polygon.rs | 76 +++ src/geometry/polygon_intersection.rs | 263 +++++++++ src/geometry/polyhedron_feature3d.rs | 284 +++++++++ src/geometry/proximity.rs | 31 + .../ball_ball_proximity_detector.rs | 68 +++ .../ball_convex_proximity_detector.rs | 53 ++ .../ball_polygon_proximity_detector.rs | 0 .../cuboid_cuboid_proximity_detector.rs | 79 +++ .../cuboid_polygon_proximity_detector.rs | 0 .../cuboid_triangle_proximity_detector.rs | 88 +++ src/geometry/proximity_detector/mod.rs | 30 + .../polygon_polygon_proximity_detector.rs | 54 ++ .../proximity_detector/proximity_detector.rs | 212 +++++++ .../proximity_detector/proximity_dispatcher.rs | 136 +++++ .../trimesh_shape_proximity_detector.rs | 133 +++++ .../voxels_shape_proximity_detector.rs | 0 src/geometry/sat.rs | 294 ++++++++++ src/geometry/triangle.rs | 9 + src/geometry/trimesh.rs | 122 ++++ src/geometry/waabb.rs | 116 ++++ src/geometry/z_order.rs | 70 +++ 49 files changed, 7897 insertions(+) create mode 100644 src/geometry/ball.rs create mode 100644 src/geometry/broad_phase.rs create mode 100644 src/geometry/broad_phase_multi_sap.rs create mode 100644 src/geometry/capsule.rs create mode 100644 src/geometry/collider.rs create mode 100644 src/geometry/collider_set.rs create mode 100644 src/geometry/contact.rs create mode 100644 src/geometry/contact_generator/ball_ball_contact_generator.rs create mode 100644 src/geometry/contact_generator/ball_convex_contact_generator.rs create mode 100644 src/geometry/contact_generator/ball_polygon_contact_generator.rs create mode 100644 src/geometry/contact_generator/capsule_capsule_contact_generator.rs create mode 100644 src/geometry/contact_generator/contact_dispatcher.rs create mode 100644 src/geometry/contact_generator/contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_capsule_contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_polygon_contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_triangle_contact_generator.rs create mode 100644 src/geometry/contact_generator/heightfield_shape_contact_generator.rs create mode 100644 src/geometry/contact_generator/mod.rs create mode 100644 src/geometry/contact_generator/polygon_polygon_contact_generator.rs create mode 100644 src/geometry/contact_generator/trimesh_shape_contact_generator.rs create mode 100644 src/geometry/contact_generator/voxels_shape_contact_generator.rs create mode 100644 src/geometry/cuboid.rs create mode 100644 src/geometry/cuboid_feature2d.rs create mode 100644 src/geometry/cuboid_feature3d.rs create mode 100644 src/geometry/interaction_graph.rs create mode 100644 src/geometry/mod.rs create mode 100644 src/geometry/narrow_phase.rs create mode 100644 src/geometry/polygon.rs create mode 100644 src/geometry/polygon_intersection.rs create mode 100644 src/geometry/polyhedron_feature3d.rs create mode 100644 src/geometry/proximity.rs create mode 100644 src/geometry/proximity_detector/ball_ball_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/ball_convex_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/ball_polygon_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/mod.rs create mode 100644 src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/proximity_detector.rs create mode 100644 src/geometry/proximity_detector/proximity_dispatcher.rs create mode 100644 src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/voxels_shape_proximity_detector.rs create mode 100644 src/geometry/sat.rs create mode 100644 src/geometry/triangle.rs create mode 100644 src/geometry/trimesh.rs create mode 100644 src/geometry/waabb.rs create mode 100644 src/geometry/z_order.rs (limited to 'src/geometry') diff --git a/src/geometry/ball.rs b/src/geometry/ball.rs new file mode 100644 index 0000000..7f4ad03 --- /dev/null +++ b/src/geometry/ball.rs @@ -0,0 +1,16 @@ +#[cfg(feature = "simd-is-enabled")] +use crate::math::{Point, SimdFloat}; + +#[cfg(feature = "simd-is-enabled")] +#[derive(Copy, Clone, Debug)] +pub(crate) struct WBall { + pub center: Point, + pub radius: SimdFloat, +} + +#[cfg(feature = "simd-is-enabled")] +impl WBall { + pub fn new(center: Point, radius: SimdFloat) -> Self { + WBall { center, radius } + } +} diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs new file mode 100644 index 0000000..a43b7af --- /dev/null +++ b/src/geometry/broad_phase.rs @@ -0,0 +1,255 @@ +use crate::geometry::ColliderHandle; +use ncollide::bounding_volume::AABB; +#[cfg(feature = "simd-is-enabled")] +use { + crate::geometry::WAABB, + crate::math::{Point, SIMD_WIDTH}, + crate::utils::WVec, + simba::simd::SimdBool as _, +}; + +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderPair { + pub collider1: ColliderHandle, + pub collider2: ColliderHandle, +} + +impl ColliderPair { + pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + ColliderPair { + collider1, + collider2, + } + } + + pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { + Self::new(collider1, collider2) + } else { + Self::new(collider2, collider1) + } + } + + pub fn swap(self) -> Self { + Self::new(self.collider2, self.collider1) + } + + pub fn zero() -> Self { + Self { + collider1: ColliderHandle::from_raw_parts(0, 0), + collider2: ColliderHandle::from_raw_parts(0, 0), + } + } +} + +pub struct WAABBHierarchyIntersections { + curr_level_interferences: Vec, + next_level_interferences: Vec, +} + +impl WAABBHierarchyIntersections { + pub fn new() -> Self { + Self { + curr_level_interferences: Vec::new(), + next_level_interferences: Vec::new(), + } + } + + pub fn computed_interferences(&self) -> &[usize] { + &self.curr_level_interferences[..] + } + + pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec { + &mut self.curr_level_interferences + } +} + +#[cfg(feature = "simd-is-enabled")] +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct WAABBHierarchy { + levels: Vec>, +} + +#[cfg(feature = "simd-is-enabled")] +impl WAABBHierarchy { + pub fn new(aabbs: &[AABB]) -> Self { + let mut waabbs: Vec<_> = aabbs + .chunks_exact(SIMD_WIDTH) + .map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH])) + .collect(); + + if aabbs.len() % SIMD_WIDTH != 0 { + let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH; + let last_i = aabbs.len() - 1; + let last_waabb = + WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]); + waabbs.push(last_waabb); + } + + let mut levels = vec![waabbs]; + + loop { + let last_level = levels.last().unwrap(); + let mut next_level = Vec::new(); + + for chunk in last_level.chunks_exact(SIMD_WIDTH) { + let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]); + let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]); + next_level.push(WAABB::new(mins, maxs)); + } + + // Deal with the last non-exact chunk. + if last_level.len() % SIMD_WIDTH != 0 { + let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH; + let last_id = last_level.len() - 1; + let mins = array![|ii| last_level[(first_id + ii).min(last_id)] + .mins + .horizontal_inf(); SIMD_WIDTH]; + let maxs = array![|ii| last_level[(first_id + ii).min(last_id)] + .maxs + .horizontal_sup(); SIMD_WIDTH]; + + let mins = Point::from(mins); + let maxs = Point::from(maxs); + next_level.push(WAABB::new(mins, maxs)); + } + + if next_level.len() == 1 { + levels.push(next_level); + break; + } + + levels.push(next_level); + } + + Self { levels } + } + + pub fn compute_interferences_with( + &self, + aabb: AABB, + workspace: &mut WAABBHierarchyIntersections, + ) { + let waabb1 = WAABB::splat(aabb); + workspace.next_level_interferences.clear(); + workspace.curr_level_interferences.clear(); + workspace.curr_level_interferences.push(0); + + for level in self.levels.iter().rev() { + for i in &workspace.curr_level_interferences { + // This `if let` handle the case when `*i` is out of bounds because + // the initial number of aabbs was not a power of SIMD_WIDTH. + if let Some(waabb2) = level.get(*i) { + // NOTE: using `intersect.bitmask()` and performing bit comparisons + // is much more efficient than testing if each intersect.extract(i) is true. + let intersect = waabb1.intersects_lanewise(waabb2); + let bitmask = intersect.bitmask(); + + for j in 0..SIMD_WIDTH { + if (bitmask & (1 << j)) != 0 { + workspace.next_level_interferences.push(i * SIMD_WIDTH + j) + } + } + } + } + + std::mem::swap( + &mut workspace.curr_level_interferences, + &mut workspace.next_level_interferences, + ); + workspace.next_level_interferences.clear(); + } + } +} + +#[cfg(not(feature = "simd-is-enabled"))] +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct WAABBHierarchy { + levels: Vec>>, +} + +#[cfg(not(feature = "simd-is-enabled"))] +impl WAABBHierarchy { + const GROUP_SIZE: usize = 4; + + pub fn new(aabbs: &[AABB]) -> Self { + use ncollide::bounding_volume::BoundingVolume; + + let mut levels = vec![aabbs.to_vec()]; + + loop { + let last_level = levels.last().unwrap(); + let mut next_level = Vec::new(); + + for chunk in last_level.chunks(Self::GROUP_SIZE) { + let mut merged = chunk[0]; + for aabb in &chunk[1..] { + merged.merge(aabb) + } + + next_level.push(merged); + } + + if next_level.len() == 1 { + levels.push(next_level); + break; + } + + levels.push(next_level); + } + + Self { levels } + } + + pub fn compute_interferences_with( + &self, + aabb1: AABB, + workspace: &mut WAABBHierarchyIntersections, + ) { + use ncollide::bounding_volume::BoundingVolume; + + workspace.next_level_interferences.clear(); + workspace.curr_level_interferences.clear(); + workspace.curr_level_interferences.push(0); + + for level in self.levels[1..].iter().rev() { + for i in &workspace.curr_level_interferences { + for j in 0..Self::GROUP_SIZE { + if let Some(aabb2) = level.get(*i + j) { + if aabb1.intersects(aabb2) { + workspace + .next_level_interferences + .push((i + j) * Self::GROUP_SIZE) + } + } + } + } + + std::mem::swap( + &mut workspace.curr_level_interferences, + &mut workspace.next_level_interferences, + ); + workspace.next_level_interferences.clear(); + } + + // Last level. + for i in &workspace.curr_level_interferences { + for j in 0..Self::GROUP_SIZE { + if let Some(aabb2) = self.levels[0].get(*i + j) { + if aabb1.intersects(aabb2) { + workspace.next_level_interferences.push(i + j) + } + } + } + } + + std::mem::swap( + &mut workspace.curr_level_interferences, + &mut workspace.next_level_interferences, + ); + workspace.next_level_interferences.clear(); + } +} diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs new file mode 100644 index 0000000..8356339 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap.rs @@ -0,0 +1,645 @@ +use crate::dynamics::RigidBodySet; +use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet}; +use crate::math::{Point, Vector, DIM}; +#[cfg(feature = "enhanced-determinism")] +use crate::utils::FxHashMap32 as HashMap; +use bit_vec::BitVec; +use ncollide::bounding_volume::{BoundingVolume, AABB}; +#[cfg(not(feature = "enhanced-determinism"))] +use rustc_hash::FxHashMap as HashMap; +use std::cmp::Ordering; +use std::ops::{Index, IndexMut}; + +const NUM_SENTINELS: usize = 1; +const NEXT_FREE_SENTINEL: u32 = u32::MAX; +const SENTINEL_VALUE: f32 = f32::MAX; +const CELL_WIDTH: f32 = 20.0; + +pub enum BroadPhasePairEvent { + AddPair(ColliderPair), + DeletePair(ColliderPair), +} + +fn sort2(a: u32, b: u32) -> (u32, u32) { + assert_ne!(a, b); + + if a < b { + (a, b) + } else { + (b, a) + } +} + +fn point_key(point: Point) -> Point { + (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() +} + +fn region_aabb(index: Point) -> AABB { + let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into(); + let maxs = mins + Vector::repeat(CELL_WIDTH); + AABB::new(mins, maxs) +} + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct Endpoint { + value: f32, + packed_flag_proxy: u32, +} + +const START_FLAG_MASK: u32 = 0b1 << 31; +const PROXY_MASK: u32 = u32::MAX ^ START_FLAG_MASK; +const START_SENTINEL_TAG: u32 = u32::MAX; +const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK; + +impl Endpoint { + pub fn start_endpoint(value: f32, proxy: u32) -> Self { + Self { + value, + packed_flag_proxy: proxy | START_FLAG_MASK, + } + } + + pub fn end_endpoint(value: f32, proxy: u32) -> Self { + Self { + value, + packed_flag_proxy: proxy & PROXY_MASK, + } + } + + pub fn start_sentinel() -> Self { + Self { + value: -SENTINEL_VALUE, + packed_flag_proxy: START_SENTINEL_TAG, + } + } + + pub fn end_sentinel() -> Self { + Self { + value: SENTINEL_VALUE, + packed_flag_proxy: END_SENTINEL_TAG, + } + } + + pub fn is_sentinel(self) -> bool { + self.packed_flag_proxy & PROXY_MASK == PROXY_MASK + } + + pub fn proxy(self) -> u32 { + self.packed_flag_proxy & PROXY_MASK + } + + pub fn is_start(self) -> bool { + (self.packed_flag_proxy & START_FLAG_MASK) != 0 + } + + pub fn is_end(self) -> bool { + (self.packed_flag_proxy & START_FLAG_MASK) == 0 + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct SAPAxis { + min_bound: f32, + max_bound: f32, + endpoints: Vec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + new_endpoints: Vec<(Endpoint, usize)>, // Workspace +} + +impl SAPAxis { + fn new(min_bound: f32, max_bound: f32) -> Self { + assert!(min_bound <= max_bound); + + Self { + min_bound, + max_bound, + endpoints: vec![Endpoint::start_sentinel(), Endpoint::end_sentinel()], + new_endpoints: Vec::new(), + } + } + + fn batch_insert( + &mut self, + dim: usize, + new_proxies: &[usize], + proxies: &Proxies, + reporting: Option<&mut HashMap<(u32, u32), bool>>, + ) { + if new_proxies.is_empty() { + return; + } + + self.new_endpoints.clear(); + + for proxy_id in new_proxies { + let proxy = &proxies[*proxy_id]; + assert!(proxy.aabb.mins[dim] <= self.max_bound); + assert!(proxy.aabb.maxs[dim] >= self.min_bound); + let start_endpoint = Endpoint::start_endpoint(proxy.aabb.mins[dim], *proxy_id as u32); + let end_endpoint = Endpoint::end_endpoint(proxy.aabb.maxs[dim], *proxy_id as u32); + + self.new_endpoints.push((start_endpoint, 0)); + self.new_endpoints.push((end_endpoint, 0)); + } + + self.new_endpoints + .sort_by(|a, b| a.0.value.partial_cmp(&b.0.value).unwrap_or(Ordering::Equal)); + + let mut curr_existing_index = self.endpoints.len() - NUM_SENTINELS - 1; + let new_num_endpoints = self.endpoints.len() + self.new_endpoints.len(); + self.endpoints + .resize(new_num_endpoints, Endpoint::end_sentinel()); + let mut curr_shift_index = new_num_endpoints - NUM_SENTINELS - 1; + + // Sort the endpoints. + // TODO: specialize for the case where this is the + // first time we insert endpoints to this axis? + for new_endpoint in self.new_endpoints.iter_mut().rev() { + loop { + let existing_endpoint = self.endpoints[curr_existing_index]; + if existing_endpoint.value <= new_endpoint.0.value { + break; + } + + self.endpoints[curr_shift_index] = existing_endpoint; + + curr_shift_index -= 1; + curr_existing_index -= 1; + } + + self.endpoints[curr_shift_index] = new_endpoint.0; + new_endpoint.1 = curr_shift_index; + curr_shift_index -= 1; + } + + // Report pairs using a single mbp pass on each new endpoint. + let endpoints_wo_last_sentinel = &self.endpoints[..self.endpoints.len() - 1]; + if let Some(reporting) = reporting { + for (endpoint, endpoint_id) in self.new_endpoints.drain(..).filter(|e| e.0.is_start()) { + let proxy1 = &proxies[endpoint.proxy() as usize]; + let min = endpoint.value; + let max = proxy1.aabb.maxs[dim]; + + for endpoint2 in &endpoints_wo_last_sentinel[endpoint_id + 1..] { + if endpoint2.proxy() == endpoint.proxy() { + continue; + } + + let proxy2 = &proxies[endpoint2.proxy() as usize]; + + // NOTE: some pairs with equal aabb.mins[dim] may end up being reported twice. + if (endpoint2.is_start() && endpoint2.value < max) + || (endpoint2.is_end() && proxy2.aabb.mins[dim] <= min) + { + // Report pair. + if proxy1.aabb.intersects(&proxy2.aabb) { + // Report pair. + let pair = sort2(endpoint.proxy(), endpoint2.proxy()); + reporting.insert(pair, true); + } + } + } + } + } + } + + fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> bool { + let mut deleted_any = false; + for endpoint in &self.endpoints { + if endpoint.value < self.min_bound { + if endpoint.is_end() { + existing_proxies.set(endpoint.proxy() as usize, false); + deleted_any = true; + } + } else { + break; + } + } + + for endpoint in self.endpoints.iter().rev() { + if endpoint.value > self.max_bound { + if endpoint.is_start() { + existing_proxies.set(endpoint.proxy() as usize, false); + deleted_any = true; + } + } else { + break; + } + } + + deleted_any + } + + fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) { + self.endpoints + .retain(|endpt| endpt.is_sentinel() || existing_proxies[endpt.proxy() as usize]) + } + + fn update_endpoints( + &mut self, + dim: usize, + proxies: &Proxies, + reporting: &mut HashMap<(u32, u32), bool>, + ) { + let last_endpoint = self.endpoints.len() - NUM_SENTINELS; + for i in NUM_SENTINELS..last_endpoint { + let mut endpoint_i = self.endpoints[i]; + let aabb_i = proxies[endpoint_i.proxy() as usize].aabb; + + if endpoint_i.is_start() { + endpoint_i.value = aabb_i.mins[dim]; + } else { + endpoint_i.value = aabb_i.maxs[dim]; + } + + let mut j = i; + + if endpoint_i.is_start() { + while endpoint_i.value < self.endpoints[j - 1].value { + let endpoint_j = self.endpoints[j - 1]; + self.endpoints[j] = endpoint_j; + + if endpoint_j.is_end() { + // Report start collision. + if aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].aabb) { + let pair = sort2(endpoint_i.proxy(), endpoint_j.proxy()); + reporting.insert(pair, true); + } + } + + j -= 1; + } + } else { + while endpoint_i.value < self.endpoints[j - 1].value { + let endpoint_j = self.endpoints[j - 1]; + self.endpoints[j] = endpoint_j; + + if endpoint_j.is_start() { + // Report end collision. + if !aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].aabb) { + let pair = sort2(endpoint_i.proxy(), endpoint_j.proxy()); + reporting.insert(pair, false); + } + } + + j -= 1; + } + } + + self.endpoints[j] = endpoint_i; + } + + // println!( + // "Num start swaps: {}, end swaps: {}, dim: {}", + // num_start_swaps, num_end_swaps, dim + // ); + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct SAPRegion { + axii: [SAPAxis; DIM], + existing_proxies: BitVec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + to_insert: Vec, // Workspace + need_update: bool, +} + +impl SAPRegion { + pub fn new(bounds: AABB) -> Self { + let axii = [ + SAPAxis::new(bounds.mins.x, bounds.maxs.x), + SAPAxis::new(bounds.mins.y, bounds.maxs.y), + #[cfg(feature = "dim3")] + SAPAxis::new(bounds.mins.z, bounds.maxs.z), + ]; + SAPRegion { + axii, + existing_proxies: BitVec::new(), + to_insert: Vec::new(), + need_update: false, + } + } + + pub fn predelete_proxy(&mut self, _proxy_id: usize) { + // 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. + self.need_update = true; + } + + pub fn preupdate_proxy(&mut self, proxy_id: usize) -> bool { + let mask_len = self.existing_proxies.len(); + if proxy_id >= mask_len { + self.existing_proxies.grow(proxy_id + 1 - mask_len, false); + } + + if !self.existing_proxies[proxy_id] { + self.to_insert.push(proxy_id); + self.existing_proxies.set(proxy_id, true); + false + } else { + self.need_update = true; + true + } + } + + pub fn update(&mut self, proxies: &Proxies, reporting: &mut HashMap<(u32, u32), bool>) { + if self.need_update { + // Update endpoints. + let mut deleted_any = false; + for dim in 0..DIM { + self.axii[dim].update_endpoints(dim, proxies, reporting); + deleted_any = self.axii[dim] + .delete_out_of_bounds_proxies(&mut self.existing_proxies) + || deleted_any; + } + + if deleted_any { + for dim in 0..DIM { + self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + } + } + + self.need_update = false; + } + + if !self.to_insert.is_empty() { + // Insert new proxies. + for dim in 1..DIM { + self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + } + self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.to_insert.clear(); + } + } +} + +/// A broad-phase based on multiple Sweep-and-Prune instances running of disjoint region of the 3D world. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct BroadPhase { + proxies: Proxies, + regions: HashMap, SAPRegion>, + deleted_any: bool, + // We could think serializing this workspace is useless. + // It turns out is is important to serialize at least its capacity + // and restore this capacity when deserializing the hashmap. + // This is because the order of future elements inserted into the + // hashmap depends on its capacity (because the internal bucket indices + // depend on this capacity). So not restoring this capacity may alter + // the order at which future elements are reported. This will in turn + // alter the order at which the pairs are registered in the narrow-phase, + // thus altering the order of the contact manifold. In the end, this + // alters the order of the resolution of contacts, resulting in + // diverging simulation after restoration of a snapshot. + #[cfg_attr( + feature = "serde-serialize", + serde( + serialize_with = "crate::utils::serialize_hashmap_capacity", + deserialize_with = "crate::utils::deserialize_hashmap_capacity" + ) + )] + reporting: HashMap<(u32, u32), bool>, // Workspace +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct BroadPhaseProxy { + handle: ColliderHandle, + aabb: AABB, + next_free: u32, +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct Proxies { + elements: Vec, + first_free: u32, +} + +impl Proxies { + pub fn new() -> Self { + Self { + elements: Vec::new(), + first_free: NEXT_FREE_SENTINEL, + } + } + + pub fn insert(&mut self, proxy: BroadPhaseProxy) -> usize { + if self.first_free != NEXT_FREE_SENTINEL { + let proxy_id = self.first_free; + self.first_free = self.elements[self.first_free as usize].next_free; + self.elements[self.first_free as usize] = proxy; + proxy_id as usize + } else { + self.elements.push(proxy); + self.elements.len() - 1 + } + } + + pub fn remove(&mut self, proxy_id: usize) { + self.elements[proxy_id].next_free = self.first_free; + self.first_free = proxy_id as u32; + } + + // // FIXME: take holes into account? + // pub fn get(&self, i: usize) -> Option<&BroadPhaseProxy> { + // self.elements.get(i) + // } + + // FIXME: take holes into account? + pub fn get_mut(&mut self, i: usize) -> Option<&mut BroadPhaseProxy> { + self.elements.get_mut(i) + } +} + +impl Index for Proxies { + type Output = BroadPhaseProxy; + fn index(&self, i: usize) -> &BroadPhaseProxy { + self.elements.index(i) + } +} + +impl IndexMut for Proxies { + fn index_mut(&mut self, i: usize) -> &mut BroadPhaseProxy { + self.elements.index_mut(i) + } +} + +impl BroadPhase { + /// Create a new empty broad-phase. + pub fn new() -> Self { + BroadPhase { + proxies: Proxies::new(), + regions: HashMap::default(), + reporting: HashMap::default(), + deleted_any: false, + } + } + + pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) { + for collider in handles.iter().filter_map(|h| colliders.get(*h)) { + if collider.proxy_index == crate::INVALID_USIZE { + // This collider has not been added to the broad-phase yet. + continue; + } + + let proxy = &mut self.proxies[collider.proxy_index]; + + // Push the proxy to infinity, but not beyond the sentinels. + proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0); + proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0); + // Discretize the AABB to find the regions that need to be invalidated. + let start = point_key(proxy.aabb.mins); + let end = point_key(proxy.aabb.maxs); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { + region.predelete_proxy(collider.proxy_index); + self.deleted_any = true; + } + } + } + + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { + region.predelete_proxy(collider.proxy_index); + self.deleted_any = true; + } + } + } + } + + self.proxies.remove(collider.proxy_index); + } + } + + pub(crate) fn update_aabbs( + &mut self, + prediction_distance: f32, + bodies: &RigidBodySet, + colliders: &mut ColliderSet, + ) { + // First, if we have any pending removals we have + // to deal with them now because otherwise we will + // end up with an ABA problems when reusing proxy + // ids. + self.complete_removals(); + + for body_handle in bodies + .active_dynamic_set + .iter() + .chain(bodies.active_kinematic_set.iter()) + { + for handle in &bodies[*body_handle].colliders { + let collider = &mut colliders[*handle]; + let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); + + if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { + proxy.aabb = aabb; + } else { + let proxy = BroadPhaseProxy { + handle: *handle, + aabb, + next_free: NEXT_FREE_SENTINEL, + }; + collider.proxy_index = self.proxies.insert(proxy); + } + + // Discretize the aabb. + let proxy_id = collider.proxy_index; + // let start = Point::origin(); + // let end = Point::origin(); + let start = point_key(aabb.mins); + let end = point_key(aabb.maxs); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + let region_key = Point::new(i, j); + let region_bounds = region_aabb(region_key); + let region = self + .regions + .entry(region_key) + .or_insert_with(|| SAPRegion::new(region_bounds)); + let _ = region.preupdate_proxy(proxy_id); + } + } + + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + let region_key = Point::new(i, j, k); + let region_bounds = region_aabb(region_key); + let region = self + .regions + .entry(region_key) + .or_insert_with(|| SAPRegion::new(region_bounds)); + let _ = region.preupdate_proxy(proxy_id); + } + } + } + } + } + } + + pub(crate) fn complete_removals(&mut self) { + if self.deleted_any { + for (_, region) in &mut self.regions { + region.update(&self.proxies, &mut self.reporting); + } + + // NOTE: we don't care about reporting pairs. + self.reporting.clear(); + self.deleted_any = false; + } + } + + pub(crate) fn find_pairs(&mut self, out_events: &mut Vec) { + // println!("num regions: {}", self.regions.len()); + + self.reporting.clear(); + for (_, region) in &mut self.regions { + region.update(&self.proxies, &mut self.reporting) + } + + // Convert reports to broad phase events. + // let t = instant::now(); + // let mut num_add_events = 0; + // let mut num_delete_events = 0; + + for ((proxy1, proxy2), colliding) in &self.reporting { + let proxy1 = &self.proxies[*proxy1 as usize]; + let proxy2 = &self.proxies[*proxy2 as usize]; + + let handle1 = proxy1.handle; + let handle2 = proxy2.handle; + + if *colliding { + out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new( + handle1, handle2, + ))); + // num_add_events += 1; + } else { + out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new( + handle1, handle2, + ))); + // num_delete_events += 1; + } + } + + // println!( + // "Event conversion time: {}, add: {}/{}, delete: {}/{}", + // instant::now() - t, + // num_add_events, + // out_events.len(), + // num_delete_events, + // out_events.len() + // ); + } +} diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs new file mode 100644 index 0000000..0d754af --- /dev/null +++ b/src/geometry/capsule.rs @@ -0,0 +1,168 @@ +use crate::geometry::AABB; +use crate::math::{Isometry, Point, Rotation, Vector}; +use approx::AbsDiffEq; +use na::Unit; +use ncollide::query::{PointProjection, PointQuery}; +use ncollide::shape::{FeatureId, Segment}; + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A capsule shape defined as a segment with a radius. +pub struct Capsule { + /// The first endpoint of the capsule. + pub a: Point, + /// The second enpdoint of the capsule. + pub b: Point, + /// The radius of the capsule. + pub radius: f32, +} + +impl Capsule { + /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius. + pub fn new_x(half_height: f32, radius: f32) -> Self { + let b = Point::from(Vector::x() * half_height); + Self::new(-b, b, radius) + } + + /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius. + pub fn new_y(half_height: f32, radius: f32) -> Self { + let b = Point::from(Vector::y() * half_height); + Self::new(-b, b, radius) + } + + /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius. + #[cfg(feature = "dim3")] + pub fn new_z(half_height: f32, radius: f32) -> Self { + let b = Point::from(Vector::z() * half_height); + Self::new(-b, b, radius) + } + + /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. + pub fn new(a: Point, b: Point, radius: f32) -> Self { + Self { a, b, radius } + } + + /// The axis-aligned bounding box of this capsule. + pub fn aabb(&self, pos: &Isometry) -> AABB { + let a = pos * self.a; + let b = pos * self.b; + let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); + let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); + AABB::new(mins.into(), maxs.into()) + } + + /// The height of this capsule. + pub fn height(&self) -> f32 { + (self.b - self.a).norm() + } + + /// The half-height of this capsule. + pub fn half_height(&self) -> f32 { + self.height() / 2.0 + } + + /// The center of this capsule. + pub fn center(&self) -> Point { + na::center(&self.a, &self.b) + } + + /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. + pub fn transform_by(&self, pos: &Isometry) -> Self { + Self::new(pos * self.a, pos * self.b, self.radius) + } + + /// The rotation `r` such that `r * Y` is collinear with `b - a`. + pub fn rotation_wrt_y(&self) -> Rotation { + let mut dir = self.b - self.a; + if dir.y < 0.0 { + dir = -dir; + } + + #[cfg(feature = "dim2")] + { + Rotation::rotation_between(&Vector::y(), &dir) + } + + #[cfg(feature = "dim3")] + { + Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()) + } + } + + /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`. + pub fn transform_wrt_y(&self) -> Isometry { + let rot = self.rotation_wrt_y(); + Isometry::from_parts(self.center().coords.into(), rot) + } +} + +// impl SupportMap for Capsule { +// fn local_support_point(&self, dir: &Vector) -> Point { +// let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis()); +// self.local_support_point_toward(&dir) +// } +// +// fn local_support_point_toward(&self, dir: &Unit) -> Point { +// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) { +// self.a + **dir * self.radius +// } else { +// self.b + **dir * self.radius +// } +// } +// } + +// TODO: this code has been extracted from ncollide and added here +// so we can modify it to fit with our new definition of capsule. +// Wa should find a way to avoid this code duplication. +impl PointQuery for Capsule { + #[inline] + fn project_point( + &self, + m: &Isometry, + pt: &Point, + solid: bool, + ) -> PointProjection { + let seg = Segment::new(self.a, self.b); + let proj = seg.project_point(m, pt, solid); + let dproj = *pt - proj.point; + + if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) { + let inside = dist <= self.radius; + if solid && inside { + return PointProjection::new(true, *pt); + } else { + return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); + } + } else if solid { + return PointProjection::new(true, *pt); + } + + #[cfg(feature = "dim2")] + if let Some(dir) = seg.normal() { + let dir = m * *dir; + PointProjection::new(true, proj.point + dir * self.radius) + } else { + // The segment has no normal, likely because it degenerates to a point. + PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) + } + + #[cfg(feature = "dim3")] + if let Some(dir) = seg.direction() { + use crate::utils::WBasis; + let dir = m * dir.orthonormal_basis()[0]; + PointProjection::new(true, proj.point + dir * self.radius) + } else { + // The segment has no normal, likely because it degenerates to a point. + PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) + } + } + + #[inline] + fn project_point_with_feature( + &self, + m: &Isometry, + pt: &Point, + ) -> (PointProjection, FeatureId) { + (self.project_point(m, pt, false), FeatureId::Face(0)) + } +} diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs new file mode 100644 index 0000000..2457212 --- /dev/null +++ b/src/geometry/collider.rs @@ -0,0 +1,373 @@ +use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, + Proximity, Triangle, Trimesh, +}; +use crate::math::{Isometry, Point, Vector}; +use na::Point3; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use num::Zero; + +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// An enum grouping all the possible shape of a collider. +pub enum Shape { + /// A ball shape. + Ball(Ball), + /// A convex polygon shape. + Polygon(Polygon), + /// A cuboid shape. + Cuboid(Cuboid), + /// A capsule shape. + Capsule(Capsule), + /// A triangle shape. + Triangle(Triangle), + /// A triangle mesh shape. + Trimesh(Trimesh), + /// A heightfield shape. + HeightField(HeightField), +} + +impl Shape { + /// Gets a reference to the underlying ball shape, if `self` is one. + pub fn as_ball(&self) -> Option<&Ball> { + match self { + Shape::Ball(b) => Some(b), + _ => None, + } + } + + /// Gets a reference to the underlying polygon shape, if `self` is one. + pub fn as_polygon(&self) -> Option<&Polygon> { + match self { + Shape::Polygon(p) => Some(p), + _ => None, + } + } + + /// Gets a reference to the underlying cuboid shape, if `self` is one. + pub fn as_cuboid(&self) -> Option<&Cuboid> { + match self { + Shape::Cuboid(c) => Some(c), + _ => None, + } + } + + /// Gets a reference to the underlying capsule shape, if `self` is one. + pub fn as_capsule(&self) -> Option<&Capsule> { + match self { + Shape::Capsule(c) => Some(c), + _ => None, + } + } + + /// Gets a reference to the underlying triangle mesh shape, if `self` is one. + pub fn as_trimesh(&self) -> Option<&Trimesh> { + match self { + Shape::Trimesh(c) => Some(c), + _ => None, + } + } + + /// Gets a reference to the underlying heightfield shape, if `self` is one. + pub fn as_heightfield(&self) -> Option<&HeightField> { + match self { + Shape::HeightField(h) => Some(h), + _ => None, + } + } + + /// Gets a reference to the underlying triangle shape, if `self` is one. + pub fn as_triangle(&self) -> Option<&Triangle> { + match self { + Shape::Triangle(c) => Some(c), + _ => None, + } + } + + /// Computes the axis-aligned bounding box of this shape. + pub fn compute_aabb(&self, position: &Isometry) -> AABB { + match self { + Shape::Ball(ball) => ball.bounding_volume(position), + Shape::Polygon(poly) => poly.aabb(position), + Shape::Capsule(caps) => caps.aabb(position), + Shape::Cuboid(cuboid) => cuboid.bounding_volume(position), + Shape::Triangle(triangle) => triangle.bounding_volume(position), + Shape::Trimesh(trimesh) => trimesh.aabb(position), + Shape::HeightField(heightfield) => heightfield.bounding_volume(position), + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// 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: Shape, + density: f32, + is_sensor: bool, + pub(crate) parent: RigidBodyHandle, + pub(crate) delta: Isometry, + pub(crate) position: Isometry, + pub(crate) predicted_position: Isometry, + /// The friction coefficient of this collider. + pub friction: f32, + /// The restitution coefficient of this collider. + pub restitution: f32, + pub(crate) contact_graph_index: ColliderGraphIndex, + pub(crate) proximity_graph_index: ColliderGraphIndex, + pub(crate) proxy_index: usize, +} + +impl Clone for Collider { + fn clone(&self) -> Self { + Self { + shape: self.shape.clone(), + parent: RigidBodySet::invalid_handle(), + contact_graph_index: ColliderGraphIndex::new(crate::INVALID_U32), + proximity_graph_index: ColliderGraphIndex::new(crate::INVALID_U32), + proxy_index: crate::INVALID_USIZE, + ..*self + } + } +} + +impl Collider { + /// The rigid body this collider is attached to. + pub fn parent(&self) -> RigidBodyHandle { + self.parent + } + + /// Is this collider a sensor? + pub fn is_sensor(&self) -> bool { + self.is_sensor + } + + #[doc(hidden)] + pub fn set_position_debug(&mut self, position: Isometry) { + self.position = position; + } + + /// The position of this collider expressed in the local-space of the rigid-body it is attached to. + pub fn delta(&self) -> &Isometry { + &self.delta + } + + /// The world-space position of this collider. + pub fn position(&self) -> &Isometry { + &self.position + } + + /// The density of this collider. + pub fn density(&self) -> f32 { + self.density + } + + /// The geometric shape of this collider. + pub fn shape(&self) -> &Shape { + &self.shape + } + + /// Compute the axis-aligned bounding box of this collider. + pub fn compute_aabb(&self) -> AABB { + self.shape.compute_aabb(&self.position) + } + + // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { + // let aabb1 = self.shape.compute_aabb(&self.position); + // let aabb2 = self.shape.compute_aabb(&self.predicted_position); + // aabb1.merged(&aabb2) + // } + + /// Compute the local-space mass properties of this collider. + pub fn mass_properties(&self) -> MassProperties { + match &self.shape { + Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius), + #[cfg(feature = "dim2")] + Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()), + #[cfg(feature = "dim3")] + Shape::Polygon(_p) => unimplemented!(), + Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents), + Shape::Capsule(caps) => { + MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) + } + Shape::Triangle(_) => MassProperties::zero(), + Shape::Trimesh(_) => MassProperties::zero(), + Shape::HeightField(_) => MassProperties::zero(), + } + } +} + +/// A structure responsible for building a new collider. +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderBuilder { + /// The shape of the collider to be built. + pub shape: Shape, + /// The density of the collider to be built. + pub density: f32, + /// The friction coefficient of the collider to be built. + pub friction: f32, + /// The restitution coefficient of the collider to be built. + pub restitution: f32, + /// The position of this collider relative to the local frame of the rigid-body it is attached to. + pub delta: Isometry, + /// Is this collider a sensor? + pub is_sensor: bool, +} + +impl ColliderBuilder { + /// Initialize a new collider builder with the given shape. + pub fn new(shape: Shape) -> Self { + Self { + shape, + density: 1.0, + friction: Self::default_friction(), + restitution: 0.0, + delta: Isometry::identity(), + is_sensor: false, + } + } + + /// Initialize a new collider builder with a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + Self::new(Shape::Ball(Ball::new(radius))) + } + + /// Initialize a new collider builder with a cuboid shape defined by its half-extents. + #[cfg(feature = "dim2")] + pub fn cuboid(hx: f32, hy: f32) -> Self { + let cuboid = Cuboid { + half_extents: Vector::new(hx, hy), + }; + + Self::new(Shape::Cuboid(cuboid)) + + /* + use crate::math::Point; + let vertices = vec![ + Point::new(hx, -hy), + Point::new(hx, hy), + Point::new(-hx, hy), + Point::new(-hx, -hy), + ]; + let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; + let polygon = Polygon::new(vertices, normals); + + Self::new(Shape::Polygon(polygon)) + */ + } + + /// Initialize a new collider builder with a capsule shape aligned with the `x` axis. + pub fn capsule_x(half_height: f32, radius: f32) -> Self { + let capsule = Capsule::new_x(half_height, radius); + Self::new(Shape::Capsule(capsule)) + } + + /// Initialize a new collider builder with a capsule shape aligned with the `y` axis. + pub fn capsule_y(half_height: f32, radius: f32) -> Self { + let capsule = Capsule::new_y(half_height, radius); + Self::new(Shape::Capsule(capsule)) + } + + /// Initialize a new collider builder with a capsule shape aligned with the `z` axis. + #[cfg(feature = "dim3")] + pub fn capsule_z(half_height: f32, radius: f32) -> Self { + let capsule = Capsule::new_z(half_height, radius); + Self::new(Shape::Capsule(capsule)) + } + + /// Initialize a new collider builder with a cuboid shape defined by its half-extents. + #[cfg(feature = "dim3")] + pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self { + let cuboid = Cuboid { + half_extents: Vector::new(hx, hy, hz), + }; + + Self::new(Shape::Cuboid(cuboid)) + } + + /// Initializes a collider builder with a segment shape. + /// + /// A segment shape is modeled by a capsule with a 0 radius. + pub fn segment(a: Point, b: Point) -> Self { + let capsule = Capsule::new(a, b, 0.0); + Self::new(Shape::Capsule(capsule)) + } + + /// Initializes a collider builder with a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + let triangle = Triangle::new(a, b, c); + Self::new(Shape::Triangle(triangle)) + } + + /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { + let trimesh = Trimesh::new(vertices, indices); + Self::new(Shape::Trimesh(trimesh)) + } + + /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim2")] + pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + let heightfield = HeightField::new(heights, scale); + Self::new(Shape::HeightField(heightfield)) + } + + /// 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(heights: na::DMatrix, scale: Vector) -> Self { + let heightfield = HeightField::new(heights, scale); + Self::new(Shape::HeightField(heightfield)) + } + + /// The default friction coefficient used by the collider builder. + pub fn default_friction() -> f32 { + 0.5 + } + + /// Sets whether or not the collider built by this builder is a sensor. + pub fn sensor(mut self, is_sensor: bool) -> Self { + self.is_sensor = is_sensor; + self + } + + /// Sets the friction coefficient of the collider this builder will build. + pub fn friction(mut self, friction: f32) -> Self { + self.friction = friction; + self + } + + /// Sets the density of the collider this builder will build. + pub fn density(mut self, density: f32) -> Self { + self.density = density; + self + } + + /// Set the position of this collider in the local-space of the rigid-body it is attached to. + pub fn delta(mut self, delta: Isometry) -> Self { + self.delta = delta; + self + } + + /// Buildes a new collider attached to the given rigid-body. + pub fn build(&self) -> Collider { + Collider { + shape: self.shape.clone(), + density: self.density, + friction: self.friction, + restitution: self.restitution, + delta: self.delta, + is_sensor: self.is_sensor, + parent: RigidBodySet::invalid_handle(), + position: Isometry::identity(), + predicted_position: Isometry::identity(), + contact_graph_index: InteractionGraph::::invalid_graph_index(), + proximity_graph_index: InteractionGraph::::invalid_graph_index(), + proxy_index: crate::INVALID_USIZE, + } + } +} diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs new file mode 100644 index 0000000..73d4a06 --- /dev/null +++ b/src/geometry/collider_set.rs @@ -0,0 +1,139 @@ +use crate::data::arena::Arena; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::Collider; +use std::ops::{Index, IndexMut}; + +/// The unique identifier of a collider added to a collider set. +pub type ColliderHandle = crate::data::arena::Index; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A set of colliders that can be handled by a physics `World`. +pub struct ColliderSet { + pub(crate) colliders: Arena, +} + +impl ColliderSet { + /// Create a new empty set of colliders. + pub fn new() -> Self { + ColliderSet { + colliders: Arena::new(), + } + } + + /// An always-invalid collider handle. + pub fn invalid_handle() -> ColliderHandle { + ColliderHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + } + + /// Iterate through all the colliders on this set. + pub fn iter(&self) -> impl Iterator { + self.colliders.iter() + } + + /// The number of colliders on this set. + pub fn len(&self) -> usize { + self.colliders.len() + } + + /// Is this collider handle valid? + pub fn contains(&self, handle: ColliderHandle) -> bool { + self.colliders.contains(handle) + } + + /// Inserts a new collider to this set and retrieve its handle. + pub fn insert( + &mut self, + mut coll: Collider, + parent_handle: RigidBodyHandle, + bodies: &mut RigidBodySet, + ) -> ColliderHandle { + let mass_properties = coll.mass_properties(); + coll.parent = parent_handle; + let parent = bodies + .get_mut_internal(parent_handle) + .expect("Parent rigid body not found."); + coll.position = parent.position * coll.delta; + coll.predicted_position = parent.predicted_position * coll.delta; + let handle = self.colliders.insert(coll); + parent.colliders.push(handle); + parent.mass_properties += mass_properties; + parent.update_world_mass_properties(); + bodies.activate(parent_handle); + handle + } + + pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option { + self.colliders.remove(handle) + } + + /// Gets the collider with the given handle without a known generation. + /// + /// This is useful when you know you want the collider at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the collider position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen(&self, i: usize) -> Option<(&Collider, ColliderHandle)> { + self.colliders.get_unknown_gen(i) + } + + /// Gets a mutable reference to the collider with the given handle without a known generation. + /// + /// This is useful when you know you want the collider at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the collider position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get_mut(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> { + self.colliders.get_unknown_gen_mut(i) + } + + /// Get the collider with the given handle. + pub fn get(&self, handle: ColliderHandle) -> Option<&Collider> { + self.colliders.get(handle) + } + + /// Gets a mutable reference to the collider with the given handle. + pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { + self.colliders.get_mut(handle) + } + + pub(crate) fn get2_mut_internal( + &mut self, + h1: ColliderHandle, + h2: ColliderHandle, + ) -> (Option<&mut Collider>, Option<&mut Collider>) { + self.colliders.get2_mut(h1, h2) + } + + // pub fn iter_mut(&mut self) -> impl Iterator { + // // let sender = &self.activation_channel_sender; + // self.colliders.iter_mut().map(move |(h, rb)| { + // (h, ColliderMut::new(h, rb /*sender.clone()*/)) + // }) + // } + + // pub(crate) fn iter_mut_internal( + // &mut self, + // ) -> impl Iterator { + // self.colliders.iter_mut() + // } +} + +impl Index for ColliderSet { + type Output = Collider; + + fn index(&self, index: ColliderHandle) -> &Collider { + &self.colliders[index] + } +} + +impl IndexMut for ColliderSet { + fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider { + &mut self.colliders[index] + } +} diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs new file mode 100644 index 0000000..0beec0a --- /dev/null +++ b/src/geometry/contact.rs @@ -0,0 +1,485 @@ +use crate::dynamics::BodyPair; +use crate::geometry::contact_generator::ContactPhase; +use crate::geometry::{Collider, ColliderPair, ColliderSet}; +use crate::math::{Isometry, Point, Vector}; +use std::any::Any; +#[cfg(feature = "simd-is-enabled")] +use { + crate::math::{SimdFloat, SIMD_WIDTH}, + simba::simd::SimdValue, +}; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The type local linear approximation of the neighborhood of a pair contact points on two shapes +pub enum KinematicsCategory { + /// Both neighborhoods are assimilated to a single point. + PointPoint, + /// The first shape's neighborhood at the contact point is assimilated to a plane while + /// the second is assimilated to a point. + PlanePoint, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// Local contact geometry at the neighborhood of a pair of contact points. +pub struct ContactKinematics { + /// The local contact geometry. + pub category: KinematicsCategory, + /// The dilation applied to the first contact geometry. + pub radius1: f32, + /// The dilation applied to the second contact geometry. + pub radius2: f32, +} + +impl Default for ContactKinematics { + fn default() -> Self { + ContactKinematics { + category: KinematicsCategory::PointPoint, + radius1: 0.0, + radius2: 0.0, + } + } +} + +#[cfg(feature = "simd-is-enabled")] +pub(crate) struct WContact { + pub local_p1: Point, + pub local_p2: Point, + pub local_n1: Vector, + pub local_n2: Vector, + pub dist: SimdFloat, + pub fid1: [u8; SIMD_WIDTH], + pub fid2: [u8; SIMD_WIDTH