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/pipeline/query_pipeline.rs | 245 ++++------------------------------------- 1 file changed, 19 insertions(+), 226 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 15caaef..070e996 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,231 +1,10 @@ use crate::dynamics::RigidBodySet; -use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, AABB, WAABB}; +use crate::geometry::{ + Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree, AABB, WAABB, +}; use crate::math::{Point, Vector}; use ncollide::bounding_volume::BoundingVolume; -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -struct NodeIndex { - index: u32, // Index of the addressed node in the `children` 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 WAABBHierarchyNodeChildren { - waabb: WAABB, - // Index of the children of the 4 nodes represented by self. - // If this is a leaf, it contains the proxy ids instead. - grand_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(), - } - } -} - -struct WAABBHierarchy { - children: Vec, - dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. - proxies: Vec, -} - -impl WAABBHierarchy { - pub fn new() -> Self { - WAABBHierarchy { - children: Vec::new(), - dirty: Vec::new(), - proxies: Vec::new(), - } - } - - pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) { - self.children.clear(); - self.dirty.clear(); - self.proxies.clear(); - - // Create proxies. - let mut indices = Vec::with_capacity(colliders.len()); - let mut proxies = vec![ColliderNodeIndex::invalid(); colliders.len()]; - for (handle, collider) in colliders.iter() { - let index = handle.into_raw_parts().0; - if proxies.len() < handle.into_raw_parts().0 { - proxies.resize(index + 1, ColliderNodeIndex::invalid()); - } - - proxies[index].handle = handle; - indices.push(index); - } - - // Compute AABBs. - let mut aabbs = vec![AABB::new_invalid(); 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 = WAABBHierarchyNodeChildren { - waabb: WAABB::new_invalid(), - grand_children: [1; 4], - parent: NodeIndex::invalid(), - leaf: false, - }; - - self.children.push(root_node); - let root_id = NodeIndex::new(0, 0); - let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id); - self.children[0].waabb = WAABB::splat(aabb); - } - - 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.children.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 = WAABBHierarchyNodeChildren { - waabb: WAABB::from(leaf_aabbs), - grand_children: proxy_ids, - parent, - leaf: true, - }; - - self.children.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, subdiv_dims[0]); - let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, subdiv_dims[1]); - let (right_bottom, right_top) = split_indices_wrt_dim(right, &aabbs, subdiv_dims[1]); - - let node = WAABBHierarchyNodeChildren { - waabb: WAABB::new_invalid(), - grand_children: [0; 4], // Will be set after the recursive call - parent, - leaf: false, - }; - - let id = self.children.len() as u32; - self.children.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-children. - self.children[id as usize].grand_children = [a.0, b.0, c.0, d.0]; - self.children[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(&c.1); - (id, my_aabb) - } -} - -fn split_indices_wrt_dim<'a>( - indices: &'a mut [usize], - aabbs: &[AABB], - 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] > center[dim] { - indices.swap(icurr, ilast); - ilast -= 1; - } else { - icurr += 1; - } - } - - indices.split_at_mut(icurr) -} - /// A pipeline for performing queries on all the colliders of a scene. pub struct QueryPipeline { // hierarchy: WAABBHierarchy, @@ -261,11 +40,24 @@ impl QueryPipeline { ray: &Ray, max_toi: f32, ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { + let t0 = instant::now(); + let mut tree = WQuadtree::new(); + tree.clear_and_rebuild(colliders); + println!("Built quadtree in time: {}", instant::now() - t0); + let t0 = instant::now(); + let inter = tree.cast_ray(ray, max_toi); + println!( + "Found {} interefrences in time {}.", + inter.len(), + instant::now() - t0 + ); + + let t0 = instant::now(); let mut best = f32::MAX; let mut result = None; - // FIXME: this is a brute-force approach. - for (handle, collider) in colliders.iter() { + for handle in inter { + let collider = &colliders[handle]; if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { if inter.toi < best { best = inter.toi; @@ -273,6 +65,7 @@ impl QueryPipeline { } } } + println!("Cast time: {}", instant::now() - t0); result } -- cgit