From 3c85a6ac41397cf95199933c6a93909bc070a844 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 8 Sep 2020 21:18:17 +0200 Subject: Start implementing ray-casting. This adds a QueryPipeline structure responsible for scene queries. Currently this structure is able to perform a brute-force ray-cast. This commit also includes the beginning of implementation of a SIMD-based acceleration structure which will be used for these scene queries in the future. --- src/dynamics/rigid_body.rs | 18 ++- src/geometry/collider.rs | 49 ++++++- src/geometry/mod.rs | 6 +- src/geometry/waabb.rs | 93 ++++++++++++- src/lib.rs | 9 +- src/pipeline/mod.rs | 2 + src/pipeline/query_pipeline.rs | 306 +++++++++++++++++++++++++++++++++++++++++ 7 files changed, 473 insertions(+), 10 deletions(-) create mode 100644 src/pipeline/query_pipeline.rs (limited to 'src') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index a2fcacc..d32ea46 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -137,6 +137,15 @@ impl RigidBody { crate::utils::inv(self.mass_properties.inv_mass) } + /// The predicted position of this rigid-body. + /// + /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position` + /// method and is used for estimating the kinematic body velocity at the next timestep. + /// For non-kinematic bodies, this value is currently unspecified. + pub fn predicted_position(&self) -> &Isometry { + &self.predicted_position + } + /// Adds a collider to this rigid-body. pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { let mass_properties = coll @@ -201,12 +210,17 @@ impl RigidBody { self.position = self.integrate_velocity(dt) * self.position; } - /// Sets the position of this rigid body. + /// Sets the position and `next_kinematic_position` of this rigid body. + /// + /// This will teleport the rigid-body to the specified position/orientation, + /// completely ignoring any physics rule. If this body is kinematic, this will + /// also set the next kinematic position to the same value, effectively + /// resetting to zero the next interpolated velocity of the kinematic body. pub fn set_position(&mut self, pos: Isometry) { self.position = pos; // TODO: update the predicted position for dynamic bodies too? - if self.is_static() { + if self.is_static() || self.is_kinematic() { self.predicted_position = pos; } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 2d55857..183446b 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,11 +1,12 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::query::RayCast; use num::Zero; #[derive(Clone)] @@ -97,6 +98,46 @@ impl Shape { Shape::HeightField(heightfield) => heightfield.bounding_volume(position), } } + + /// Computes the first intersection point between a ray in this collider. + /// + /// Some shapes are not supported yet and will always return `None`. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + pub fn cast_ray( + &self, + position: &Isometry, + ray: &Ray, + max_toi: f32, + ) -> Option { + match self { + Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), + Shape::Polygon(_poly) => None, + Shape::Capsule(caps) => { + let pos = position * caps.transform_wrt_y(); + let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); + caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) + } + Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), + #[cfg(feature = "dim2")] + Shape::Triangle(triangle) => { + // This is not implemented yet in 2D. + None + } + #[cfg(feature = "dim3")] + Shape::Triangle(triangle) => { + triangle.toi_and_normal_with_ray(position, ray, max_toi, true) + } + Shape::Trimesh(_trimesh) => None, + Shape::HeightField(heightfield) => { + heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) + } + } + } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -353,6 +394,12 @@ impl ColliderBuilder { self } + /// Sets the restitution coefficient of the collider this builder will build. + pub fn restitution(mut self, restitution: f32) -> Self { + self.restitution = restitution; + self + } + /// Sets the density of the collider this builder will build. pub fn density(mut self, density: f32) -> Self { self.density = Some(density); diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4f72778..5fcdf71 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -36,6 +36,10 @@ pub type AABB = ncollide::bounding_volume::AABB; pub type ContactEvent = ncollide::pipeline::ContactEvent; /// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not). pub type ProximityEvent = ncollide::pipeline::ProximityEvent; +/// A ray that can be cast against colliders. +pub type Ray = ncollide::query::Ray; +/// The intersection between a ray and a collider. +pub type RayIntersection = ncollide::query::RayIntersection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; @@ -48,7 +52,6 @@ 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; -#[cfg(feature = "simd-is-enabled")] pub(crate) use self::waabb::WAABB; //pub(crate) use self::z_order::z_cmp_floats; @@ -75,6 +78,5 @@ mod proximity_detector; pub(crate) mod sat; pub(crate) mod triangle; mod trimesh; -#[cfg(feature = "simd-is-enabled")] mod waabb; //mod z_order; diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index c3853bc..702b5aa 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,15 +1,27 @@ #[cfg(feature = "serde-serialize")] use crate::math::DIM; -use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH}; +use crate::math::{Point, SIMD_WIDTH}; use ncollide::bounding_volume::AABB; -use simba::simd::{SimdPartialOrd, SimdValue}; +#[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 WAABB { pub mins: Point, pub maxs: Point, } +#[derive(Debug, Copy, Clone)] +#[cfg(not(feature = "simd-is-enabled"))] +pub(crate) struct WAABB { + pub mins: [Point; SIMD_WIDTH], + pub maxs: [Point; SIMD_WIDTH], +} + #[cfg(feature = "serde-serialize")] impl serde::Serialize for WAABB { fn serialize(&self, serializer: S) -> Result @@ -18,16 +30,24 @@ impl serde::Serialize for WAABB { { use serde::ser::SerializeStruct; + #[cfg(feature = "simd-is-enabled")] let mins: Point<[f32; SIMD_WIDTH]> = Point::from( self.mins .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); + #[cfg(feature = "simd-is-enabled")] let maxs: Point<[f32; SIMD_WIDTH]> = Point::from( self.maxs .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); + + #[cfg(not(feature = "simd-is-enabled"))] + let mins = self.mins; + #[cfg(not(feature = "simd-is-enabled"))] + let maxs = self.maxs; + let mut waabb = serializer.serialize_struct("WAABB", 2)?; waabb.serialize_field("mins", &mins)?; waabb.serialize_field("maxs", &maxs)?; @@ -52,6 +72,7 @@ impl<'de> serde::Deserialize<'de> for WAABB { ) } + #[cfg(feature = "simd-is-enabled")] fn visit_seq(self, mut seq: A) -> Result where A: serde::de::SeqAccess<'de>, @@ -66,17 +87,36 @@ impl<'de> serde::Deserialize<'de> for WAABB { let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e))); Ok(WAABB { mins, maxs }) } + + #[cfg(not(feature = "simd-is-enabled"))] + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let mins = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + let maxs = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; + Ok(WAABB { mins, maxs }) + } } deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {}) } } +#[cfg(feature = "simd-is-enabled")] impl WAABB { pub fn new(mins: Point, maxs: Point) -> Self { Self { mins, maxs } } + pub fn new_invalid() -> Self { + Self::splat(AABB::new_invalid()) + } + pub fn splat(aabb: AABB) -> Self { Self { mins: Point::splat(aabb.mins), @@ -103,6 +143,7 @@ impl WAABB { } } +#[cfg(feature = "simd-is-enabled")] impl From<[AABB; SIMD_WIDTH]> for WAABB { fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; @@ -114,3 +155,51 @@ impl From<[AABB; SIMD_WIDTH]> for WAABB { } } } + +#[cfg(not(feature = "simd-is-enabled"))] +impl WAABB { + pub fn new_invalid() -> Self { + Self::splat(AABB::new_invalid()) + } + + pub fn splat(aabb: AABB) -> Self { + Self { + mins: [aabb.mins; SIMD_WIDTH], + maxs: [aabb.maxs; SIMD_WIDTH], + } + } + + #[cfg(feature = "dim2")] + pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] { + array![|ii| + self.mins[ii].x <= other.maxs[ii].x + && other.mins[ii].x <= self.maxs[ii].x + && self.mins[ii].y <= other.maxs[ii].y + && other.mins[ii].y <= self.maxs[ii].y + ; SIMD_WIDTH + ] + } + + #[cfg(feature = "dim3")] + pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] { + array![|ii| + self.mins[ii].x <= other.maxs[ii].x + && other.mins[ii].x <= self.maxs[ii].x + && self.mins[ii].y <= other.maxs[ii].y + && other.mins[ii].y <= self.maxs[ii].y + && self.mins[ii].z <= other.maxs[ii].z + && other.mins[ii].z <= self.maxs[ii].z + ; SIMD_WIDTH + ] + } +} + +#[cfg(not(feature = "simd-is-enabled"))] +impl From<[AABB; SIMD_WIDTH]> for WAABB { + fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { + let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; + let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; + + WAABB { mins, maxs } + } +} diff --git a/src/lib.rs b/src/lib.rs index 118ac23..8cbf673 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -44,7 +44,6 @@ macro_rules! enable_flush_to_zero( } ); -#[cfg(feature = "simd-is-enabled")] macro_rules! array( ($callback: expr; SIMD_WIDTH) => { { @@ -139,7 +138,6 @@ pub mod utils; #[cfg(feature = "dim2")] /// Math primitives used throughout Rapier. pub mod math { - #[cfg(feature = "simd-is-enabled")] pub use super::simd::*; use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2}; @@ -182,7 +180,6 @@ pub mod math { #[cfg(feature = "dim3")] /// Math primitives used throughout Rapier. pub mod math { - #[cfg(feature = "simd-is-enabled")] pub use super::simd::*; use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3}; @@ -220,6 +217,12 @@ pub mod math { pub type SdpMatrix = crate::utils::SdpMatrix3; } +#[cfg(not(feature = "simd-is-enabled"))] +mod simd { + /// The number of lanes of a SIMD number. + pub const SIMD_WIDTH: usize = 4; +} + #[cfg(feature = "simd-is-enabled")] mod simd { #[allow(unused_imports)] diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 6298d18..287de9d 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -3,7 +3,9 @@ pub use collision_pipeline::CollisionPipeline; pub use event_handler::{ChannelEventCollector, EventHandler}; pub use physics_pipeline::PhysicsPipeline; +pub use query_pipeline::QueryPipeline; mod collision_pipeline; mod event_handler; mod physics_pipeline; +mod query_pipeline; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs new file mode 100644 index 0000000..15caaef --- /dev/null +++ b/src/pipeline/query_pipeline.rs @@ -0,0 +1,306 @@ +use crate::dynamics::RigidBodySet; +use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, 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, +} + +impl Default for QueryPipeline { + fn default() -> Self { + Self::new() + } +} + +impl QueryPipeline { + /// Initializes an empty query pipeline. + pub fn new() -> Self { + Self { + // hierarchy: WAABBHierarchy::new(), + } + } + + /// Update the acceleration structure on the query pipeline. + pub fn update(&mut self, _bodies: &mut RigidBodySet, _colliders: &mut ColliderSet) {} + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + pub fn cast_ray<'a>( + &self, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: f32, + ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { + let mut best = f32::MAX; + let mut result = None; + + // FIXME: this is a brute-force approach. + for (handle, collider) in colliders.iter() { + if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if inter.toi < best { + best = inter.toi; + result = Some((handle, collider, inter)); + } + } + } + + result + } + + /// Find the all intersections between a ray and a set of collider and passes them to a callback. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + /// - `callback`: function executed on each collider for which a ray intersection has been found. + /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, + /// this method will exit early, ignory any further raycast. + pub fn interferences_with_ray<'a>( + &self, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: f32, + mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, + ) { + // FIXME: this is a brute-force approach. + for (handle, collider) in colliders.iter() { + if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if !callback(handle, collider, inter) { + return; + } + } + } + } +} -- cgit From e16b7722be23f7b6627bd54e174d7782d33c53fe Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Sep 2020 22:22:55 +0200 Subject: Fix crash caused by the removal of a kinematic body. --- src/dynamics/rigid_body_set.rs | 46 +++++++++++++++++++++++++++++----------- src/pipeline/physics_pipeline.rs | 46 ++++++++++++++++++++++++++++++++++------ 2 files changed, 74 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 99d6f10..14a6471 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,7 +2,7 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::dynamics::{Joint, RigidBody}; +use crate::dynamics::{BodyStatus, Joint, RigidBody}; use crate::geometry::{ColliderSet, ContactPair, InteractionGraph}; use crossbeam::channel::{Receiver, Sender}; use std::ops::{Deref, DerefMut, Index, IndexMut}; @@ -128,9 +128,23 @@ impl RigidBodySet { pub(crate) fn activate(&mut self, handle: RigidBodyHandle) { let mut rb = &mut self.bodies[handle]; - if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) { - rb.active_set_id = self.active_dynamic_set.len(); - self.active_dynamic_set.push(handle); + match rb.body_status { + // XXX: this case should only concern the dynamic bodies. + // For static bodies we should use the modified_inactive_set, or something + // similar. Right now we do this for static bodies as well so the broad-phase + // takes them into account the first time they are inserted. + BodyStatus::Dynamic | BodyStatus::Static => { + if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) { + rb.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } + } + BodyStatus::Kinematic => { + if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) { + rb.active_set_id = self.active_kinematic_set.len(); + self.active_kinematic_set.push(handle); + } + } } } @@ -143,13 +157,14 @@ impl RigidBodySet { pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle { let handle = self.bodies.insert(rb); let rb = &mut self.bodies[handle]; - rb.active_set_id = self.active_dynamic_set.len(); if !rb.is_sleeping() && rb.is_dynamic() { + rb.active_set_id = self.active_dynamic_set.len(); self.active_dynamic_set.push(handle); } if rb.is_kinematic() { + rb.active_set_id = self.active_kinematic_set.len(); self.active_kinematic_set.push(handle); } @@ -166,12 +181,15 @@ impl RigidBodySet { pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option { let rb = self.bodies.remove(handle)?; - // Remove this body from the active dynamic set. - if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) { - self.active_dynamic_set.swap_remove(rb.active_set_id); + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + + for active_set in &mut active_sets { + if active_set.get(rb.active_set_id) == Some(&handle) { + active_set.swap_remove(rb.active_set_id); - if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) { - self.bodies[*replacement].active_set_id = rb.active_set_id; + if let Some(replacement) = active_set.get(rb.active_set_id) { + self.bodies[*replacement].active_set_id = rb.active_set_id; + } } } @@ -181,6 +199,7 @@ impl RigidBodySet { /// Forces the specified rigid-body to wake up if it is dynamic. pub fn wake_up(&mut self, handle: RigidBodyHandle) { if let Some(rb) = self.bodies.get_mut(handle) { + // TODO: what about kinematic bodies? if rb.is_dynamic() { rb.wake_up(); @@ -214,8 +233,11 @@ impl RigidBodySet { /// /// Using this is discouraged in favor of `self.get_mut(handle)` which does not /// suffer form the ABA problem. - pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> { - self.bodies.get_unknown_gen_mut(i) + pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> { + let sender = &self.activation_channel.0; + self.bodies + .get_unknown_gen_mut(i) + .map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle)) } /// Gets the rigid-body with the given handle. diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 8449477..192ca9a 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -298,8 +298,9 @@ impl PhysicsPipeline { #[cfg(test)] mod test { - use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; - use crate::geometry::{BroadPhase, ColliderSet, NarrowPhase}; + use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet}; + use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; + use crate::math::Vector; use crate::pipeline::PhysicsPipeline; #[test] @@ -310,12 +311,45 @@ mod test { let mut bf = BroadPhase::new(); let mut nf = NarrowPhase::new(); - let mut set = RigidBodySet::new(); - let rb = RigidBodyBuilder::new_dynamic().build(); + let mut bodies = RigidBodySet::new(); // Check that removing the body right after inserting it works. - let h1 = set.insert(rb.clone()); - pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + // We add two dynamic bodies, one kinematic body and one static body before removing + // them. This include a non-regression test where deleting a kimenatic body crashes. + let rb = RigidBodyBuilder::new_dynamic().build(); + let h1 = bodies.insert(rb.clone()); + let h2 = bodies.insert(rb.clone()); + + // The same but with a kinematic body. + let rb = RigidBodyBuilder::new_kinematic().build(); + let h3 = bodies.insert(rb.clone()); + + // The same but with a static body. + let rb = RigidBodyBuilder::new_static().build(); + let h4 = bodies.insert(rb.clone()); + + let to_delete = [h1, h2, h3, h4]; + for h in &to_delete { + pipeline.remove_rigid_body( + *h, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + ); + } + + pipeline.step( + &Vector::zeros(), + &IntegrationParameters::default(), + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + &(), + ); } #[test] -- cgit From 7b8e322446ffa36e3f47078e23eb61ef423175dc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 10:43:20 +0200 Subject: Make kinematic bodies properly wake up dynamic bodies. --- src/dynamics/joint/joint_set.rs | 4 +-- src/dynamics/rigid_body.rs | 16 +++++++-- src/dynamics/rigid_body_set.rs | 74 +++++++++++++++++++++++++++----------- src/geometry/narrow_phase.rs | 17 +++++---- src/pipeline/collision_pipeline.rs | 2 +- src/pipeline/physics_pipeline.rs | 49 +++++++++++++++++++++---- 6 files changed, 123 insertions(+), 39 deletions(-) (limited to 'src') diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 51d7432..90f5190 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -202,8 +202,8 @@ impl JointSet { } // Wake up the attached bodies. - bodies.wake_up(h1); - bodies.wake_up(h2); + bodies.wake_up(h1, true); + bodies.wake_up(h2, true); } if let Some(other) = self.joint_graph.remove_node(deleted_id) { diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index d32ea46..af1fb4a 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -181,10 +181,13 @@ impl RigidBody { } /// Wakes up this rigid body if it is sleeping. - pub fn wake_up(&mut self) { + /// + /// If `strong` is `true` then it is assured that the rigid-body will + /// remain awake during multiple subsequent timesteps. + pub fn wake_up(&mut self, strong: bool) { self.activation.sleeping = false; - if self.activation.energy == 0.0 && self.is_dynamic() { + if (strong || self.activation.energy == 0.0) && self.is_dynamic() { self.activation.energy = self.activation.threshold.abs() * 2.0; } } @@ -198,9 +201,18 @@ impl RigidBody { /// Is this rigid body sleeping? pub fn is_sleeping(&self) -> bool { + // TODO: should we: + // - return false for static bodies. + // - return true for non-sleeping dynamic bodies. + // - return true only for kinematic bodies with non-zero velocity? self.activation.sleeping } + /// Is the velocity of this body not zero? + pub fn is_moving(&self) -> bool { + !self.linvel.is_zero() || !self.angvel.is_zero() + } + fn integrate_velocity(&self, dt: f32) -> Isometry { let com = &self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 14a6471..3970d54 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -3,8 +3,9 @@ use rayon::prelude::*; use crate::data::arena::Arena; use crate::dynamics::{BodyStatus, Joint, RigidBody}; -use crate::geometry::{ColliderSet, ContactPair, InteractionGraph}; +use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph}; use crossbeam::channel::{Receiver, Sender}; +use num::Zero; use std::ops::{Deref, DerefMut, Index, IndexMut}; /// A mutable reference to a rigid-body. @@ -197,11 +198,14 @@ impl RigidBodySet { } /// Forces the specified rigid-body to wake up if it is dynamic. - pub fn wake_up(&mut self, handle: RigidBodyHandle) { + /// + /// If `strong` is `true` then it is assured that the rigid-body will + /// remain awake during multiple subsequent timesteps. + pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) { if let Some(rb) = self.bodies.get_mut(handle) { // TODO: what about kinematic bodies? if rb.is_dynamic() { - rb.wake_up(); + rb.wake_up(strong); if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) { rb.active_set_id = self.active_dynamic_set.len(); @@ -447,6 +451,45 @@ impl RigidBodySet { } } + // Read all the contacts and push objects touching touching this rigid-body. + #[inline(always)] + fn push_contacting_colliders( + rb: &RigidBody, + colliders: &ColliderSet, + contact_graph: &InteractionGraph, + stack: &mut Vec, + ) { + for collider_handle in &rb.colliders { + let collider = &colliders[*collider_handle]; + + for inter in contact_graph.interactions_with(collider.contact_graph_index) { + for manifold in &inter.2.manifolds { + if manifold.num_active_contacts() > 0 { + let other = + crate::utils::other_handle((inter.0, inter.1), *collider_handle); + let other_body = colliders[other].parent; + stack.push(other_body); + break; + } + } + } + } + } + + // Now iterate on all active kinematic bodies and push all the bodies + // touching them to the stack so they can be woken up. + for h in self.active_kinematic_set.iter() { + let rb = &self.bodies[*h]; + + if !rb.is_moving() { + // If the kinematic body does not move, it does not have + // to wake up any dynamic body. + continue; + } + + push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack); + } + // println!("Selection: {}", instant::now() - t); // let t = instant::now(); @@ -465,7 +508,9 @@ impl RigidBodySet { // We already visited this body and its neighbors. // Also, we don't propagate awake state through static bodies. continue; - } else if self.stack.len() < island_marker { + } + + if self.stack.len() < island_marker { if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() >= min_island_size { @@ -476,29 +521,16 @@ impl RigidBodySet { island_marker = self.stack.len(); } - rb.wake_up(); + rb.wake_up(false); rb.active_island_id = self.active_islands.len() - 1; rb.active_set_id = self.active_dynamic_set.len(); rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id]; rb.active_set_timestamp = self.active_set_timestamp; self.active_dynamic_set.push(handle); - // Read all the contacts and push objects touching this one. - for collider_handle in &rb.colliders { - let collider = &colliders[*collider_handle]; - - for inter in contact_graph.interactions_with(collider.contact_graph_index) { - for manifold in &inter.2.manifolds { - if manifold.num_active_contacts() > 0 { - let other = - crate::utils::other_handle((inter.0, inter.1), *collider_handle); - let other_body = colliders[other].parent; - self.stack.push(other_body); - break; - } - } - } - } + // Transmit the active state to all the rigid-bodies with colliders + // in contact or joined with this collider. + push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack); for inter in joint_graph.interactions_with(rb.joint_graph_index) { let other = crate::utils::other_handle((inter.0, inter.1), handle); diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 1a36511..016da33 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -87,11 +87,11 @@ impl NarrowPhase { // Wake up every body in contact with the deleted collider. for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) { if let Some(parent) = colliders.get(a).map(|c| c.parent) { - bodies.wake_up(parent) + bodies.wake_up(parent, true) } if let Some(parent) = colliders.get(b).map(|c| c.parent) { - bodies.wake_up(parent) + bodies.wake_up(parent, true) } } @@ -119,6 +119,7 @@ impl NarrowPhase { pub(crate) fn register_pairs( &mut self, colliders: &mut ColliderSet, + bodies: &mut RigidBodySet, broad_phase_events: &[BroadPhasePairEvent], events: &dyn EventHandler, ) { @@ -218,9 +219,13 @@ impl NarrowPhase { .contact_graph .remove_edge(co1.contact_graph_index, co2.contact_graph_index); - // Emit a contact stopped event if we had a proximity before removing the edge. + // Emit a contact stopped event if we had a contact before removing the edge. + // Also wake up the dynamic bodies that were in contact. if let Some(ctct) = contact_pair { if ctct.has_any_active_contact() { + bodies.wake_up(co1.parent, true); + bodies.wake_up(co2.parent, true); + events.handle_contact_event(ContactEvent::Stopped( pair.collider1, pair.collider2, @@ -250,8 +255,7 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) - { + if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) { // No need to update this contact because nothing moved. return; } @@ -359,7 +363,8 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) + if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static())) + || (!rb1.is_dynamic() && !rb2.is_dynamic()) { // No need to update this contact because nothing moved. return; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 0184295..f30dae7 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -50,7 +50,7 @@ impl CollisionPipeline { self.broad_phase_events.clear(); broad_phase.find_pairs(&mut self.broad_phase_events); - narrow_phase.register_pairs(colliders, &self.broad_phase_events, events); + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events); narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 192ca9a..3fcd1af 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -75,6 +75,15 @@ impl PhysicsPipeline { self.counters.step_started(); bodies.maintain_active_set(); + // Update kinematic bodies velocities. + // TODO: what is the best place for this? It should at least be + // located before the island computation because we test the velocity + // there to determine if this kinematic body should wake-up dynamic + // bodies it is touching. + bodies.foreach_active_kinematic_body_mut_internal(|_, body| { + body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + }); + self.counters.stages.collision_detection_time.start(); self.counters.cd.broad_phase_time.start(); self.broadphase_collider_pairs.clear(); @@ -91,7 +100,7 @@ impl PhysicsPipeline { broad_phase.find_pairs(&mut self.broad_phase_events); // println!("Find pairs time: {}", instant::now() - t); - narrow_phase.register_pairs(colliders, &self.broad_phase_events, events); + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); self.counters.cd.broad_phase_time.pause(); // println!("Num contact pairs: {}", pairs.len()); @@ -122,11 +131,6 @@ impl PhysicsPipeline { ); self.counters.stages.island_construction_time.pause(); - // Update kinematic bodies velocities. - bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); - }); - if self.manifold_indices.len() < bodies.num_islands() { self.manifold_indices .resize(bodies.num_islands(), Vec::new()); @@ -261,7 +265,7 @@ impl PhysicsPipeline { if let Some(parent) = bodies.get_mut_internal(collider.parent) { parent.remove_collider_internal(handle, &collider); - bodies.wake_up(collider.parent); + bodies.wake_up(collider.parent, true); } Some(collider) @@ -303,6 +307,37 @@ mod test { use crate::math::Vector; use crate::pipeline::PhysicsPipeline; + #[test] + fn kinematic_and_static_contact_crash() { + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + let mut pipeline = PhysicsPipeline::new(); + let mut bf = BroadPhase::new(); + let mut nf = NarrowPhase::new(); + let mut bodies = RigidBodySet::new(); + + let rb = RigidBodyBuilder::new_static().build(); + let h1 = bodies.insert(rb.clone()); + let co = ColliderBuilder::ball(10.0).build(); + colliders.insert(co.clone(), h1, &mut bodies); + + // The same but with a kinematic body. + let rb = RigidBodyBuilder::new_kinematic().build(); + let h2 = bodies.insert(rb.clone()); + colliders.insert(co, h2, &mut bodies); + + pipeline.step( + &Vector::zeros(), + &IntegrationParameters::default(), + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + &(), + ); + } + #[test] fn rigid_body_removal_before_step() { let mut colliders = ColliderSet::new(); -- cgit 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/mod.rs | 4 +- src/geometry/waabb.rs | 64 ++++++++- src/geometry/wquadtree.rs | 285 +++++++++++++++++++++++++++++++++++++++++ src/pipeline/query_pipeline.rs | 245 +++-------------------------------- src/utils.rs | 17 ++- 5 files changed, 386 insertions(+), 229 deletions(-) create mode 100644 src/geometry/wquadtree.rs (limited to 'src') diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 5fcdf71..406727f 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -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; diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 702b5aa..0664a50 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -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, + pub dir: Vector, +} + +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; SIMD_WIDTH], + pub dir: [Vector; 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) 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) +} 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 = sel