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/wquadtree.rs | 285 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 285 insertions(+) create mode 100644 src/geometry/wquadtree.rs (limited to 'src/geometry/wquadtree.rs') 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/wquadtree.rs | 155 ++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 150 insertions(+), 5 deletions(-) (limited to 'src/geometry/wquadtree.rs') 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/wquadtree.rs | 93 ++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 76 insertions(+), 17 deletions(-) (limited to 'src/geometry/wquadtree.rs') 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 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/wquadtree.rs') 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 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/wquadtree.rs') 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 8e432b298bd71df7d1b776b77c15713d9bea280e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 10:46:59 +0200 Subject: Make the WQuadTree more generic and use it as the trimesh acceleration structure. --- src/geometry/wquadtree.rs | 146 +++++++++++++++++++++++++++++++++------------- 1 file changed, 107 insertions(+), 39 deletions(-) (limited to 'src/geometry/wquadtree.rs') diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 17ecffe..627ad9a 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -7,6 +7,31 @@ use simba::simd::{SimdBool, SimdValue}; use std::collections::VecDeque; use std::ops::Range; +pub trait IndexedData: Copy { + fn default() -> Self; + fn index(&self) -> usize; +} + +impl IndexedData for usize { + fn default() -> Self { + u32::MAX as usize + } + + fn index(&self) -> usize { + *self + } +} + +impl IndexedData for ColliderHandle { + fn default() -> Self { + ColliderSet::invalid_handle() + } + + fn index(&self) -> usize { + self.into_raw_parts().0 + } +} + #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct NodeIndex { @@ -41,38 +66,32 @@ struct WQuadtreeNode { #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct WQuadtreeProxy { +struct WQuadtreeProxy { node: NodeIndex, - handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? + data: T, // The collider data. TODO: only set the collider generation here? } -impl WQuadtreeProxy { +impl WQuadtreeProxy { fn invalid() -> Self { Self { node: NodeIndex::invalid(), - handle: ColliderSet::invalid_handle(), + data: T::default(), } } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct WQuadtree { +#[derive(Clone, Debug)] +pub struct WQuadtree { nodes: Vec, dirty_nodes: VecDeque, - proxies: Vec, + proxies: Vec>, } -impl WQuadtree { - pub fn new() -> Self { - WQuadtree { - nodes: Vec::new(), - dirty_nodes: VecDeque::new(), - proxies: Vec::new(), - } - } - - pub fn pre_update(&mut self, handle: ColliderHandle) { - let id = handle.into_raw_parts().0; +// FIXME: this should be generic too. +impl WQuadtree { + pub fn pre_update(&mut self, data: ColliderHandle) { + let id = data.into_raw_parts().0; let node_id = self.proxies[id].node.index; let node = &mut self.nodes[node_id as usize]; if !node.dirty { @@ -86,7 +105,7 @@ impl WQuadtree { 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. + // NOTE: this will data 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]; @@ -94,7 +113,7 @@ impl WQuadtree { 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]; + let collider = &colliders[proxy.data]; *new_aabb = collider.compute_aabb(); } } else { @@ -107,7 +126,7 @@ impl WQuadtree { let node = &mut self.nodes[id as usize]; let new_waabb = WAABB::from(new_aabbs); - if !node.waabb.contains_lanewise(&new_waabb).all() { + if !node.waabb.contains(&new_waabb).all() { node.waabb = new_waabb; node.waabb.dilate_by_factor(dilation_factor); self.dirty_nodes.push_back(node.parent.index); @@ -116,31 +135,40 @@ impl WQuadtree { } } } +} + +impl WQuadtree { + pub fn new() -> Self { + WQuadtree { + nodes: Vec::new(), + dirty_nodes: VecDeque::new(), + proxies: Vec::new(), + } + } - pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet, dilation_factor: f32) { + pub fn clear_and_rebuild( + &mut self, + data: impl ExactSizeIterator, + dilation_factor: f32, + ) { self.nodes.clear(); self.proxies.clear(); // Create proxies. - let mut indices = Vec::with_capacity(colliders.len()); - self.proxies = vec![WQuadtreeProxy::invalid(); colliders.len()]; + let mut indices = Vec::with_capacity(data.len()); + let mut aabbs = vec![AABB::new_invalid(); data.len()]; + self.proxies = vec![WQuadtreeProxy::invalid(); data.len()]; - for (handle, collider) in colliders.iter() { - let index = handle.into_raw_parts().0; + for (data, aabb) in data { + let index = data.index(); if index >= self.proxies.len() { self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); + aabbs.resize(index + 1, AABB::new_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(); + self.proxies[index].data = data; aabbs[index] = aabb; + indices.push(index); } // Build the tree recursively. @@ -279,7 +307,47 @@ impl WQuadtree { (id, my_aabb) } - pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { + // FIXME: implement a visitor pattern to merge intersect_aabb + // and intersect_ray into a single method. + pub fn intersect_aabb(&self, aabb: &AABB) -> Vec { + let mut res = Vec::new(); + + if self.nodes.is_empty() { + return res; + } + + // Special case for the root. + let mut stack = vec![0u32]; + let waabb = WAABB::splat(*aabb); + while let Some(inode) = stack.pop() { + let node = self.nodes[inode as usize]; + let intersections = node.waabb.intersects(&waabb); + let bitmask = intersections.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 { + if node.leaf { + // We found a leaf! + // Unfortunately, invalid AABBs return a intersection as well. + if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { + res.push(proxy.data); + } + } else { + // Internal node, visit the child. + // Unfortunately, we have this check because invalid AABBs + // return a intersection as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push(node.children[ii]); + } + } + } + } + } + + res + } + + pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { let mut res = Vec::new(); if self.nodes.is_empty() { @@ -301,7 +369,7 @@ impl WQuadtree { // 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); + res.push(proxy.data); } } else { // Internal node, visit the child. @@ -324,14 +392,14 @@ struct WQuadtreeIncrementalBuilderStep { parent: NodeIndex, } -struct WQuadtreeIncrementalBuilder { - quadtree: WQuadtree, +struct WQuadtreeIncrementalBuilder { + quadtree: WQuadtree, to_insert: Vec, aabbs: Vec, indices: Vec, } -impl WQuadtreeIncrementalBuilder { +impl WQuadtreeIncrementalBuilder { pub fn new() -> Self { Self { quadtree: WQuadtree::new(), -- cgit From cf86ee40a199dc84f7b8dd045e57b3a15bca79fb Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 11:21:39 +0200 Subject: Use the WQuadtree for the exhaustive ray-cast too. --- src/geometry/wquadtree.rs | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) (limited to 'src/geometry/wquadtree.rs') diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 627ad9a..fe1ba2a 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -309,11 +309,9 @@ impl WQuadtree { // FIXME: implement a visitor pattern to merge intersect_aabb // and intersect_ray into a single method. - pub fn intersect_aabb(&self, aabb: &AABB) -> Vec { - let mut res = Vec::new(); - + pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec) { if self.nodes.is_empty() { - return res; + return; } // Special case for the root. @@ -330,7 +328,7 @@ impl WQuadtree { // We found a leaf! // Unfortunately, invalid AABBs return a intersection as well. if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { - res.push(proxy.data); + out.push(proxy.data); } } else { // Internal node, visit the child. @@ -343,15 +341,11 @@ impl WQuadtree { } } } - - res } - pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { - let mut res = Vec::new(); - + pub fn cast_ray(&self, ray: &Ray, max_toi: f32, out: &mut Vec) { if self.nodes.is_empty() { - return res; + return; } // Special case for the root. @@ -369,7 +363,7 @@ impl WQuadtree { // 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.data); + out.push(proxy.data); } } else { // Internal node, visit the child. @@ -382,8 +376,6 @@ impl WQuadtree { } } } - - res } } -- cgit From 682ff61f94931ef205a9f81e7d00417ac88537c1 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 15:23:48 +0200 Subject: Don't let the PubSub internal offsets overflow + fix some warnings. --- src/geometry/wquadtree.rs | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/geometry/wquadtree.rs') diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index fe1ba2a..233ebd1 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -379,11 +379,13 @@ impl WQuadtree { } } +#[allow(dead_code)] struct WQuadtreeIncrementalBuilderStep { range: Range, parent: NodeIndex, } +#[allow(dead_code)] struct WQuadtreeIncrementalBuilder { quadtree: WQuadtree, to_insert: Vec, @@ -391,6 +393,7 @@ struct WQuadtreeIncrementalBuilder { indices: Vec, } +#[allow(dead_code)] impl WQuadtreeIncrementalBuilder { pub fn new() -> Self { Self { -- cgit From e87b73a2a20fee1ed333d564ba46dbf1c3ca75e2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 15:49:22 +0200 Subject: Fix compilation in 2D. --- src/geometry/wquadtree.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src/geometry/wquadtree.rs') diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 233ebd1..fce04eb 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -1,6 +1,8 @@ use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB}; use crate::geometry::{WRay, WAABB}; -use crate::math::{Point, Vector}; +use crate::math::Point; +#[cfg(feature = "dim3")] +use crate::math::Vector; use crate::simd::{SimdFloat, SIMD_WIDTH}; use ncollide::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; @@ -252,6 +254,7 @@ impl WQuadtree { // Find the axis with minimum variance. This is the axis along // which we are **not** subdividing our set. + #[allow(unused_mut)] // Does not need to be mutable in 2D. let mut subdiv_dims = [0, 1]; #[cfg(feature = "dim3")] { @@ -466,6 +469,7 @@ impl WQuadtreeIncrementalBuilder { // Find the axis with minimum variance. This is the axis along // which we are **not** subdividing our set. + #[allow(unused_mut)] // Does not need to be mutable in 2D. let mut subdiv_dims = [0, 1]; #[cfg(feature = "dim3")] { -- cgit