nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Complete the WQuadtree construction and ray-cast.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-09-21 17:26:57 +02:00
parent 7b8e322446
commit 2dda0e5ce4
5 changed files with 386 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

245
src/pipeline/query_pipeline.rs
View File

@@ -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
}