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

4
src/geometry/mod.rs
View File

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

64
src/geometry/waabb.rs
View File

@@ -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<SimdFloat>,
pub dir: Vector<SimdFloat>,
}
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<f32>; SIMD_WIDTH],
pub dir: [Vector<f32>; 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)

285
src/geometry/wquadtree.rs Normal file
View File

@@ -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<WQuadtreeNodeChildren>,
dirty: Vec<bool>, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism.
proxies: Vec<ColliderNodeIndex>,
}
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(&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, &center, subdiv_dims[0]);
let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, &center, subdiv_dims[1]);
let (right_bottom, right_top) =
split_indices_wrt_dim(right, &aabbs, &center, 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<ColliderHandle> {
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<f32>,
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)
}

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
}

17
src/utils.rs
View File

@@ -11,7 +11,11 @@ use simba::simd::SimdValue;
use std::collections::HashMap;
use std::ops::{Add, Mul};
#[cfg(feature = "simd-is-enabled")]
use {crate::simd::SimdFloat, na::SimdPartialOrd, num::One};
use {
crate::simd::{SimdBool, SimdFloat},
na::SimdPartialOrd,
num::One,
};
// pub(crate) const SIN_10_DEGREES: f32 = 0.17364817766;
// pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
@@ -31,6 +35,17 @@ pub(crate) fn inv(val: f32) -> f32 {
}
}
/// Conditionally swaps each lanes of `a` with those of `b`.
///
/// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then
/// `a.extract(i)` is swapped with `b.extract(i)`.
#[cfg(feature = "simd-is-enabled")]
pub fn simd_swap(do_swap: SimdBool, a: &mut SimdFloat, b: &mut SimdFloat) {
let _a = *a;
*a = b.select(do_swap, *a);
*b = _a.select(do_swap, *b);
}
/// Trait to copy the sign of each component of one scalar/vector/matrix to another.
pub trait WSign<Rhs>: Sized {
// See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652