From 3c85a6ac41397cf95199933c6a93909bc070a844 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 8 Sep 2020 21:18:17 +0200 Subject: Start implementing ray-casting. This adds a QueryPipeline structure responsible for scene queries. Currently this structure is able to perform a brute-force ray-cast. This commit also includes the beginning of implementation of a SIMD-based acceleration structure which will be used for these scene queries in the future. --- src/geometry/collider.rs | 49 ++++++++++++++++++++++++- src/geometry/mod.rs | 6 ++-- src/geometry/waabb.rs | 93 ++++++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 143 insertions(+), 5 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 2d55857..183446b 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,11 +1,12 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::query::RayCast; use num::Zero; #[derive(Clone)] @@ -97,6 +98,46 @@ impl Shape { Shape::HeightField(heightfield) => heightfield.bounding_volume(position), } } + + /// Computes the first intersection point between a ray in this collider. + /// + /// Some shapes are not supported yet and will always return `None`. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + pub fn cast_ray( + &self, + position: &Isometry, + ray: &Ray, + max_toi: f32, + ) -> Option { + match self { + Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), + Shape::Polygon(_poly) => None, + Shape::Capsule(caps) => { + let pos = position * caps.transform_wrt_y(); + let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); + caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) + } + Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), + #[cfg(feature = "dim2")] + Shape::Triangle(triangle) => { + // This is not implemented yet in 2D. + None + } + #[cfg(feature = "dim3")] + Shape::Triangle(triangle) => { + triangle.toi_and_normal_with_ray(position, ray, max_toi, true) + } + Shape::Trimesh(_trimesh) => None, + Shape::HeightField(heightfield) => { + heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) + } + } + } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -353,6 +394,12 @@ impl ColliderBuilder { self } + /// Sets the restitution coefficient of the collider this builder will build. + pub fn restitution(mut self, restitution: f32) -> Self { + self.restitution = restitution; + self + } + /// Sets the density of the collider this builder will build. pub fn density(mut self, density: f32) -> Self { self.density = Some(density); diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4f72778..5fcdf71 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -36,6 +36,10 @@ pub type AABB = ncollide::bounding_volume::AABB; pub type ContactEvent = ncollide::pipeline::ContactEvent; /// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not). pub type ProximityEvent = ncollide::pipeline::ProximityEvent; +/// A ray that can be cast against colliders. +pub type Ray = ncollide::query::Ray; +/// The intersection between a ray and a collider. +pub type RayIntersection = ncollide::query::RayIntersection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; @@ -48,7 +52,6 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] pub(crate) use self::polyhedron_feature3d::PolyhedronFace; -#[cfg(feature = "simd-is-enabled")] pub(crate) use self::waabb::WAABB; //pub(crate) use self::z_order::z_cmp_floats; @@ -75,6 +78,5 @@ mod proximity_detector; pub(crate) mod sat; pub(crate) mod triangle; mod trimesh; -#[cfg(feature = "simd-is-enabled")] mod waabb; //mod z_order; diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index c3853bc..702b5aa 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,15 +1,27 @@ #[cfg(feature = "serde-serialize")] use crate::math::DIM; -use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH}; +use crate::math::{Point, SIMD_WIDTH}; use ncollide::bounding_volume::AABB; -use simba::simd::{SimdPartialOrd, SimdValue}; +#[cfg(feature = "simd-is-enabled")] +use { + crate::math::{SimdBool, SimdFloat}, + simba::simd::{SimdPartialOrd, SimdValue}, +}; #[derive(Debug, Copy, Clone)] +#[cfg(feature = "simd-is-enabled")] pub(crate) struct WAABB { pub mins: Point, pub maxs: Point, } +#[derive(Debug, Copy, Clone)] +#[cfg(not(feature = "simd-is-enabled"))] +pub(crate) struct WAABB { + pub mins: [Point; SIMD_WIDTH], + pub maxs: [Point; SIMD_WIDTH], +} + #[cfg(feature = "serde-serialize")] impl serde::Serialize for WAABB { fn serialize(&self, serializer: S) -> Result @@ -18,16 +30,24 @@ impl serde::Serialize for WAABB { { use serde::ser::SerializeStruct; + #[cfg(feature = "simd-is-enabled")] let mins: Point<[f32; SIMD_WIDTH]> = Point::from( self.mins .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); + #[cfg(feature = "simd-is-enabled")] let maxs: Point<[f32; SIMD_WIDTH]> = Point::from( self.maxs .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); + + #[cfg(not(feature = "simd-is-enabled"))] + let mins = self.mins; + #[cfg(not(feature = "simd-is-enabled"))] + let maxs = self.maxs; + let mut waabb = serializer.serialize_struct("WAABB", 2)?; waabb.serialize_field("mins", &mins)?; waabb.serialize_field("maxs", &maxs)?; @@ -52,6 +72,7 @@ impl<'de> serde::Deserialize<'de> for WAABB { ) } + #[cfg(feature = "simd-is-enabled")] fn visit_seq(self, mut seq: A) -> Result where A: serde::de::SeqAccess<'de>, @@ -66,17 +87,36 @@ impl<'de> serde::Deserialize<'de> for WAABB { let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e))); Ok(WAABB { mins, maxs }) } + + #[cfg(not(feature = "simd-is-enabled"))] + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let mins = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + let maxs = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; + Ok(WAABB { mins, maxs }) + } } deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {}) } } +#[cfg(feature = "simd-is-enabled")] impl WAABB { pub fn new(mins: Point, maxs: Point) -> Self { Self { mins, maxs } } + pub fn new_invalid() -> Self { + Self::splat(AABB::new_invalid()) + } + pub fn splat(aabb: AABB) -> Self { Self { mins: Point::splat(aabb.mins), @@ -103,6 +143,7 @@ impl WAABB { } } +#[cfg(feature = "simd-is-enabled")] impl From<[AABB; SIMD_WIDTH]> for WAABB { fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; @@ -114,3 +155,51 @@ impl From<[AABB; SIMD_WIDTH]> for WAABB { } } } + +#[cfg(not(feature = "simd-is-enabled"))] +impl WAABB { + pub fn new_invalid() -> Self { + Self::splat(AABB::new_invalid()) + } + + pub fn splat(aabb: AABB) -> Self { + Self { + mins: [aabb.mins; SIMD_WIDTH], + maxs: [aabb.maxs; SIMD_WIDTH], + } + } + + #[cfg(feature = "dim2")] + pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] { + array![|ii| + self.mins[ii].x <= other.maxs[ii].x + && other.mins[ii].x <= self.maxs[ii].x + && self.mins[ii].y <= other.maxs[ii].y + && other.mins[ii].y <= self.maxs[ii].y + ; SIMD_WIDTH + ] + } + + #[cfg(feature = "dim3")] + pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] { + array![|ii| + self.mins[ii].x <= other.maxs[ii].x + && other.mins[ii].x <= self.maxs[ii].x + && self.mins[ii].y <= other.maxs[ii].y + && other.mins[ii].y <= self.maxs[ii].y + && self.mins[ii].z <= other.maxs[ii].z + && other.mins[ii].z <= self.maxs[ii].z + ; SIMD_WIDTH + ] + } +} + +#[cfg(not(feature = "simd-is-enabled"))] +impl From<[AABB; SIMD_WIDTH]> for WAABB { + fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { + let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; + let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; + + WAABB { mins, maxs } + } +} -- cgit From 7b8e322446ffa36e3f47078e23eb61ef423175dc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 10:43:20 +0200 Subject: Make kinematic bodies properly wake up dynamic bodies. --- src/geometry/narrow_phase.rs | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 1a36511..016da33 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -87,11 +87,11 @@ impl NarrowPhase { // Wake up every body in contact with the deleted collider. for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) { if let Some(parent) = colliders.get(a).map(|c| c.parent) { - bodies.wake_up(parent) + bodies.wake_up(parent, true) } if let Some(parent) = colliders.get(b).map(|c| c.parent) { - bodies.wake_up(parent) + bodies.wake_up(parent, true) } } @@ -119,6 +119,7 @@ impl NarrowPhase { pub(crate) fn register_pairs( &mut self, colliders: &mut ColliderSet, + bodies: &mut RigidBodySet, broad_phase_events: &[BroadPhasePairEvent], events: &dyn EventHandler, ) { @@ -218,9 +219,13 @@ impl NarrowPhase { .contact_graph .remove_edge(co1.contact_graph_index, co2.contact_graph_index); - // Emit a contact stopped event if we had a proximity before removing the edge. + // Emit a contact stopped event if we had a contact before removing the edge. + // Also wake up the dynamic bodies that were in contact. if let Some(ctct) = contact_pair { if ctct.has_any_active_contact() { + bodies.wake_up(co1.parent, true); + bodies.wake_up(co2.parent, true); + events.handle_contact_event(ContactEvent::Stopped( pair.collider1, pair.collider2, @@ -250,8 +255,7 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) - { + if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) { // No need to update this contact because nothing moved. return; } @@ -359,7 +363,8 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) + if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static())) + || (!rb1.is_dynamic() && !rb2.is_dynamic()) { // No need to update this contact because nothing moved. return; -- cgit From 2dda0e5ce48ed0d93b4b0fa3098ba08f59a50a0a Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 17:26:57 +0200 Subject: Complete the WQuadtree construction and ray-cast. --- src/geometry/mod.rs | 4 +- src/geometry/waabb.rs | 64 ++++++++++- src/geometry/wquadtree.rs | 285 ++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 351 insertions(+), 2 deletions(-) create mode 100644 src/geometry/wquadtree.rs (limited to 'src/geometry') diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 5fcdf71..406727f 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -52,7 +52,8 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] pub(crate) use self::polyhedron_feature3d::PolyhedronFace; -pub(crate) use self::waabb::WAABB; +pub(crate) use self::waabb::{WRay, WAABB}; +pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; mod ball; @@ -79,4 +80,5 @@ pub(crate) mod sat; pub(crate) mod triangle; mod trimesh; mod waabb; +mod wquadtree; //mod z_order; diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 702b5aa..0664a50 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,13 +1,39 @@ +use crate::geometry::Ray; #[cfg(feature = "serde-serialize")] use crate::math::DIM; -use crate::math::{Point, SIMD_WIDTH}; +use crate::math::{Point, Vector, SIMD_WIDTH}; +use crate::utils; use ncollide::bounding_volume::AABB; +use num::{One, Zero}; #[cfg(feature = "simd-is-enabled")] use { crate::math::{SimdBool, SimdFloat}, simba::simd::{SimdPartialOrd, SimdValue}, }; +#[derive(Debug, Copy, Clone)] +#[cfg(feature = "simd-is-enabled")] +pub(crate) struct WRay { + pub origin: Point, + pub dir: Vector, +} + +impl WRay { + pub fn splat(ray: Ray) -> Self { + Self { + origin: Point::splat(ray.origin), + dir: Vector::splat(ray.dir), + } + } +} + +#[derive(Debug, Copy, Clone)] +#[cfg(not(feature = "simd-is-enabled"))] +pub(crate) struct WRay { + pub origin: [Point; SIMD_WIDTH], + pub dir: [Vector; SIMD_WIDTH], +} + #[derive(Debug, Copy, Clone)] #[cfg(feature = "simd-is-enabled")] pub(crate) struct WAABB { @@ -124,6 +150,42 @@ impl WAABB { } } + pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool { + let _0 = SimdFloat::zero(); + let _1 = SimdFloat::one(); + let _infinity = SimdFloat::splat(f32::MAX); + + let mut hit = SimdBool::splat(true); + let mut tmin = SimdFloat::zero(); + let mut tmax = max_toi; + + // TODO: could this be optimized more considering we really just need a boolean answer? + for i in 0usize..DIM { + let is_not_zero = ray.dir[i].simd_ne(_0); + let is_zero_test = + (ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i])); + let is_not_zero_test = { + let denom = _1 / ray.dir[i]; + let mut inter_with_near_plane = + ((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -_infinity); + let mut inter_with_far_plane = + ((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, _infinity); + + let gt = inter_with_near_plane.simd_gt(inter_with_far_plane); + utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane); + + tmin = tmin.simd_max(inter_with_near_plane); + tmax = tmax.simd_min(inter_with_far_plane); + + tmin.simd_le(tmax) + }; + + hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test); + } + + hit + } + #[cfg(feature = "dim2")] pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs new file mode 100644 index 0000000..4e3bf54 --- /dev/null +++ b/src/geometry/wquadtree.rs @@ -0,0 +1,285 @@ +use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB}; +use crate::geometry::{WRay, WAABB}; +use crate::math::{Point, Vector}; +use crate::simd::{SimdFloat, SIMD_WIDTH}; +use ncollide::bounding_volume::BoundingVolume; +use simba::simd::{SimdBool, SimdValue}; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +struct NodeIndex { + index: u32, // Index of the addressed node in the `nodes` array. + lane: u8, // SIMD lane of the addressed node. +} + +impl NodeIndex { + fn new(index: u32, lane: u8) -> Self { + Self { index, lane } + } + + fn invalid() -> Self { + Self { + index: u32::MAX, + lane: 0, + } + } +} + +#[derive(Copy, Clone, Debug)] +struct WQuadtreeNodeChildren { + waabb: WAABB, + // Index of the nodes of the 4 nodes represented by self. + // If this is a leaf, it contains the proxy ids instead. + children: [u32; 4], + parent: NodeIndex, + leaf: bool, // TODO: pack this with the NodexIndex.lane? +} + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +struct ColliderNodeIndex { + node: NodeIndex, + handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? +} + +impl ColliderNodeIndex { + fn invalid() -> Self { + Self { + node: NodeIndex::invalid(), + handle: ColliderSet::invalid_handle(), + } + } +} + +pub struct WQuadtree { + nodes: Vec, + dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. + proxies: Vec, +} + +impl WQuadtree { + pub fn new() -> Self { + WQuadtree { + nodes: Vec::new(), + dirty: Vec::new(), + proxies: Vec::new(), + } + } + + pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) { + self.nodes.clear(); + self.dirty.clear(); + self.proxies.clear(); + + // Create proxies. + let mut indices = Vec::with_capacity(colliders.len()); + self.proxies = vec![ColliderNodeIndex::invalid(); colliders.len()]; + + for (handle, collider) in colliders.iter() { + let index = handle.into_raw_parts().0; + if self.proxies.len() < index { + self.proxies.resize(index + 1, ColliderNodeIndex::invalid()); + } + + self.proxies[index].handle = handle; + indices.push(index); + } + + // Compute AABBs. + let mut aabbs = vec![AABB::new_invalid(); self.proxies.len()]; + for (handle, collider) in colliders.iter() { + let index = handle.into_raw_parts().0; + let aabb = collider.compute_aabb(); + aabbs[index] = aabb; + } + + // Build the tree recursively. + let root_node = WQuadtreeNodeChildren { + waabb: WAABB::new_invalid(), + children: [1, u32::MAX, u32::MAX, u32::MAX], + parent: NodeIndex::invalid(), + leaf: false, + }; + + self.nodes.push(root_node); + let root_id = NodeIndex::new(0, 0); + let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id); + self.nodes[0].waabb = WAABB::from([ + aabb, + AABB::new_invalid(), + AABB::new_invalid(), + AABB::new_invalid(), + ]); + } + + fn do_recurse_build( + &mut self, + indices: &mut [usize], + aabbs: &[AABB], + parent: NodeIndex, + ) -> (u32, AABB) { + // Leaf case. + if indices.len() <= 4 { + let my_id = self.nodes.len(); + let mut my_aabb = AABB::new_invalid(); + let mut leaf_aabbs = [AABB::new_invalid(); 4]; + let mut proxy_ids = [u32::MAX; 4]; + + for (k, id) in indices.iter().enumerate() { + my_aabb.merge(&aabbs[*id]); + leaf_aabbs[k] = aabbs[*id]; + proxy_ids[k] = *id as u32; + } + + let node = WQuadtreeNodeChildren { + waabb: WAABB::from(leaf_aabbs), + children: proxy_ids, + parent, + leaf: true, + }; + + self.nodes.push(node); + return (my_id as u32, my_aabb); + } + + // Compute the center and variance along each dimension. + // In 3D we compute the variance to not-subdivide the dimension with lowest variance. + // Therefore variance computation is not needed in 2D because we only have 2 dimension + // to split in the first place. + let mut center = Point::origin(); + #[cfg(feature = "dim3")] + let mut variance = Vector::zeros(); + + let denom = 1.0 / (indices.len() as f32); + + for i in &*indices { + let coords = aabbs[*i].center().coords; + center += coords * denom; + #[cfg(feature = "dim3")] + { + variance += coords.component_mul(&coords) * denom; + } + } + + #[cfg(feature = "dim3")] + { + variance = variance - center.coords.component_mul(¢er.coords); + } + + // Find the axis with minimum variance. This is the axis along + // which we are **not** subdividing our set. + let mut subdiv_dims = [0, 1]; + #[cfg(feature = "dim3")] + { + let min = variance.imin(); + subdiv_dims[0] = (min + 1) % 3; + subdiv_dims[1] = (min + 2) % 3; + } + + // Split the set along the two subdiv_dims dimensions. + // TODO: should we split wrt. the median instead of the average? + // TODO: we should ensure each subslice contains at least 4 elements each (or less if + // indices has less than 16 elements in the first place. + let (left, right) = split_indices_wrt_dim(indices, &aabbs, ¢er, subdiv_dims[0]); + + let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, ¢er, subdiv_dims[1]); + let (right_bottom, right_top) = + split_indices_wrt_dim(right, &aabbs, ¢er, subdiv_dims[1]); + + // println!( + // "Recursing on children: {}, {}, {}, {}", + // left_bottom.len(), + // left_top.len(), + // right_bottom.len(), + // right_top.len() + // ); + + let node = WQuadtreeNodeChildren { + waabb: WAABB::new_invalid(), + children: [0; 4], // Will be set after the recursive call + parent, + leaf: false, + }; + + let id = self.nodes.len() as u32; + self.nodes.push(node); + + // Recurse! + let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0)); + let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1)); + let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2)); + let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3)); + + // Now we know the indices of the grand-nodes. + self.nodes[id as usize].children = [a.0, b.0, c.0, d.0]; + self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]); + + // TODO: will this chain of .merged be properly optimized? + let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1); + (id, my_aabb) + } + + pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { + let mut res = Vec::new(); + + if self.nodes.is_empty() { + return res; + } + + // Special case for the root. + let mut stack = vec![0u32]; + let wray = WRay::splat(*ray); + let wmax_toi = SimdFloat::splat(max_toi); + while let Some(inode) = stack.pop() { + let node = self.nodes[inode as usize]; + let hits = node.waabb.intersects_ray(&wray, wmax_toi); + let bitmask = hits.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 { + if node.leaf { + // We found a leaf! + // Unfortunately, invalid AABBs return a hit as well. + if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { + res.push(proxy.handle); + } + } else { + // Internal node, visit the child. + // Un fortunately, we have this check because invalid AABBs + // return a hit as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push(node.children[ii]); + } + } + } + } + } + + res + } +} + +fn split_indices_wrt_dim<'a>( + indices: &'a mut [usize], + aabbs: &[AABB], + split_point: &Point, + dim: usize, +) -> (&'a mut [usize], &'a mut [usize]) { + let mut icurr = 0; + let mut ilast = indices.len() - 1; + + // The loop condition we can just do 0..indices.len() + // instead of the test icurr < ilast because we know + // we will iterate exactly once per index. + for _ in 0..indices.len() { + let i = indices[icurr]; + let center = aabbs[i].center(); + + if center[dim] > split_point[dim] { + indices.swap(icurr, ilast); + ilast -= 1; + } else { + icurr += 1; + } + } + + indices.split_at_mut(icurr) +} -- cgit From 56f6051b047aded906b8a89cbc66672c6f1e698e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 18:29:32 +0200 Subject: Start adding incremental topology update for the WQuadtree. --- src/geometry/waabb.rs | 5 ++ src/geometry/wquadtree.rs | 155 ++++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 155 insertions(+), 5 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 0664a50..c04514a 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -150,6 +150,11 @@ impl WAABB { } } + pub fn replace(&mut self, i: usize, aabb: AABB) { + self.mins.replace(i, aabb.mins); + self.maxs.replace(i, aabb.maxs); + } + pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool { let _0 = SimdFloat::zero(); let _1 = SimdFloat::one(); diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 4e3bf54..8fc1163 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -4,6 +4,7 @@ use crate::math::{Point, Vector}; use crate::simd::{SimdFloat, SIMD_WIDTH}; use ncollide::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; +use std::ops::Range; #[derive(Copy, Clone, Debug, PartialEq, Eq)] struct NodeIndex { @@ -25,7 +26,7 @@ impl NodeIndex { } #[derive(Copy, Clone, Debug)] -struct WQuadtreeNodeChildren { +struct WQuadtreeNode { waabb: WAABB, // Index of the nodes of the 4 nodes represented by self. // If this is a leaf, it contains the proxy ids instead. @@ -50,7 +51,7 @@ impl ColliderNodeIndex { } pub struct WQuadtree { - nodes: Vec, + nodes: Vec, dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. proxies: Vec, } @@ -92,7 +93,7 @@ impl WQuadtree { } // Build the tree recursively. - let root_node = WQuadtreeNodeChildren { + let root_node = WQuadtreeNode { waabb: WAABB::new_invalid(), children: [1, u32::MAX, u32::MAX, u32::MAX], parent: NodeIndex::invalid(), @@ -129,7 +130,7 @@ impl WQuadtree { proxy_ids[k] = *id as u32; } - let node = WQuadtreeNodeChildren { + let node = WQuadtreeNode { waabb: WAABB::from(leaf_aabbs), children: proxy_ids, parent, @@ -192,7 +193,7 @@ impl WQuadtree { // right_top.len() // ); - let node = WQuadtreeNodeChildren { + let node = WQuadtreeNode { waabb: WAABB::new_invalid(), children: [0; 4], // Will be set after the recursive call parent, @@ -257,6 +258,150 @@ impl WQuadtree { } } +struct WQuadtreeIncrementalBuilderStep { + range: Range, + parent: NodeIndex, +} + +struct WQuadtreeIncrementalBuilder { + quadtree: WQuadtree, + to_insert: Vec, + aabbs: Vec, + indices: Vec, +} + +impl WQuadtreeIncrementalBuilder { + pub fn new() -> Self { + Self { + quadtree: WQuadtree::new(), + to_insert: Vec::new(), + aabbs: Vec::new(), + indices: Vec::new(), + } + } + + pub fn update_single_depth(&mut self) { + if let Some(to_insert) = self.to_insert.pop() { + let indices = &mut self.indices[to_insert.range]; + + // Leaf case. + if indices.len() <= 4 { + let id = self.quadtree.nodes.len(); + let mut aabb = AABB::new_invalid(); + let mut leaf_aabbs = [AABB::new_invalid(); 4]; + let mut proxy_ids = [u32::MAX; 4]; + + for (k, id) in indices.iter().enumerate() { + aabb.merge(&self.aabbs[*id]); + leaf_aabbs[k] = self.aabbs[*id]; + proxy_ids[k] = *id as u32; + } + + let node = WQuadtreeNode { + waabb: WAABB::from(leaf_aabbs), + children: proxy_ids, + parent: to_insert.parent, + leaf: true, + }; + + self.quadtree.nodes[to_insert.parent.index as usize].children + [to_insert.parent.lane as usize] = id as u32; + self.quadtree.nodes[to_insert.parent.index as usize] + .waabb + .replace(to_insert.parent.lane as usize, aabb); + self.quadtree.nodes.push(node); + return; + } + + // Compute the center and variance along each dimension. + // In 3D we compute the variance to not-subdivide the dimension with lowest variance. + // Therefore variance computation is not needed in 2D because we only have 2 dimension + // to split in the first place. + let mut center = Point::origin(); + #[cfg(feature = "dim3")] + let mut variance = Vector::zeros(); + + let denom = 1.0 / (indices.len() as f32); + let mut aabb = AABB::new_invalid(); + + for i in &*indices { + let coords = self.aabbs[*i].center().coords; + aabb.merge(&self.aabbs[*i]); + center += coords * denom; + #[cfg(feature = "dim3")] + { + variance += coords.component_mul(&coords) * denom; + } + } + + #[cfg(feature = "dim3")] + { + variance = variance - center.coords.component_mul(¢er.coords); + } + + // Find the axis with minimum variance. This is the axis along + // which we are **not** subdividing our set. + let mut subdiv_dims = [0, 1]; + #[cfg(feature = "dim3")] + { + let min = variance.imin(); + subdiv_dims[0] = (min + 1) % 3; + subdiv_dims[1] = (min + 2) % 3; + } + + // Split the set along the two subdiv_dims dimensions. + // TODO: should we split wrt. the median instead of the average? + // TODO: we should ensure each subslice contains at least 4 elements each (or less if + // indices has less than 16 elements in the first place. + let (left, right) = + split_indices_wrt_dim(indices, &self.aabbs, ¢er, subdiv_dims[0]); + + let (left_bottom, left_top) = + split_indices_wrt_dim(left, &self.aabbs, ¢er, subdiv_dims[1]); + let (right_bottom, right_top) = + split_indices_wrt_dim(right, &self.aabbs, ¢er, subdiv_dims[1]); + + let node = WQuadtreeNode { + waabb: WAABB::new_invalid(), + children: [0; 4], // Will be set after the recursive call + parent: to_insert.parent, + leaf: false, + }; + + let id = self.quadtree.nodes.len() as u32; + self.quadtree.nodes.push(node); + + // Recurse! + let a = left_bottom.len(); + let b = a + left_top.len(); + let c = b + right_bottom.len(); + let d = c + right_top.len(); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: 0..a, + parent: NodeIndex::new(id, 0), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: a..b, + parent: NodeIndex::new(id, 1), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: b..c, + parent: NodeIndex::new(id, 2), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: c..d, + parent: NodeIndex::new(id, 3), + }); + + self.quadtree.nodes[to_insert.parent.index as usize].children + [to_insert.parent.lane as usize] = id as u32; + self.quadtree.nodes[to_insert.parent.index as usize] + .waabb + .replace(to_insert.parent.lane as usize, aabb); + } + } +} + fn split_indices_wrt_dim<'a>( indices: &'a mut [usize], aabbs: &[AABB], -- cgit From a7d77a01447d2b77694b2a957d000790af60b383 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 22 Sep 2020 15:29:29 +0200 Subject: Add non-topological WQuadtree update. --- src/geometry/waabb.rs | 31 ++++++++++++++++ src/geometry/wquadtree.rs | 93 ++++++++++++++++++++++++++++++++++++++--------- 2 files changed, 107 insertions(+), 17 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index c04514a..0a90801 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -150,6 +150,12 @@ impl WAABB { } } + pub fn dilate_by_factor(&mut self, factor: SimdFloat) { + let dilation = (self.maxs - self.mins) * factor; + self.mins -= dilation; + self.maxs += dilation; + } + pub fn replace(&mut self, i: usize, aabb: AABB) { self.mins.replace(i, aabb.mins); self.maxs.replace(i, aabb.maxs); @@ -191,6 +197,24 @@ impl WAABB { hit } + #[cfg(feature = "dim2")] + pub fn contains_lanewise(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.mins.x) + & self.mins.y.simd_le(other.mins.y) + & self.maxs.x.simd_ge(other.maxs.x) + & self.maxs.y.simd_ge(other.maxs.y) + } + + #[cfg(feature = "dim3")] + pub fn contains_lanewise(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.mins.x) + & self.mins.y.simd_le(other.mins.y) + & self.mins.z.simd_le(other.mins.z) + & self.maxs.x.simd_ge(other.maxs.x) + & self.maxs.y.simd_ge(other.maxs.y) + & self.maxs.z.simd_ge(other.maxs.z) + } + #[cfg(feature = "dim2")] pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) @@ -208,6 +232,13 @@ impl WAABB { & self.mins.z.simd_le(other.maxs.z) & other.mins.z.simd_le(self.maxs.z) } + + pub fn to_merged_aabb(&self) -> AABB { + AABB::new( + self.mins.coords.map(|e| e.simd_horizontal_min()).into(), + self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), + ) + } } #[cfg(feature = "simd-is-enabled")] diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 8fc1163..b82a2c0 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -4,6 +4,7 @@ use crate::math::{Point, Vector}; use crate::simd::{SimdFloat, SIMD_WIDTH}; use ncollide::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; +use std::collections::VecDeque; use std::ops::Range; #[derive(Copy, Clone, Debug, PartialEq, Eq)] @@ -32,16 +33,17 @@ struct WQuadtreeNode { // If this is a leaf, it contains the proxy ids instead. children: [u32; 4], parent: NodeIndex, - leaf: bool, // TODO: pack this with the NodexIndex.lane? + leaf: bool, // TODO: pack this with the NodexIndex.lane? + dirty: bool, // TODO: move this to a separate bitvec? } #[derive(Copy, Clone, Debug, PartialEq, Eq)] -struct ColliderNodeIndex { +struct WQuadtreeProxy { node: NodeIndex, handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? } -impl ColliderNodeIndex { +impl WQuadtreeProxy { fn invalid() -> Self { Self { node: NodeIndex::invalid(), @@ -52,32 +54,77 @@ impl ColliderNodeIndex { pub struct WQuadtree { nodes: Vec, - dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. - proxies: Vec, + dirty_nodes: VecDeque, + proxies: Vec, } impl WQuadtree { pub fn new() -> Self { WQuadtree { nodes: Vec::new(), - dirty: Vec::new(), + dirty_nodes: VecDeque::new(), proxies: Vec::new(), } } - pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) { + pub fn pre_update(&mut self, handle: ColliderHandle) { + let id = handle.into_raw_parts().0; + let node_id = self.proxies[id].node.index; + let node = &mut self.nodes[node_id as usize]; + if !node.dirty { + node.dirty = true; + self.dirty_nodes.push_back(node_id); + } + } + + pub fn update(&mut self, colliders: &ColliderSet, dilation_factor: f32) { + // Loop on the dirty leaves. + let dilation_factor = SimdFloat::splat(dilation_factor); + + while let Some(id) = self.dirty_nodes.pop_front() { + // NOTE: this will handle the case where we reach the root of the tree. + if let Some(node) = self.nodes.get(id as usize) { + // Compute the new WAABB. + let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; + for (child_id, new_aabb) in node.children.iter().zip(new_aabbs.iter_mut()) { + if node.leaf { + // We are in a leaf: compute the colliders' AABBs. + if let Some(proxy) = self.proxies.get(*child_id as usize) { + let collider = &colliders[proxy.handle]; + *new_aabb = collider.compute_aabb(); + } + } else { + // We are in an internal node: compute the children's AABBs. + if let Some(node) = self.nodes.get(*child_id as usize) { + *new_aabb = node.waabb.to_merged_aabb(); + } + } + } + + let node = &mut self.nodes[id as usize]; + let new_waabb = WAABB::from(new_aabbs); + if !node.waabb.contains_lanewise(&new_waabb).all() { + node.waabb = new_waabb; + node.waabb.dilate_by_factor(dilation_factor); + self.dirty_nodes.push_back(node.parent.index); + } + node.dirty = false; + } + } + } + + pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet, dilation_factor: f32) { self.nodes.clear(); - self.dirty.clear(); self.proxies.clear(); // Create proxies. let mut indices = Vec::with_capacity(colliders.len()); - self.proxies = vec![ColliderNodeIndex::invalid(); colliders.len()]; + self.proxies = vec![WQuadtreeProxy::invalid(); colliders.len()]; for (handle, collider) in colliders.iter() { let index = handle.into_raw_parts().0; if self.proxies.len() < index { - self.proxies.resize(index + 1, ColliderNodeIndex::invalid()); + self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); } self.proxies[index].handle = handle; @@ -98,11 +145,12 @@ impl WQuadtree { children: [1, u32::MAX, u32::MAX, u32::MAX], parent: NodeIndex::invalid(), leaf: false, + dirty: false, }; self.nodes.push(root_node); let root_id = NodeIndex::new(0, 0); - let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id); + let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id, dilation_factor); self.nodes[0].waabb = WAABB::from([ aabb, AABB::new_invalid(), @@ -116,9 +164,10 @@ impl WQuadtree { indices: &mut [usize], aabbs: &[AABB], parent: NodeIndex, + dilation_factor: f32, ) -> (u32, AABB) { - // Leaf case. if indices.len() <= 4 { + // Leaf case. let my_id = self.nodes.len(); let mut my_aabb = AABB::new_invalid(); let mut leaf_aabbs = [AABB::new_invalid(); 4]; @@ -128,15 +177,19 @@ impl WQuadtree { my_aabb.merge(&aabbs[*id]); leaf_aabbs[k] = aabbs[*id]; proxy_ids[k] = *id as u32; + self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); } - let node = WQuadtreeNode { + let mut node = WQuadtreeNode { waabb: WAABB::from(leaf_aabbs), children: proxy_ids, parent, leaf: true, + dirty: false, }; + node.waabb + .dilate_by_factor(SimdFloat::splat(dilation_factor)); self.nodes.push(node); return (my_id as u32, my_aabb); } @@ -198,20 +251,24 @@ impl WQuadtree { children: [0; 4], // Will be set after the recursive call parent, leaf: false, + dirty: false, }; let id = self.nodes.len() as u32; self.nodes.push(node); // Recurse! - let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0)); - let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1)); - let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2)); - let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3)); + let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0), dilation_factor); + let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1), dilation_factor); + let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2), dilation_factor); + let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3), dilation_factor); // Now we know the indices of the grand-nodes. self.nodes[id as usize].children = [a.0, b.0, c.0, d.0]; self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]); + self.nodes[id as usize] + .waabb + .dilate_by_factor(SimdFloat::splat(dilation_factor)); // TODO: will this chain of .merged be properly optimized? let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1); @@ -302,6 +359,7 @@ impl WQuadtreeIncrementalBuilder { children: proxy_ids, parent: to_insert.parent, leaf: true, + dirty: false, }; self.quadtree.nodes[to_insert.parent.index as usize].children @@ -366,6 +424,7 @@ impl WQuadtreeIncrementalBuilder { children: [0; 4], // Will be set after the recursive call parent: to_insert.parent, leaf: false, + dirty: false, }; let id = self.quadtree.nodes.len() as u32; -- cgit From 84bd60e4a5b88c9aa824797c8a444945b46e96b2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 22 Sep 2020 17:57:29 +0200 Subject: Fix compilation when SIMD is not enabled. --- src/geometry/waabb.rs | 89 --------------------------------------------------- 1 file changed, 89 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 0a90801..4981373 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -5,14 +5,12 @@ use crate::math::{Point, Vector, SIMD_WIDTH}; use crate::utils; use ncollide::bounding_volume::AABB; use num::{One, Zero}; -#[cfg(feature = "simd-is-enabled")] use { crate::math::{SimdBool, SimdFloat}, simba::simd::{SimdPartialOrd, SimdValue}, }; #[derive(Debug, Copy, Clone)] -#[cfg(feature = "simd-is-enabled")] pub(crate) struct WRay { pub origin: Point, pub dir: Vector, @@ -28,26 +26,11 @@ impl WRay { } #[derive(Debug, Copy, Clone)] -#[cfg(not(feature = "simd-is-enabled"))] -pub(crate) struct WRay { - pub origin: [Point; SIMD_WIDTH], - pub dir: [Vector; SIMD_WIDTH], -} - -#[derive(Debug, Copy, Clone)] -#[cfg(feature = "simd-is-enabled")] pub(crate) struct WAABB { pub mins: Point, pub maxs: Point, } -#[derive(Debug, Copy, Clone)] -#[cfg(not(feature = "simd-is-enabled"))] -pub(crate) struct WAABB { - pub mins: [Point; SIMD_WIDTH], - pub maxs: [Point; SIMD_WIDTH], -} - #[cfg(feature = "serde-serialize")] impl serde::Serialize for WAABB { fn serialize(&self, serializer: S) -> Result @@ -56,24 +39,17 @@ impl serde::Serialize for WAABB { { use serde::ser::SerializeStruct; - #[cfg(feature = "simd-is-enabled")] let mins: Point<[f32; SIMD_WIDTH]> = Point::from( self.mins .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); - #[cfg(feature = "simd-is-enabled")] let maxs: Point<[f32; SIMD_WIDTH]> = Point::from( self.maxs .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); - #[cfg(not(feature = "simd-is-enabled"))] - let mins = self.mins; - #[cfg(not(feature = "simd-is-enabled"))] - let maxs = self.maxs; - let mut waabb = serializer.serialize_struct("WAABB", 2)?; waabb.serialize_field("mins", &mins)?; waabb.serialize_field("maxs", &maxs)?; @@ -98,7 +74,6 @@ impl<'de> serde::Deserialize<'de> for WAABB { ) } - #[cfg(feature = "simd-is-enabled")] fn visit_seq(self, mut seq: A) -> Result where A: serde::de::SeqAccess<'de>, @@ -113,27 +88,12 @@ impl<'de> serde::Deserialize<'de> for WAABB { let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e))); Ok(WAABB { mins, maxs }) } - - #[cfg(not(feature = "simd-is-enabled"))] - fn visit_seq(self, mut seq: A) -> Result - where - A: serde::de::SeqAccess<'de>, - { - let mins = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - let maxs = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; - Ok(WAABB { mins, maxs }) - } } deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {}) } } -#[cfg(feature = "simd-is-enabled")] impl WAABB { pub fn new(mins: Point, maxs: Point) -> Self { Self { mins, maxs } @@ -241,7 +201,6 @@ impl WAABB { } } -#[cfg(feature = "simd-is-enabled")] impl From<[AABB; SIMD_WIDTH]> for WAABB { fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; @@ -253,51 +212,3 @@ impl From<[AABB; SIMD_WIDTH]> for WAABB { } } } - -#[cfg(not(feature = "simd-is-enabled"))] -impl WAABB { - pub fn new_invalid() -> Self { - Self::splat(AABB::new_invalid()) - } - - pub fn splat(aabb: AABB) -> Self { - Self { - mins: [aabb.mins; SIMD_WIDTH], - maxs: [aabb.maxs; SIMD_WIDTH], - } - } - - #[cfg(feature = "dim2")] - pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] { - array![|ii| - self.mins[ii].x <= other.maxs[ii].x - && other.mins[ii].x <= self.maxs[ii].x - && self.mins[ii].y <= other.maxs[ii].y - && other.mins[ii].y <= self.maxs[ii].y - ; SIMD_WIDTH - ] - } - - #[cfg(feature = "dim3")] - pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] { - array![|ii| - self.mins[ii].x <= other.maxs[ii].x - && other.mins[ii].x <= self.maxs[ii].x - && self.mins[ii].y <= other.maxs[ii].y - && other.mins[ii].y <= self.maxs[ii].y - && self.mins[ii].z <= other.maxs[ii].z - && other.mins[ii].z <= self.maxs[ii].z - ; SIMD_WIDTH - ] - } -} - -#[cfg(not(feature = "simd-is-enabled"))] -impl From<[AABB; SIMD_WIDTH]> for WAABB { - fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { - let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; - let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; - - WAABB { mins, maxs } - } -} -- cgit From b39887a121305a36852604bf7c2726c1649934a1 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 22 Sep 2020 18:45:25 +0200 Subject: Fix crash of WQuadtree when the collider set contains holes. --- src/geometry/wquadtree.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/geometry') diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index b82a2c0..01b2092 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -123,7 +123,7 @@ impl WQuadtree { for (handle, collider) in colliders.iter() { let index = handle.into_raw_parts().0; - if self.proxies.len() < index { + if index >= self.proxies.len() { self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); } -- cgit From c031f96ac548645932c5605bfc17869618e9212b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 28 Sep 2020 10:04:56 +0200 Subject: Fix compilation when parallelism is not enabled. --- src/geometry/waabb.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 4981373..1706370 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,7 +1,5 @@ use crate::geometry::Ray; -#[cfg(feature = "serde-serialize")] -use crate::math::DIM; -use crate::math::{Point, Vector, SIMD_WIDTH}; +use crate::math::{Point, Vector, DIM, SIMD_WIDTH}; use crate::utils; use ncollide::bounding_volume::AABB; use num::{One, Zero}; -- cgit From bbfe926a11fdf4f4eb0ddbac0c777a202e07d269 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 16:51:32 +0200 Subject: Make the WQuadtree serializable. --- src/geometry/wquadtree.rs | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/geometry') diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 01b2092..17ecffe 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -8,6 +8,7 @@ use std::collections::VecDeque; use std::ops::Range; #[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct NodeIndex { index: u32, // Index of the addressed node in the `nodes` array. lane: u8, // SIMD lane of the addressed node. @@ -27,6 +28,7 @@ impl NodeIndex { } #[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct WQuadtreeNode { waabb: WAABB, // Index of the nodes of the 4 nodes represented by self. @@ -38,6 +40,7 @@ struct WQuadtreeNode { } #[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct WQuadtreeProxy { node: NodeIndex, handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? @@ -52,6 +55,7 @@ impl WQuadtreeProxy { } } +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct WQuadtree { nodes: Vec, dirty_nodes: VecDeque, -- cgit From 93aa7b6e1e8cbfd73542ed10ad5c26ae0a8b9848 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 19:04:18 +0200 Subject: Use the publish-subscribe mechanism to handle collider removals across pipelines. --- src/geometry/broad_phase_multi_sap.rs | 79 ++++++++++++++---------- src/geometry/collider_set.rs | 45 +++++++++++++- src/geometry/mod.rs | 1 + src/geometry/narrow_phase.rs | 110 ++++++++++++++++++++++++---------- 4 files changed, 168 insertions(+), 67 deletions(-) (limited to 'src/geometry') diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 054fccf..5c80e0e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,5 +1,6 @@ +use crate::data::pubsub::PubSubCursor; use crate::dynamics::RigidBodySet; -use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet}; +use crate::geometry::{Collider, ColliderHandle, ColliderPair, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; #[cfg(feature = "enhanced-determinism")] use crate::utils::FxHashMap32 as HashMap; @@ -381,6 +382,7 @@ impl SAPRegion { pub struct BroadPhase { proxies: Proxies, regions: HashMap, SAPRegion>, + removed_colliders: Option>, deleted_any: bool, // We could think serializing this workspace is useless. // It turns out is is important to serialize at least its capacity @@ -469,6 +471,7 @@ impl BroadPhase { /// Create a new empty broad-phase. pub fn new() -> Self { BroadPhase { + removed_colliders: None, proxies: Proxies::new(), regions: HashMap::default(), reporting: HashMap::default(), @@ -476,46 +479,60 @@ impl BroadPhase { } } - 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; - } + /// Maintain the broad-phase internal state by taking collider removal into account. + pub fn maintain(&mut self, colliders: &mut ColliderSet) { + // Ensure we already subscribed. + if self.removed_colliders.is_none() { + self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + } - let proxy = &mut self.proxies[collider.proxy_index]; + let mut cursor = self.removed_colliders.take().unwrap(); + for collider in colliders.removed_colliders.read(&cursor) { + self.remove_collider(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); + colliders.removed_colliders.ack(&mut cursor); + self.removed_colliders = Some(cursor); + } - #[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; - } + fn remove_collider<'a>(&mut self, proxy_index: usize) { + if proxy_index == crate::INVALID_USIZE { + // This collider has not been added to the broad-phase yet. + return; + } + + let proxy = &mut self.proxies[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(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; - } + #[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(proxy_index); + self.deleted_any = true; } } } - - self.proxies.remove(collider.proxy_index); } + + self.proxies.remove(proxy_index); } pub(crate) fn update_aabbs( diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 22bba1b..3a259ae 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,14 +1,25 @@ use crate::data::arena::Arena; +use crate::data::pubsub::PubSub; use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::Collider; +use crate::geometry::{Collider, ColliderGraphIndex}; use std::ops::{Index, IndexMut}; /// The unique identifier of a collider added to a collider set. pub type ColliderHandle = crate::data::arena::Index; +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct RemovedCollider { + pub handle: ColliderHandle, + pub(crate) contact_graph_index: ColliderGraphIndex, + pub(crate) proximity_graph_index: ColliderGraphIndex, + pub(crate) proxy_index: usize, +} + #[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) removed_colliders: PubSub, pub(crate) colliders: Arena, } @@ -16,6 +27,7 @@ impl ColliderSet { /// Create a new empty set of colliders. pub fn new() -> Self { ColliderSet { + removed_colliders: PubSub::new(), colliders: Arena::new(), } } @@ -60,8 +72,35 @@ impl ColliderSet { handle } - pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option { - self.colliders.remove(handle) + /// Remove a collider from this set and update its parent accordingly. + pub fn remove( + &mut self, + handle: ColliderHandle, + bodies: &mut RigidBodySet, + ) -> Option { + let collider = self.colliders.remove(handle)?; + + /* + * Delete the collider from its parent body. + */ + if let Some(mut parent) = bodies.get_mut_internal(collider.parent) { + parent.remove_collider_internal(handle, &collider); + bodies.wake_up(collider.parent, true); + } + + /* + * Publish removal. + */ + let message = RemovedCollider { + handle, + contact_graph_index: collider.contact_graph_index, + proximity_graph_index: collider.proximity_graph_index, + proxy_index: collider.proxy_index, + }; + + self.removed_colliders.publish(message); + + Some(collider) } /// Gets the collider with the given handle without a known generation. diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 406727f..4456961 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -45,6 +45,7 @@ pub type RayIntersection = ncollide::query::RayIntersection; pub(crate) use self::ball::WBall; pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections}; pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent; +pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; #[cfg(feature = "dim2")] diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 016da33..60c5e1a 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -14,13 +14,16 @@ use crate::geometry::proximity_detector::{ // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair, + BroadPhasePairEvent, Collider, Colli