Complete the WQuadtree construction and ray-cast.

1 files changed, 19 insertions, 226 deletions

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<WAABBHierarchyNodeChildren>,
-    dirty: Vec<bool>, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism.
-    proxies: Vec<ColliderNodeIndex>,
-}
-
-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(&center.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
     }