diff options
Diffstat (limited to 'src/geometry')
41 files changed, 2247 insertions, 712 deletions
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 8e69d24..4cea113 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,13 +1,10 @@ +use crate::data::hashmap::HashMap; use crate::data::pubsub::Subscription; use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; 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}; @@ -135,6 +132,7 @@ impl Endpoint { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] struct SAPAxis { min_bound: f32, max_bound: f32, @@ -240,13 +238,14 @@ impl SAPAxis { } } - fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> bool { - let mut deleted_any = false; + fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> usize { + let mut deleted = 0; 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; + let proxy_idx = endpoint.proxy() as usize; + if endpoint.is_end() && existing_proxies[proxy_idx] { + existing_proxies.set(proxy_idx, false); + deleted += 1; } } else { break; @@ -255,16 +254,17 @@ impl SAPAxis { for endpoint in self.endpoints.iter().rev() { if endpoint.value > self.max_bound { - if endpoint.is_start() { + let proxy_idx = endpoint.proxy() as usize; + if endpoint.is_start() && existing_proxies[proxy_idx] { existing_proxies.set(endpoint.proxy() as usize, false); - deleted_any = true; + deleted += 1; } } else { break; } } - deleted_any + deleted } fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) { @@ -334,27 +334,58 @@ impl SAPAxis { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] struct SAPRegion { - axii: [SAPAxis; DIM], + axes: [SAPAxis; DIM], existing_proxies: BitVec, #[cfg_attr(feature = "serde-serialize", serde(skip))] to_insert: Vec<usize>, // Workspace - need_update: bool, + update_count: u8, + proxy_count: usize, } impl SAPRegion { pub fn new(bounds: AABB<f32>) -> Self { - let axii = [ + let axes = [ 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, + axes, existing_proxies: BitVec::new(), to_insert: Vec::new(), - need_update: false, + update_count: 0, + proxy_count: 0, + } + } + + pub fn recycle(bounds: AABB<f32>, mut old: Self) -> Self { + // Correct the bounds + for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) { + axis.min_bound = bound; + } + for (axis, &bound) in old.axes.iter_mut().zip(bounds.maxs.iter()) { + axis.max_bound = bound; + } + + old.update_count = 0; + + // The rest of the fields should be "empty" + assert_eq!(old.proxy_count, 0); + assert!(old.to_insert.is_empty()); + debug_assert!(!old.existing_proxies.any()); + assert!(old.axes.iter().all(|ax| ax.endpoints.len() == 2)); + + old + } + + pub fn recycle_or_new(bounds: AABB<f32>, pool: &mut Vec<Self>) -> Self { + if let Some(old) = pool.pop() { + Self::recycle(bounds, old) + } else { + Self::new(bounds) } } @@ -362,7 +393,7 @@ impl SAPRegion { // 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; + self.update_count = 1; } pub fn preupdate_proxy(&mut self, proxy_id: usize) -> bool { @@ -374,51 +405,64 @@ impl SAPRegion { if !self.existing_proxies[proxy_id] { self.to_insert.push(proxy_id); self.existing_proxies.set(proxy_id, true); + self.proxy_count += 1; false } else { - self.need_update = true; + // Here we need a second update if all proxies exit this region. In this case, we need + // to delete the final proxy, but the region may not have AABBs overlapping it, so it + // wouldn't get an update otherwise. + self.update_count = 2; true } } pub fn update(&mut self, proxies: &Proxies, reporting: &mut HashMap<(u32, u32), bool>) { - if self.need_update { + if self.update_count > 0 { // Update endpoints. - let mut deleted_any = false; + let mut deleted = 0; + 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; + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies); } - if deleted_any { + if deleted > 0 { + self.proxy_count -= deleted; for dim in 0..DIM { - self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } } - self.need_update = false; + self.update_count -= 1; } 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.axes[dim].batch_insert(dim, &self.to_insert, proxies, None); } - self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); self.to_insert.clear(); + + // In the rare event that all proxies leave this region in the next step, we need an + // update to remove them. + self.update_count = 1; } } } /// 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))] +#[derive(Clone)] pub struct BroadPhase { proxies: Proxies, regions: HashMap<Point<i32>, SAPRegion>, removed_colliders: Option<Subscription<RemovedCollider>>, deleted_any: bool, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + region_pool: Vec<SAPRegion>, // To avoid repeated allocations. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + regions_to_remove: Vec<Point<i32>>, // Workspace // 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. @@ -433,14 +477,15 @@ pub struct BroadPhase { #[cfg_attr( feature = "serde-serialize", serde( - serialize_with = "crate::utils::serialize_hashmap_capacity", - deserialize_with = "crate::utils::deserialize_hashmap_capacity" + serialize_with = "crate::data::hashmap::serialize_hashmap_capacity", + deserialize_with = "crate::data::hashmap::deserialize_hashmap_capacity" ) )] reporting: HashMap<(u32, u32), bool>, // Workspace } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub(crate) struct BroadPhaseProxy { handle: ColliderHandle, aabb: AABB<f32>, @@ -448,6 +493,7 @@ pub(crate) struct BroadPhaseProxy { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] struct Proxies { elements: Vec<BroadPhaseProxy>, first_free: u32, @@ -509,6 +555,8 @@ impl BroadPhase { removed_colliders: None, proxies: Proxies::new(), regions: HashMap::default(), + region_pool: Vec::new(), + regions_to_remove: Vec::new(), reporting: HashMap::default(), deleted_any: false, } @@ -609,15 +657,17 @@ impl BroadPhase { let start = point_key(aabb.mins); let end = point_key(aabb.maxs); + let regions = &mut self.regions; + let pool = &mut self.region_pool; + #[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 + let region = regions .entry(region_key) - .or_insert_with(|| SAPRegion::new(region_bounds)); + .or_insert_with(|| SAPRegion::recycle_or_new(region_bounds, pool)); let _ = region.preupdate_proxy(proxy_id); } } @@ -628,10 +678,9 @@ impl BroadPhase { 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 + let region = regions .entry(region_key) - .or_insert_with(|| SAPRegion::new(region_bounds)); + .or_insert_with(|| SAPRegion::recycle_or_new(region_bounds, pool)); let _ = region.preupdate_proxy(proxy_id); } } @@ -640,11 +689,26 @@ impl BroadPhase { } } + fn update_regions(&mut self) { + for (point, region) in &mut self.regions { + region.update(&self.proxies, &mut self.reporting); + if region.proxy_count == 0 { + self.regions_to_remove.push(*point); + } + } + + // Remove all the empty regions and store them in the region pool + let regions = &mut self.regions; + self.region_pool.extend( + self.regions_to_remove + .drain(..) + .map(|p| regions.remove(&p).unwrap()), + ); + } + pub(crate) fn complete_removals(&mut self) { if self.deleted_any { - for (_, region) in &mut self.regions { - region.update(&self.proxies, &mut self.reporting); - } + self.update_regions(); // NOTE: we don't care about reporting pairs. self.reporting.clear(); @@ -656,9 +720,7 @@ impl BroadPhase { // println!("num regions: {}", self.regions.len()); self.reporting.clear(); - for (_, region) in &mut self.regions { - region.update(&self.proxies, &mut self.reporting) - } + self.update_regions(); // Convert reports to broad phase events. // let t = instant::now(); diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs index 0d754af..54736cc 100644 --- a/src/geometry/capsule.rs +++ b/src/geometry/capsule.rs @@ -1,18 +1,16 @@ -use crate::geometry::AABB; +use crate::geometry::{Ray, RayIntersection, AABB}; use crate::math::{Isometry, Point, Rotation, Vector}; use approx::AbsDiffEq; use na::Unit; -use ncollide::query::{PointProjection, PointQuery}; -use ncollide::shape::{FeatureId, Segment}; +use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; +use ncollide::shape::{FeatureId, Segment, SupportMap}; #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a segment with a radius. +/// A capsule shape defined as a round segment. pub struct Capsule { - /// The first endpoint of the capsule. - pub a: Point<f32>, - /// The second enpdoint of the capsule. - pub b: Point<f32>, + /// The axis and endpoint of the capsule. + pub segment: Segment<f32>, /// The radius of the capsule. pub radius: f32, } @@ -39,13 +37,14 @@ impl Capsule { /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self { - Self { a, b, radius } + let segment = Segment::new(a, b); + Self { segment, radius } } /// The axis-aligned bounding box of this capsule. pub fn aabb(&self, pos: &Isometry<f32>) -> AABB { - let a = pos * self.a; - let b = pos * self.b; + let a = pos * self.segment.a; + let b = pos * self.segment.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()) @@ -53,7 +52,7 @@ impl Capsule { /// The height of this capsule. pub fn height(&self) -> f32 { - (self.b - self.a).norm() + (self.segment.b - self.segment.a).norm() } /// The half-height of this capsule. @@ -63,17 +62,17 @@ impl Capsule { /// The center of this capsule. pub fn center(&self) -> Point<f32> { - na::center(&self.a, &self.b) + na::center(&self.segment.a, &self.segment.b) } /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. pub fn transform_by(&self, pos: &Isometry<f32>) -> Self { - Self::new(pos * self.a, pos * self.b, self.radius) + Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) } /// The rotation `r` such that `r * Y` is collinear with `b - a`. pub fn rotation_wrt_y(&self) -> Rotation<f32> { - let mut dir = self.b - self.a; + let mut dir = self.segment.b - self.segment.a; if dir.y < 0.0 { dir = -dir; } @@ -96,24 +95,49 @@ impl Capsule { } } -// impl SupportMap<f32> 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<Vector>) -> Point { -// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) { -// self.a + **dir * self.radius -// } else { -// self.b + **dir * self.radius -// } -// } -// } +impl SupportMap<f32> for Capsule { + fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> { + 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<Vector<f32>>) -> Point<f32> { + if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { + self.segment.a + **dir * self.radius + } else { + self.segment.b + **dir * self.radius + } + } +} + +impl RayCast<f32> for Capsule { + fn toi_and_normal_with_ray( + &self, + m: &Isometry<f32>, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option<RayIntersection> { + let ls_ray = ray.inverse_transform_by(m); + + ncollide::query::ray_intersection_with_support_map_with_params( + &Isometry::identity(), + self, + &mut VoronoiSimplex::new(), + &ls_ray, + max_toi, + solid, + ) + .map(|mut res| { + res.normal = m * res.normal; + res + }) + } +} // 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. +// We should find a way to avoid this code duplication. impl PointQuery<f32> for Capsule { #[inline] fn project_point( @@ -122,7 +146,7 @@ impl PointQuery<f32> for Capsule { pt: &Point<f32>, solid: bool, ) -> PointProjection<f32> { - let seg = Segment::new(self.a, self.b); + let seg = Segment::new(self.segment.a, self.segment.b); let proj = seg.project_point(m, pt, solid); let dproj = *pt - proj.point; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 7c293b6..c2adc59 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,154 +1,199 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, + InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; -use ncollide::query::RayCast; -use num::Zero; +use ncollide::bounding_volume::AABB; +use std::ops::Deref; +use std::sync::Arc; +/// The shape of a collider. #[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), +pub struct ColliderShape(pub Arc<dyn Shape>); + +impl Deref for ColliderShape { + type Target = dyn Shape; + fn deref(&self) -> &dyn Shape { + &*self.0 + } } -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, - } +impl ColliderShape { + /// Initialize a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + ColliderShape(Arc::new(Ball::new(radius))) } - /// 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, - } + /// Initialize a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] + pub fn cylinder(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// 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, - } + /// Initialize a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self { + ColliderShape(Arc::new(RoundCylinder::new( + half_height, + radius, + border_radius, + ))) } - /// 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, - } + /// Initialize a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cone::new(half_height, radius))) } - /// 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, - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector<f32>) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// 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, - } + /// Initialize a capsule shape from its endpoints and radius. + pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(a, b, radius))) } - /// 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), - |