From 3c85a6ac41397cf95199933c6a93909bc070a844 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 8 Sep 2020 21:18:17 +0200 Subject: [PATCH 01/29] 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. --- CHANGELOG | 1 + 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 +++++++++++++++++++++++++++++++++ src_testbed/physx_backend.rs | 3 +- src_testbed/testbed.rs | 83 +++++---- 10 files changed, 528 insertions(+), 42 deletions(-) create mode 100644 src/pipeline/query_pipeline.rs diff --git a/CHANGELOG b/CHANGELOG index 9e4731e..6c01cce 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -5,6 +5,7 @@ - Add `ColliderDesc::rotation(...)` to set the rotation of a collider relative to the rigid-body it is attached to. - Add `ColliderDesc::position(...)` to set the position of a collider relative to the rigid-body it is attached to. - Add `Collider::position_wrt_parent()` to get the position of a collider relative to the rigid-body it is attached to. +- Modify `RigidBody::set_position(...)` so it also resets the next kinematic position to the same value. - Deprecate `Collider::delta()` in favor of the new `Collider::position_wrt_parent()`. - Fix multiple issues occurring when having colliders resulting in a non-zero center-of-mass. - Fix a crash happening when removing a rigid-body with a collider, stepping the simulation, adding another rigid-body 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; + } + } + } + } +} diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index b06fd7e..8e08a2e 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -196,7 +196,8 @@ impl PhysxWorld { } } else { physx_sys::PxShapeFlags { - mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8, + mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8 + | physx_sys::PxShapeFlag::eSCENE_QUERY_SHAPE as u8, } }; diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 92aea5a..ba28a4b 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -21,9 +21,9 @@ use na::{self, Point2, Point3, Vector3}; use rapier::dynamics::{ ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent}; +use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray}; use rapier::math::Vector; -use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline}; +use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; #[cfg(feature = "fluids")] use salva::{coupling::ColliderCouplingSet, object::FluidHandle, LiquidWorld}; @@ -179,6 +179,7 @@ pub struct PhysicsState { pub colliders: ColliderSet, pub joints: JointSet, pub pipeline: PhysicsPipeline, + pub query_pipeline: QueryPipeline, } impl PhysicsState { @@ -190,6 +191,7 @@ impl PhysicsState { colliders: ColliderSet::new(), joints: JointSet::new(), pipeline: PhysicsPipeline::new(), + query_pipeline: QueryPipeline::new(), } } } @@ -197,6 +199,7 @@ impl PhysicsState { pub struct TestbedState { pub running: RunMode, pub draw_colls: bool, + pub highlighted_body: Option, // pub grabbed_object: Option, // pub grabbed_object_constraint: Option, pub grabbed_object_plane: (Point3, Vector3), @@ -240,7 +243,7 @@ pub struct Testbed { hide_counters: bool, // persistant_contacts: HashMap, font: Rc, - // cursor_pos: Point2, + cursor_pos: Point2, events: PhysicsEvents, event_handler: ChannelEventCollector, ui: Option, @@ -301,6 +304,7 @@ impl Testbed { let state = TestbedState { running: RunMode::Running, draw_colls: false, + highlighted_body: None, // grabbed_object: None, // grabbed_object_constraint: None, grabbed_object_plane: (Point3::origin(), na::zero()), @@ -349,7 +353,7 @@ impl Testbed { hide_counters: true, // persistant_contacts: HashMap::new(), font: Font::default(), - // cursor_pos: Point2::new(0.0f32, 0.0), + cursor_pos: Point2::new(0.0f32, 0.0), ui, event_handler, events, @@ -408,6 +412,9 @@ impl Testbed { .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true); self.time = 0.0; self.state.timestep_id = 0; + self.state.highlighted_body = None; + self.physics.query_pipeline = QueryPipeline::new(); + self.physics.pipeline = PhysicsPipeline::new(); self.physics.pipeline.counters.enable(); #[cfg(all(feature = "dim2", feature = "other-backends"))] @@ -832,6 +839,10 @@ impl Testbed { ); } } + WindowEvent::CursorPos(x, y, _) => { + self.cursor_pos.x = x as f32; + self.cursor_pos.y = y as f32; + } _ => {} } @@ -1146,37 +1157,46 @@ impl Testbed { self.state.grabbed_object = None; self.state.grabbed_object_constraint = None; } - WindowEvent::CursorPos(x, y, modifiers) => { - self.cursor_pos.x = x as f32; - self.cursor_pos.y = y as f32; - - // update the joint - if let Some(joint) = self.state.grabbed_object_constraint { - let size = window.size(); - let (pos, dir) = self - .graphics - .camera() - .unproject(&self.cursor_pos, &na::convert(size)); - let (ref ppos, ref pdir) = self.state.grabbed_object_plane; - - if let Some(inter) = query::ray_toi_with_plane(ppos, pdir, &Ray::new(pos, dir)) - { - let joint = self - .constraints - .get_mut(joint) - .unwrap() - .downcast_mut::>() - .unwrap(); - joint.set_anchor_1(pos + dir * inter) - } - } - - event.inhibited = modifiers.contains(Modifiers::Shift); - } _ => {} } */ } + + #[cfg(feature = "dim2")] + fn highlight_hovered_body(&mut self, window: &Window) { + // Do nothing for now. + } + + #[cfg(feature = "dim3")] + fn highlight_hovered_body(&mut self, window: &Window) { + if let Some(highlighted_body) = self.state.highlighted_body { + if let Some(nodes) = self.graphics.body_nodes_mut(highlighted_body) { + for node in nodes { + node.unselect() + } + } + } + + let size = window.size(); + let (pos, dir) = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(size)); + let ray = Ray::new(pos, dir); + let hit = self + .physics + .query_pipeline + .cast_ray(&self.physics.colliders, &ray, f32::MAX); + + if let Some((_, collider, _)) = hit { + if self.physics.bodies[collider.parent()].is_dynamic() { + self.state.highlighted_body = Some(collider.parent()); + for node in self.graphics.body_nodes_mut(collider.parent()).unwrap() { + node.select() + } + } + } + } } type CameraEffects<'a> = ( @@ -1550,6 +1570,7 @@ impl State for Testbed { } } + self.highlight_hovered_body(window); self.graphics.draw(&self.physics.colliders, window); #[cfg(feature = "fluids")] From a60e1e030d624a255e544cabdf98a2df5bb6efdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Sun, 13 Sep 2020 10:34:49 +0200 Subject: [PATCH 02/29] Disable physx query shapes for now. --- src_testbed/physx_backend.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 8e08a2e..8131b37 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -196,8 +196,7 @@ impl PhysxWorld { } } else { physx_sys::PxShapeFlags { - mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8 - | physx_sys::PxShapeFlag::eSCENE_QUERY_SHAPE as u8, + mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8, // | physx_sys::PxShapeFlag::eSCENE_QUERY_SHAPE as u8, } }; From e16b7722be23f7b6627bd54e174d7782d33c53fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 14 Sep 2020 22:22:55 +0200 Subject: [PATCH 03/29] Fix crash caused by the removal of a kinematic body. --- examples3d/platform3.rs | 25 ++++++++--------- src/dynamics/rigid_body_set.rs | 46 +++++++++++++++++++++++--------- src/pipeline/physics_pipeline.rs | 46 +++++++++++++++++++++++++++----- 3 files changed, 87 insertions(+), 30 deletions(-) diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index 3836baf..bb71b76 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -65,21 +65,22 @@ pub fn init_world(testbed: &mut Testbed) { * Setup a callback to control the platform. */ testbed.add_callback(move |_, physics, _, _, time| { - let mut platform = physics.bodies.get_mut(platform_handle).unwrap(); - let mut next_pos = platform.position; + if let Some(mut platform) = physics.bodies.get_mut(platform_handle) { + let mut next_pos = platform.position; - let dt = 0.016; - next_pos.translation.vector.y += (time * 5.0).sin() * dt; - next_pos.translation.vector.z += time.sin() * 5.0 * dt; + let dt = 0.016; + next_pos.translation.vector.y += (time * 5.0).sin() * dt; + next_pos.translation.vector.z += time.sin() * 5.0 * dt; - if next_pos.translation.vector.z >= rad * 10.0 { - next_pos.translation.vector.z -= dt; + if next_pos.translation.vector.z >= rad * 10.0 { + next_pos.translation.vector.z -= dt; + } + if next_pos.translation.vector.z <= -rad * 10.0 { + next_pos.translation.vector.z += dt; + } + + platform.set_next_kinematic_position(next_pos); } - if next_pos.translation.vector.z <= -rad * 10.0 { - next_pos.translation.vector.z += dt; - } - - platform.set_next_kinematic_position(next_pos); }); /* 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]; - if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) { - self.bodies[*replacement].active_set_id = rb.active_set_id; + 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) = 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] From 7b8e322446ffa36e3f47078e23eb61ef423175dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 21 Sep 2020 10:43:20 +0200 Subject: [PATCH 04/29] Make kinematic bodies properly wake up dynamic bodies. --- examples3d/platform3.rs | 6 +++ 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 +++++++++++++++++--- src_testbed/engine.rs | 16 ++++++- src_testbed/testbed.rs | 38 +++++++++------ 9 files changed, 168 insertions(+), 54 deletions(-) diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index bb71b76..6c3483e 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -64,7 +64,13 @@ pub fn init_world(testbed: &mut Testbed) { /* * Setup a callback to control the platform. */ + let mut count = 0; testbed.add_callback(move |_, physics, _, _, time| { + count += 1; + if count % 100 > 50 { + return; + } + if let Some(mut platform) = physics.bodies.get_mut(platform_handle) { let mut next_pos = platform.position; 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(); diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 62ab37e..27a8510 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -609,7 +609,7 @@ impl GraphicsManager { } } - pub fn draw(&mut self, colliders: &ColliderSet, window: &mut Window) { + pub fn draw(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) { // use kiss3d::camera::Camera; // println!( // "camera eye {:?}, at: {:?}", @@ -618,6 +618,20 @@ impl GraphicsManager { // ); for (_, ns) in self.b2sn.iter_mut() { for n in ns.iter_mut() { + /* + if let Some(co) = colliders.get(n.collider()) { + let bo = &bodies[co.parent()]; + + if bo.is_dynamic() { + if bo.is_sleeping() { + n.set_color(Point3::new(1.0, 0.0, 0.0)); + } else { + n.set_color(Point3::new(0.0, 1.0, 0.0)); + } + } + } + */ + n.update(colliders); n.draw(window); } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index ba28a4b..6c03ef4 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -824,7 +824,7 @@ impl Testbed { .physics .bodies .iter() - .filter(|e| e.1.is_dynamic()) + .filter(|e| !e.1.is_static()) .map(|e| e.0) .collect(); let num_to_delete = (dynamic_bodies.len() / 10).max(1); @@ -1367,7 +1367,7 @@ impl State for Testbed { } } else { for (_, mut body) in self.physics.bodies.iter_mut() { - body.wake_up(); + body.wake_up(true); body.activation.threshold = -1.0; } } @@ -1571,7 +1571,8 @@ impl State for Testbed { } self.highlight_hovered_body(window); - self.graphics.draw(&self.physics.colliders, window); + self.graphics + .draw(&self.physics.bodies, &self.physics.colliders, window); #[cfg(feature = "fluids")] { @@ -1648,26 +1649,35 @@ Fluids: {:.2}ms } if self.state.flags.contains(TestbedStateFlags::DEBUG) { - let hash_bf = md5::compute(&bincode::serialize(&self.physics.broad_phase).unwrap()); - let hash_nf = md5::compute(&bincode::serialize(&self.physics.narrow_phase).unwrap()); - let hash_bodies = md5::compute(&bincode::serialize(&self.physics.bodies).unwrap()); - let hash_colliders = - md5::compute(&bincode::serialize(&self.physics.colliders).unwrap()); - let hash_joints = md5::compute(&bincode::serialize(&self.physics.joints).unwrap()); + let bf = bincode::serialize(&self.physics.broad_phase).unwrap(); + let nf = bincode::serialize(&self.physics.narrow_phase).unwrap(); + let bs = bincode::serialize(&self.physics.bodies).unwrap(); + let cs = bincode::serialize(&self.physics.colliders).unwrap(); + let js = bincode::serialize(&self.physics.joints).unwrap(); + let hash_bf = md5::compute(&bf); + let hash_nf = md5::compute(&nf); + let hash_bodies = md5::compute(&bs); + let hash_colliders = md5::compute(&cs); + let hash_joints = md5::compute(&js); profile = format!( r#"{} Hashes at frame: {} -|_ Broad phase: {:?} -|_ Narrow phase: {:?} -|_ Bodies: {:?} -|_ Colliders: {:?} -|_ Joints: {:?}"#, +|_ Broad phase [{:.1}KB]: {:?} +|_ Narrow phase [{:.1}KB]: {:?} +|_ Bodies [{:.1}KB]: {:?} +|_ Colliders [{:.1}KB]: {:?} +|_ Joints [{:.1}KB]: {:?}"#, profile, self.state.timestep_id, + bf.len() as f32 / 1000.0, hash_bf, + nf.len() as f32 / 1000.0, hash_nf, + bs.len() as f32 / 1000.0, hash_bodies, + cs.len() as f32 / 1000.0, hash_colliders, + js.len() as f32 / 1000.0, hash_joints ); } From 2dda0e5ce48ed0d93b4b0fa3098ba08f59a50a0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 21 Sep 2020 17:26:57 +0200 Subject: [PATCH 05/29] 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 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 = 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 } diff --git a/src/utils.rs b/src/utils.rs index 91ce41c..c7cb908 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -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: Sized { // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652 From 56f6051b047aded906b8a89cbc66672c6f1e698e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 21 Sep 2020 18:29:32 +0200 Subject: [PATCH 06/29] Start adding incremental topology update for the WQuadtree. --- src/geometry/waabb.rs | 5 ++ src/geometry/wquadtree.rs | 155 ++++++++++++++++++++++++++++++++++++-- 2 files changed, 155 insertions(+), 5 deletions(-) diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 0664a50..c04514a 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -150,6 +150,11 @@ impl WAABB { } } + pub fn replace(&mut self, i: usize, aabb: AABB) { + self.mins.replace(i, aabb.mins); + self.maxs.replace(i, aabb.maxs); + } + pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool { let _0 = SimdFloat::zero(); let _1 = SimdFloat::one(); diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 4e3bf54..8fc1163 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -4,6 +4,7 @@ use crate::math::{Point, Vector}; use crate::simd::{SimdFloat, SIMD_WIDTH}; use ncollide::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; +use std::ops::Range; #[derive(Copy, Clone, Debug, PartialEq, Eq)] struct NodeIndex { @@ -25,7 +26,7 @@ impl NodeIndex { } #[derive(Copy, Clone, Debug)] -struct WQuadtreeNodeChildren { +struct WQuadtreeNode { waabb: WAABB, // Index of the nodes of the 4 nodes represented by self. // If this is a leaf, it contains the proxy ids instead. @@ -50,7 +51,7 @@ impl ColliderNodeIndex { } pub struct WQuadtree { - nodes: Vec, + nodes: Vec, dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. proxies: Vec, } @@ -92,7 +93,7 @@ impl WQuadtree { } // Build the tree recursively. - let root_node = WQuadtreeNodeChildren { + let root_node = WQuadtreeNode { waabb: WAABB::new_invalid(), children: [1, u32::MAX, u32::MAX, u32::MAX], parent: NodeIndex::invalid(), @@ -129,7 +130,7 @@ impl WQuadtree { proxy_ids[k] = *id as u32; } - let node = WQuadtreeNodeChildren { + let node = WQuadtreeNode { waabb: WAABB::from(leaf_aabbs), children: proxy_ids, parent, @@ -192,7 +193,7 @@ impl WQuadtree { // right_top.len() // ); - let node = WQuadtreeNodeChildren { + let node = WQuadtreeNode { waabb: WAABB::new_invalid(), children: [0; 4], // Will be set after the recursive call parent, @@ -257,6 +258,150 @@ impl WQuadtree { } } +struct WQuadtreeIncrementalBuilderStep { + range: Range, + parent: NodeIndex, +} + +struct WQuadtreeIncrementalBuilder { + quadtree: WQuadtree, + to_insert: Vec, + aabbs: Vec, + indices: Vec, +} + +impl WQuadtreeIncrementalBuilder { + pub fn new() -> Self { + Self { + quadtree: WQuadtree::new(), + to_insert: Vec::new(), + aabbs: Vec::new(), + indices: Vec::new(), + } + } + + pub fn update_single_depth(&mut self) { + if let Some(to_insert) = self.to_insert.pop() { + let indices = &mut self.indices[to_insert.range]; + + // Leaf case. + if indices.len() <= 4 { + let id = self.quadtree.nodes.len(); + let mut 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() { + aabb.merge(&self.aabbs[*id]); + leaf_aabbs[k] = self.aabbs[*id]; + proxy_ids[k] = *id as u32; + } + + let node = WQuadtreeNode { + waabb: WAABB::from(leaf_aabbs), + children: proxy_ids, + parent: to_insert.parent, + leaf: true, + }; + + self.quadtree.nodes[to_insert.parent.index as usize].children + [to_insert.parent.lane as usize] = id as u32; + self.quadtree.nodes[to_insert.parent.index as usize] + .waabb + .replace(to_insert.parent.lane as usize, aabb); + self.quadtree.nodes.push(node); + return; + } + + // 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); + let mut aabb = AABB::new_invalid(); + + for i in &*indices { + let coords = self.aabbs[*i].center().coords; + aabb.merge(&self.aabbs[*i]); + 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, &self.aabbs, ¢er, subdiv_dims[0]); + + let (left_bottom, left_top) = + split_indices_wrt_dim(left, &self.aabbs, ¢er, subdiv_dims[1]); + let (right_bottom, right_top) = + split_indices_wrt_dim(right, &self.aabbs, ¢er, subdiv_dims[1]); + + let node = WQuadtreeNode { + waabb: WAABB::new_invalid(), + children: [0; 4], // Will be set after the recursive call + parent: to_insert.parent, + leaf: false, + }; + + let id = self.quadtree.nodes.len() as u32; + self.quadtree.nodes.push(node); + + // Recurse! + let a = left_bottom.len(); + let b = a + left_top.len(); + let c = b + right_bottom.len(); + let d = c + right_top.len(); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: 0..a, + parent: NodeIndex::new(id, 0), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: a..b, + parent: NodeIndex::new(id, 1), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: b..c, + parent: NodeIndex::new(id, 2), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: c..d, + parent: NodeIndex::new(id, 3), + }); + + self.quadtree.nodes[to_insert.parent.index as usize].children + [to_insert.parent.lane as usize] = id as u32; + self.quadtree.nodes[to_insert.parent.index as usize] + .waabb + .replace(to_insert.parent.lane as usize, aabb); + } + } +} + fn split_indices_wrt_dim<'a>( indices: &'a mut [usize], aabbs: &[AABB], From a7d77a01447d2b77694b2a957d000790af60b383 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 22 Sep 2020 15:29:29 +0200 Subject: [PATCH 07/29] Add non-topological WQuadtree update. --- src/dynamics/rigid_body_set.rs | 10 ++++ src/geometry/waabb.rs | 31 ++++++++++++ src/geometry/wquadtree.rs | 93 +++++++++++++++++++++++++++------- src/pipeline/query_pipeline.rs | 34 ++++++++++--- src_testbed/testbed.rs | 8 +++ 5 files changed, 151 insertions(+), 25 deletions(-) diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 3970d54..f54bc55 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -282,6 +282,16 @@ impl RigidBodySet { .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender))) } + /// Iter through all the active kinematic rigid-bodies on this set. + pub fn iter_active_kinematic<'a>( + &'a self, + ) -> impl Iterator { + let bodies: &'a _ = &self.bodies; + self.active_kinematic_set + .iter() + .filter_map(move |h| Some((*h, bodies.get(*h)?))) + } + /// Iter through all the active dynamic rigid-bodies on this set. pub fn iter_active_dynamic<'a>( &'a self, diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index c04514a..0a90801 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -150,6 +150,12 @@ impl WAABB { } } + pub fn dilate_by_factor(&mut self, factor: SimdFloat) { + let dilation = (self.maxs - self.mins) * factor; + self.mins -= dilation; + self.maxs += dilation; + } + pub fn replace(&mut self, i: usize, aabb: AABB) { self.mins.replace(i, aabb.mins); self.maxs.replace(i, aabb.maxs); @@ -191,6 +197,24 @@ impl WAABB { hit } + #[cfg(feature = "dim2")] + pub fn contains_lanewise(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.mins.x) + & self.mins.y.simd_le(other.mins.y) + & self.maxs.x.simd_ge(other.maxs.x) + & self.maxs.y.simd_ge(other.maxs.y) + } + + #[cfg(feature = "dim3")] + pub fn contains_lanewise(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.mins.x) + & self.mins.y.simd_le(other.mins.y) + & self.mins.z.simd_le(other.mins.z) + & self.maxs.x.simd_ge(other.maxs.x) + & self.maxs.y.simd_ge(other.maxs.y) + & self.maxs.z.simd_ge(other.maxs.z) + } + #[cfg(feature = "dim2")] pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) @@ -208,6 +232,13 @@ impl WAABB { & self.mins.z.simd_le(other.maxs.z) & other.mins.z.simd_le(self.maxs.z) } + + pub fn to_merged_aabb(&self) -> AABB { + AABB::new( + self.mins.coords.map(|e| e.simd_horizontal_min()).into(), + self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), + ) + } } #[cfg(feature = "simd-is-enabled")] diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 8fc1163..b82a2c0 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -4,6 +4,7 @@ use crate::math::{Point, Vector}; use crate::simd::{SimdFloat, SIMD_WIDTH}; use ncollide::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; +use std::collections::VecDeque; use std::ops::Range; #[derive(Copy, Clone, Debug, PartialEq, Eq)] @@ -32,16 +33,17 @@ struct WQuadtreeNode { // 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? + leaf: bool, // TODO: pack this with the NodexIndex.lane? + dirty: bool, // TODO: move this to a separate bitvec? } #[derive(Copy, Clone, Debug, PartialEq, Eq)] -struct ColliderNodeIndex { +struct WQuadtreeProxy { node: NodeIndex, handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? } -impl ColliderNodeIndex { +impl WQuadtreeProxy { fn invalid() -> Self { Self { node: NodeIndex::invalid(), @@ -52,32 +54,77 @@ impl ColliderNodeIndex { pub struct WQuadtree { nodes: Vec, - dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. - proxies: Vec, + dirty_nodes: VecDeque, + proxies: Vec, } impl WQuadtree { pub fn new() -> Self { WQuadtree { nodes: Vec::new(), - dirty: Vec::new(), + dirty_nodes: VecDeque::new(), proxies: Vec::new(), } } - pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) { + pub fn pre_update(&mut self, handle: ColliderHandle) { + let id = handle.into_raw_parts().0; + let node_id = self.proxies[id].node.index; + let node = &mut self.nodes[node_id as usize]; + if !node.dirty { + node.dirty = true; + self.dirty_nodes.push_back(node_id); + } + } + + pub fn update(&mut self, colliders: &ColliderSet, dilation_factor: f32) { + // Loop on the dirty leaves. + let dilation_factor = SimdFloat::splat(dilation_factor); + + while let Some(id) = self.dirty_nodes.pop_front() { + // NOTE: this will handle the case where we reach the root of the tree. + if let Some(node) = self.nodes.get(id as usize) { + // Compute the new WAABB. + let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; + for (child_id, new_aabb) in node.children.iter().zip(new_aabbs.iter_mut()) { + if node.leaf { + // We are in a leaf: compute the colliders' AABBs. + if let Some(proxy) = self.proxies.get(*child_id as usize) { + let collider = &colliders[proxy.handle]; + *new_aabb = collider.compute_aabb(); + } + } else { + // We are in an internal node: compute the children's AABBs. + if let Some(node) = self.nodes.get(*child_id as usize) { + *new_aabb = node.waabb.to_merged_aabb(); + } + } + } + + let node = &mut self.nodes[id as usize]; + let new_waabb = WAABB::from(new_aabbs); + if !node.waabb.contains_lanewise(&new_waabb).all() { + node.waabb = new_waabb; + node.waabb.dilate_by_factor(dilation_factor); + self.dirty_nodes.push_back(node.parent.index); + } + node.dirty = false; + } + } + } + + pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet, dilation_factor: f32) { 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()]; + self.proxies = vec![WQuadtreeProxy::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.resize(index + 1, WQuadtreeProxy::invalid()); } self.proxies[index].handle = handle; @@ -98,11 +145,12 @@ impl WQuadtree { children: [1, u32::MAX, u32::MAX, u32::MAX], parent: NodeIndex::invalid(), leaf: false, + dirty: false, }; self.nodes.push(root_node); let root_id = NodeIndex::new(0, 0); - let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id); + let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id, dilation_factor); self.nodes[0].waabb = WAABB::from([ aabb, AABB::new_invalid(), @@ -116,9 +164,10 @@ impl WQuadtree { indices: &mut [usize], aabbs: &[AABB], parent: NodeIndex, + dilation_factor: f32, ) -> (u32, AABB) { - // Leaf case. if indices.len() <= 4 { + // Leaf case. let my_id = self.nodes.len(); let mut my_aabb = AABB::new_invalid(); let mut leaf_aabbs = [AABB::new_invalid(); 4]; @@ -128,15 +177,19 @@ impl WQuadtree { my_aabb.merge(&aabbs[*id]); leaf_aabbs[k] = aabbs[*id]; proxy_ids[k] = *id as u32; + self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); } - let node = WQuadtreeNode { + let mut node = WQuadtreeNode { waabb: WAABB::from(leaf_aabbs), children: proxy_ids, parent, leaf: true, + dirty: false, }; + node.waabb + .dilate_by_factor(SimdFloat::splat(dilation_factor)); self.nodes.push(node); return (my_id as u32, my_aabb); } @@ -198,20 +251,24 @@ impl WQuadtree { children: [0; 4], // Will be set after the recursive call parent, leaf: false, + dirty: 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)); + let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0), dilation_factor); + let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1), dilation_factor); + let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2), dilation_factor); + let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3), dilation_factor); // 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]); + self.nodes[id as usize] + .waabb + .dilate_by_factor(SimdFloat::splat(dilation_factor)); // TODO: will this chain of .merged be properly optimized? let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1); @@ -302,6 +359,7 @@ impl WQuadtreeIncrementalBuilder { children: proxy_ids, parent: to_insert.parent, leaf: true, + dirty: false, }; self.quadtree.nodes[to_insert.parent.index as usize].children @@ -366,6 +424,7 @@ impl WQuadtreeIncrementalBuilder { children: [0; 4], // Will be set after the recursive call parent: to_insert.parent, leaf: false, + dirty: false, }; let id = self.quadtree.nodes.len() as u32; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 070e996..304ba18 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -7,7 +7,9 @@ use ncollide::bounding_volume::BoundingVolume; /// A pipeline for performing queries on all the colliders of a scene. pub struct QueryPipeline { - // hierarchy: WAABBHierarchy, + quadtree: WQuadtree, + tree_built: bool, + dilation_factor: f32, } impl Default for QueryPipeline { @@ -20,12 +22,32 @@ impl QueryPipeline { /// Initializes an empty query pipeline. pub fn new() -> Self { Self { - // hierarchy: WAABBHierarchy::new(), + quadtree: WQuadtree::new(), + tree_built: false, + dilation_factor: 0.01, } } /// Update the acceleration structure on the query pipeline. - pub fn update(&mut self, _bodies: &mut RigidBodySet, _colliders: &mut ColliderSet) {} + pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { + if !self.tree_built { + self.quadtree + .clear_and_rebuild(colliders, self.dilation_factor); + self.tree_built = true; + return; + } + + for (_, body) in bodies + .iter_active_dynamic() + .chain(bodies.iter_active_kinematic()) + { + for handle in &body.colliders { + self.quadtree.pre_update(*handle) + } + } + + self.quadtree.update(colliders, self.dilation_factor); + } /// Find the closest intersection between a ray and a set of collider. /// @@ -41,11 +63,7 @@ impl QueryPipeline { 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); + let inter = self.quadtree.cast_ray(ray, max_toi); println!( "Found {} interefrences in time {}.", inter.len(), diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 6c03ef4..9f63438 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -675,6 +675,10 @@ impl Testbed { &self.event_handler, ); + self.physics + .query_pipeline + .update(&self.physics.bodies, &self.physics.colliders); + #[cfg(feature = "fluids")] { fluids_time = instant::now(); @@ -1466,6 +1470,10 @@ impl State for Testbed { &self.event_handler, ); + self.physics + .query_pipeline + .update(&self.physics.bodies, &self.physics.colliders); + #[cfg(feature = "fluids")] { fluids_time = instant::now(); From 84bd60e4a5b88c9aa824797c8a444945b46e96b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 22 Sep 2020 17:57:29 +0200 Subject: [PATCH 08/29] Fix compilation when SIMD is not enabled. --- src/geometry/waabb.rs | 89 ------------------------------------------- src/lib.rs | 15 ++++++-- src/utils.rs | 15 -------- 3 files changed, 11 insertions(+), 108 deletions(-) diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 0a90801..4981373 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -5,14 +5,12 @@ 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, @@ -28,26 +26,11 @@ impl WRay { } #[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 { 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 @@ -56,24 +39,17 @@ 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)?; @@ -98,7 +74,6 @@ 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>, @@ -113,27 +88,12 @@ 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 } @@ -241,7 +201,6 @@ 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]; @@ -253,51 +212,3 @@ 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 8cbf673..3674717 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -219,8 +219,15 @@ pub mod math { #[cfg(not(feature = "simd-is-enabled"))] mod simd { + use simba::simd::{AutoBoolx4, AutoF32x4}; /// The number of lanes of a SIMD number. pub const SIMD_WIDTH: usize = 4; + /// SIMD_WIDTH - 1 + pub const SIMD_LAST_INDEX: usize = 3; + /// A SIMD float with SIMD_WIDTH lanes. + pub type SimdFloat = AutoF32x4; + /// A SIMD bool with SIMD_WIDTH lanes. + pub type SimdBool = AutoBoolx4; } #[cfg(feature = "simd-is-enabled")] @@ -236,16 +243,16 @@ mod simd { /// SIMD_WIDTH - 1 pub const SIMD_LAST_INDEX: usize = 3; #[cfg(not(feature = "simd-nightly"))] - /// A SIMD float with SIMD_WIDTH lanes. + /// A SIMD float with SIMD_WIDTH lanes. pub type SimdFloat = WideF32x4; #[cfg(not(feature = "simd-nightly"))] - /// A SIMD bool with SIMD_WIDTH lanes. + /// A SIMD bool with SIMD_WIDTH lanes. pub type SimdBool = WideBoolF32x4; #[cfg(feature = "simd-nightly")] - /// A SIMD float with SIMD_WIDTH lanes. + /// A SIMD float with SIMD_WIDTH lanes. pub type SimdFloat = f32x4; #[cfg(feature = "simd-nightly")] - /// A bool float with SIMD_WIDTH lanes. + /// A bool float with SIMD_WIDTH lanes. pub type SimdBool = m32x4; // pub const SIMD_WIDTH: usize = 8; diff --git a/src/utils.rs b/src/utils.rs index c7cb908..ecdd4fd 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -5,12 +5,10 @@ use crate::dynamics::RigidBodyHandle; use indexmap::IndexMap as HashMap; use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; use num::Zero; -#[cfg(feature = "simd-is-enabled")] use simba::simd::SimdValue; #[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))] use std::collections::HashMap; use std::ops::{Add, Mul}; -#[cfg(feature = "simd-is-enabled")] use { crate::simd::{SimdBool, SimdFloat}, na::SimdPartialOrd, @@ -39,7 +37,6 @@ pub(crate) fn inv(val: f32) -> f32 { /// /// 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); @@ -92,7 +89,6 @@ impl> WSign> for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WSign for SimdFloat { fn copy_sign_to(self, to: SimdFloat) -> SimdFloat { self.simd_copysign(to) @@ -117,7 +113,6 @@ impl WComponent for f32 { } } -#[cfg(feature = "simd-is-enabled")] impl WComponent for SimdFloat { type Element = f32; @@ -334,7 +329,6 @@ impl WDot for f32 { } } -#[cfg(feature = "simd-is-enabled")] impl WCrossMatrix for Vector3 { type CrossMat = Matrix3; @@ -349,7 +343,6 @@ impl WCrossMatrix for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WCrossMatrix for Vector2 { type CrossMat = Vector2; @@ -359,7 +352,6 @@ impl WCrossMatrix for Vector2 { } } -#[cfg(feature = "simd-is-enabled")] impl WCross> for Vector3 { type Result = Vector3; @@ -368,7 +360,6 @@ impl WCross> for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WCross> for SimdFloat { type Result = Vector2; @@ -377,7 +368,6 @@ impl WCross> for SimdFloat { } } -#[cfg(feature = "simd-is-enabled")] impl WCross> for Vector2 { type Result = SimdFloat; @@ -388,7 +378,6 @@ impl WCross> for Vector2 { } } -#[cfg(feature = "simd-is-enabled")] impl WDot> for Vector3 { type Result = SimdFloat; @@ -397,7 +386,6 @@ impl WDot> for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WDot> for Vector2 { type Result = SimdFloat; @@ -406,7 +394,6 @@ impl WDot> for Vector2 { } } -#[cfg(feature = "simd-is-enabled")] impl WDot for SimdFloat { type Result = SimdFloat; @@ -460,7 +447,6 @@ impl WAngularInertia for f32 { } } -#[cfg(feature = "simd-is-enabled")] impl WAngularInertia for SimdFloat { type AngVector = SimdFloat; type LinVector = Vector2; @@ -875,7 +861,6 @@ impl WAngularInertia for SdpMatrix3 { } } -#[cfg(feature = "simd-is-enabled")] impl WAngularInertia for SdpMatrix3 { type AngVector = Vector3; type LinVector = Vector3; From b39887a121305a36852604bf7c2726c1649934a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 22 Sep 2020 18:45:25 +0200 Subject: [PATCH 09/29] Fix crash of WQuadtree when the collider set contains holes. --- src/geometry/wquadtree.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index b82a2c0..01b2092 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -123,7 +123,7 @@ impl WQuadtree { for (handle, collider) in colliders.iter() { let index = handle.into_raw_parts().0; - if self.proxies.len() < index { + if index >= self.proxies.len() { self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); } From 52bbcc79fe1989f391e17b8c240a886bd8529d1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 22 Sep 2020 18:46:57 +0200 Subject: [PATCH 10/29] Query pipeline: aggressive workaround until we properly support collider addition/removal. --- src/pipeline/query_pipeline.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 304ba18..666eb58 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -33,7 +33,7 @@ impl QueryPipeline { if !self.tree_built { self.quadtree .clear_and_rebuild(colliders, self.dilation_factor); - self.tree_built = true; + // self.tree_built = true; // FIXME: uncomment this once we handle insertion/removals properly. return; } From c031f96ac548645932c5605bfc17869618e9212b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 28 Sep 2020 10:04:56 +0200 Subject: [PATCH 11/29] Fix compilation when parallelism is not enabled. --- src/geometry/waabb.rs | 4 +--- src/pipeline/query_pipeline.rs | 16 ++++++++-------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 4981373..1706370 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,7 +1,5 @@ use crate::geometry::Ray; -#[cfg(feature = "serde-serialize")] -use crate::math::DIM; -use crate::math::{Point, Vector, SIMD_WIDTH}; +use crate::math::{Point, Vector, DIM, SIMD_WIDTH}; use crate::utils; use ncollide::bounding_volume::AABB; use num::{One, Zero}; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 666eb58..1a37ad8 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -62,15 +62,15 @@ impl QueryPipeline { ray: &Ray, max_toi: f32, ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { - let t0 = instant::now(); + // let t0 = instant::now(); let inter = self.quadtree.cast_ray(ray, max_toi); - println!( - "Found {} interefrences in time {}.", - inter.len(), - instant::now() - t0 - ); + // println!( + // "Found {} interefrences in time {}.", + // inter.len(), + // instant::now() - t0 + // ); - let t0 = instant::now(); + // let t0 = instant::now(); let mut best = f32::MAX; let mut result = None; @@ -83,7 +83,7 @@ impl QueryPipeline { } } } - println!("Cast time: {}", instant::now() - t0); + // println!("Cast time: {}", instant::now() - t0); result } From 9c22e59416bd76b2a6e817e9e29692a22a3113e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 5 Oct 2020 16:50:31 +0200 Subject: [PATCH 12/29] Add a simple publish-subscribe mechanism. --- src/data/pubsub.rs | 121 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 src/data/pubsub.rs diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs new file mode 100644 index 0000000..b3b263c --- /dev/null +++ b/src/data/pubsub.rs @@ -0,0 +1,121 @@ +//! Publish-subscribe mechanism for internal events. + +use std::collections::VecDeque; + +/// The position of a subscriber on a pub-sub queue. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct PubSubCursor { + // Index of the next message to read. + id: u32, + next: u32, +} + +impl PubSubCursor { + fn id(&self, num_deleted: u32) -> usize { + (self.id - num_deleted) as usize + } + + fn next(&self, num_deleted: u32) -> usize { + (self.next - num_deleted) as usize + } +} + +/// A pub-sub queue. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct PubSub { + deleted_messages: u32, + deleted_offsets: u32, + messages: VecDeque, + offsets: VecDeque, +} + +impl PubSub { + /// Create a new empty pub-sub queue. + pub fn new() -> Self { + Self { + deleted_offsets: 0, + deleted_messages: 0, + messages: VecDeque::new(), + offsets: VecDeque::new(), + } + } + + /// Publish a new message. + pub fn publish(&mut self, message: T) { + if self.offsets.is_empty() { + // No subscribers, drop the message. + return; + } + + self.messages.push_back(message); + } + + /// Subscribe to the queue. + /// + /// A subscription cannot be cancelled. + pub fn subscribe(&mut self) -> PubSubCursor { + let cursor = PubSubCursor { + next: self.messages.len() as u32 + self.deleted_messages, + id: self.offsets.len() as u32 + self.deleted_offsets, + }; + + self.offsets.push_back(cursor.next); + cursor + } + + /// Get the messages not yet read by the given subscriber. + pub fn read(&self, cursor: &PubSubCursor) -> impl Iterator { + let next = cursor.next(self.deleted_messages); + + // TODO: use self.queue.range(next..) once it is stabilised. + MessageRange { + queue: &self.messages, + next, + } + } + + /// Makes the given subscribe acknowledge all the messages in the queue. + /// + /// A subscriber cannot read acknowledged messages any more. + pub fn ack(&mut self, cursor: &mut PubSubCursor) { + // Update the cursor. + cursor.next = self.messages.len() as u32 + self.deleted_messages; + self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX; + cursor.id = self.offsets.len() as u32 + self.deleted_offsets; + self.offsets.push_back(cursor.next); + + // Now clear the messages we don't need to + // maintain in memory anymore. + while self.offsets.front() == Some(&u32::MAX) { + self.offsets.pop_front(); + self.deleted_offsets += 1; + } + + // There must be at least one offset otherwise + // that would mean we have no subscribers. + let next = self.offsets.front().unwrap(); + let num_to_delete = *next - self.deleted_messages; + + for _ in 0..num_to_delete { + self.messages.pop_front(); + } + + self.deleted_messages += num_to_delete; + } +} + +struct MessageRange<'a, T> { + queue: &'a VecDeque, + next: usize, +} + +impl<'a, T> Iterator for MessageRange<'a, T> { + type Item = &'a T; + + #[inline(always)] + fn next(&mut self) -> Option<&'a T> { + let result = self.queue.get(self.next); + self.next += 1; + result + } +} From d8dfd864a453a9ead60f970549b2d289d7025364 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 5 Oct 2020 16:51:16 +0200 Subject: [PATCH 13/29] Joint set iteration: also yield the joint handle. --- src/dynamics/joint/joint_set.rs | 12 ++++++++---- src_testbed/nphysics_backend.rs | 6 +++--- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 90f5190..2b1895f 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -66,17 +66,21 @@ impl JointSet { } /// Iterates through all the joint on this set. - pub fn iter(&self) -> impl Iterator { - self.joint_graph.graph.edges.iter().map(|e| &e.weight) + pub fn iter(&self) -> impl Iterator { + self.joint_graph + .graph + .edges + .iter() + .map(|e| (e.weight.handle, &e.weight)) } /// Iterates mutably through all the joint on this set. - pub fn iter_mut(&mut self) -> impl Iterator { + pub fn iter_mut(&mut self) -> impl Iterator { self.joint_graph .graph .edges .iter_mut() - .map(|e| &mut e.weight) + .map(|e| (e.weight.handle, &mut e.weight)) } // /// The set of joints as an array. diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index c9ff2d1..c2a7fb1 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -66,10 +66,10 @@ impl NPhysicsWorld { } for joint in joints.iter() { - let b1 = BodyPartHandle(rapier2nphysics[&joint.body1], 0); - let b2 = BodyPartHandle(rapier2nphysics[&joint.body2], 0); + let b1 = BodyPartHandle(rapier2nphysics[&joint.1.body1], 0); + let b2 = BodyPartHandle(rapier2nphysics[&joint.1.body2], 0); - match &joint.params { + match &joint.1.params { JointParams::FixedJoint(params) => { let c = FixedConstraint::new( b1, From bbfe926a11fdf4f4eb0ddbac0c777a202e07d269 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 5 Oct 2020 16:51:32 +0200 Subject: [PATCH 14/29] Make the WQuadtree serializable. --- src/geometry/wquadtree.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 01b2092..17ecffe 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -8,6 +8,7 @@ use std::collections::VecDeque; use std::ops::Range; #[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct NodeIndex { index: u32, // Index of the addressed node in the `nodes` array. lane: u8, // SIMD lane of the addressed node. @@ -27,6 +28,7 @@ impl NodeIndex { } #[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct WQuadtreeNode { waabb: WAABB, // Index of the nodes of the 4 nodes represented by self. @@ -38,6 +40,7 @@ struct WQuadtreeNode { } #[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct WQuadtreeProxy { node: NodeIndex, handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? @@ -52,6 +55,7 @@ impl WQuadtreeProxy { } } +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct WQuadtree { nodes: Vec, dirty_nodes: VecDeque, From 2d0a888484dd296cc785caf978252dd97b58e10a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 5 Oct 2020 16:51:50 +0200 Subject: [PATCH 15/29] Make the query pipeline serializable. --- src/dynamics/integration_parameters.rs | 2 +- src/pipeline/query_pipeline.rs | 3 ++- src_testbed/physx_backend.rs | 6 +++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index faf22e1..b31c3f6 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -2,7 +2,7 @@ #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { - /// The timestep (default: `1.0 / 60.0`) + /// The timestep length (default: `1.0 / 60.0`) dt: f32, /// The inverse of `dt`. inv_dt: f32, diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 1a37ad8..ce4e063 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -6,6 +6,7 @@ use crate::math::{Point, Vector}; use ncollide::bounding_volume::BoundingVolume; /// A pipeline for performing queries on all the colliders of a scene. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct QueryPipeline { quadtree: WQuadtree, tree_built: bool, @@ -65,7 +66,7 @@ impl QueryPipeline { // let t0 = instant::now(); let inter = self.quadtree.cast_ray(ray, max_toi); // println!( - // "Found {} interefrences in time {}.", + // "Found {} interferences in time {}.", // inter.len(), // instant::now() - t0 // ); diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 8131b37..ec2e2bf 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -261,10 +261,10 @@ impl PhysxWorld { fn setup_joints(&mut self, joints: &JointSet) { unsafe { for joint in joints.iter() { - let actor1 = self.rapier2physx[&joint.body1]; - let actor2 = self.rapier2physx[&joint.body2]; + let actor1 = self.rapier2physx[&joint.1.body1]; + let actor2 = self.rapier2physx[&joint.1.body2]; - match &joint.params { + match &joint.1.params { JointParams::BallJoint(params) => { let frame1 = physx::transform::gl_to_px_tf( Isometry3::new(params.local_anchor1.coords, na::zero()).into_glam(), From 93aa7b6e1e8cbfd73542ed10ad5c26ae0a8b9848 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 5 Oct 2020 19:04:18 +0200 Subject: [PATCH 16/29] Use the publish-subscribe mechanism to handle collider removals across pipelines. --- examples3d/add_remove3.rs | 11 +-- src/data/mod.rs | 1 + src/data/pubsub.rs | 19 +++-- src/dynamics/rigid_body_set.rs | 34 ++++++-- src/geometry/broad_phase_multi_sap.rs | 85 ++++++++++++-------- src/geometry/collider_set.rs | 45 ++++++++++- src/geometry/mod.rs | 1 + src/geometry/narrow_phase.rs | 110 ++++++++++++++++++-------- src/pipeline/collision_pipeline.rs | 24 ------ src/pipeline/physics_pipeline.rs | 65 ++++----------- src_testbed/testbed.rs | 15 +--- 11 files changed, 236 insertions(+), 174 deletions(-) diff --git a/examples3d/add_remove3.rs b/examples3d/add_remove3.rs index 17aad44..6b58adf 100644 --- a/examples3d/add_remove3.rs +++ b/examples3d/add_remove3.rs @@ -28,14 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { .map(|e| e.0) .collect(); for handle in to_remove { - physics.pipeline.remove_rigid_body( - handle, - &mut physics.broad_phase, - &mut physics.narrow_phase, - &mut physics.bodies, - &mut physics.colliders, - &mut physics.joints, - ); + physics + .bodies + .remove(handle, &mut physics.colliders, &mut physics.joints); graphics.remove_body_nodes(window, handle); } }); diff --git a/src/data/mod.rs b/src/data/mod.rs index 7c00442..5d3efa6 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -2,3 +2,4 @@ pub mod arena; pub(crate) mod graph; +pub mod pubsub; diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs index b3b263c..1ac7498 100644 --- a/src/data/pubsub.rs +++ b/src/data/pubsub.rs @@ -1,16 +1,18 @@ //! Publish-subscribe mechanism for internal events. +use serde::export::PhantomData; use std::collections::VecDeque; /// The position of a subscriber on a pub-sub queue. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct PubSubCursor { +pub struct PubSubCursor { // Index of the next message to read. id: u32, next: u32, + _phantom: PhantomData, } -impl PubSubCursor { +impl PubSubCursor { fn id(&self, num_deleted: u32) -> usize { (self.id - num_deleted) as usize } @@ -53,18 +55,25 @@ impl PubSub { /// Subscribe to the queue. /// /// A subscription cannot be cancelled. - pub fn subscribe(&mut self) -> PubSubCursor { + pub fn subscribe(&mut self) -> PubSubCursor { let cursor = PubSubCursor { next: self.messages.len() as u32 + self.deleted_messages, id: self.offsets.len() as u32 + self.deleted_offsets, + _phantom: PhantomData, }; self.offsets.push_back(cursor.next); cursor } + /// Read the i-th message not yet read by the given subsciber. + pub fn read_ith(&self, cursor: &PubSubCursor, i: usize) -> Option<&T> { + self.messages + .get(cursor.next(self.deleted_messages) as usize + i) + } + /// Get the messages not yet read by the given subscriber. - pub fn read(&self, cursor: &PubSubCursor) -> impl Iterator { + pub fn read(&self, cursor: &PubSubCursor) -> impl Iterator { let next = cursor.next(self.deleted_messages); // TODO: use self.queue.range(next..) once it is stabilised. @@ -77,7 +86,7 @@ impl PubSub { /// Makes the given subscribe acknowledge all the messages in the queue. /// /// A subscriber cannot read acknowledged messages any more. - pub fn ack(&mut self, cursor: &mut PubSubCursor) { + pub fn ack(&mut self, cursor: &mut PubSubCursor) { // Update the cursor. cursor.next = self.messages.len() as u32 + self.deleted_messages; self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX; diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index f54bc55..7906083 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,7 +2,8 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::dynamics::{BodyStatus, Joint, RigidBody}; +use crate::data::pubsub::PubSub; +use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody}; use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph}; use crossbeam::channel::{Receiver, Sender}; use num::Zero; @@ -176,12 +177,17 @@ impl RigidBodySet { handle } - pub(crate) fn num_islands(&self) -> usize { - self.active_islands.len() - 1 - } - - pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option { + /// Removes a rigid-body, and all its attached colliders and joints, from these sets. + pub fn remove( + &mut self, + handle: RigidBodyHandle, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ) -> Option { let rb = self.bodies.remove(handle)?; + /* + * Update active sets. + */ let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; for active_set in &mut active_sets { @@ -194,9 +200,25 @@ impl RigidBodySet { } } + /* + * Remove colliders attached to this rigid-body. + */ + for collider in &rb.colliders { + colliders.remove(*collider, self); + } + + /* + * Remove joints attached to this rigid-body. + */ + joints.remove_rigid_body(rb.joint_graph_index, self); + Some(rb) } + pub(crate) fn num_islands(&self) -> usize { + self.active_islands.len() - 1 + } + /// Forces the specified rigid-body to wake up if it is dynamic. /// /// If `strong` is `true` then it is assured that the rigid-body will diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 054fccf..5c80e0e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,5 +1,6 @@ +use crate::data::pubsub::PubSubCursor; use crate::dynamics::RigidBodySet; -use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet}; +use crate::geometry::{Collider, ColliderHandle, ColliderPair, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; #[cfg(feature = "enhanced-determinism")] use crate::utils::FxHashMap32 as HashMap; @@ -381,6 +382,7 @@ impl SAPRegion { pub struct BroadPhase { proxies: Proxies, regions: HashMap, SAPRegion>, + removed_colliders: Option>, deleted_any: bool, // We could think serializing this workspace is useless. // It turns out is is important to serialize at least its capacity @@ -469,6 +471,7 @@ impl BroadPhase { /// Create a new empty broad-phase. pub fn new() -> Self { BroadPhase { + removed_colliders: None, proxies: Proxies::new(), regions: HashMap::default(), reporting: HashMap::default(), @@ -476,46 +479,60 @@ impl BroadPhase { } } - pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) { - for collider in handles.iter().filter_map(|h| colliders.get(*h)) { - if collider.proxy_index == crate::INVALID_USIZE { - // This collider has not been added to the broad-phase yet. - continue; + /// Maintain the broad-phase internal state by taking collider removal into account. + pub fn maintain(&mut self, colliders: &mut ColliderSet) { + // Ensure we already subscribed. + if self.removed_colliders.is_none() { + self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + } + + let mut cursor = self.removed_colliders.take().unwrap(); + for collider in colliders.removed_colliders.read(&cursor) { + self.remove_collider(collider.proxy_index); + } + + colliders.removed_colliders.ack(&mut cursor); + self.removed_colliders = Some(cursor); + } + + fn remove_collider<'a>(&mut self, proxy_index: usize) { + if proxy_index == crate::INVALID_USIZE { + // This collider has not been added to the broad-phase yet. + return; + } + + let proxy = &mut self.proxies[proxy_index]; + + // Push the proxy to infinity, but not beyond the sentinels. + proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0); + proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0); + // Discretize the AABB to find the regions that need to be invalidated. + let start = point_key(proxy.aabb.mins); + let end = point_key(proxy.aabb.maxs); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { + region.predelete_proxy(proxy_index); + self.deleted_any = true; + } } + } - let proxy = &mut self.proxies[collider.proxy_index]; - - // Push the proxy to infinity, but not beyond the sentinels. - proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0); - proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0); - // Discretize the AABB to find the regions that need to be invalidated. - let start = point_key(proxy.aabb.mins); - let end = point_key(proxy.aabb.maxs); - - #[cfg(feature = "dim2")] - for i in start.x..=end.x { - for j in start.y..=end.y { - if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { - region.predelete_proxy(collider.proxy_index); + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { + region.predelete_proxy(proxy_index); self.deleted_any = true; } } } - - #[cfg(feature = "dim3")] - for i in start.x..=end.x { - for j in start.y..=end.y { - for k in start.z..=end.z { - if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { - region.predelete_proxy(collider.proxy_index); - self.deleted_any = true; - } - } - } - } - - self.proxies.remove(collider.proxy_index); } + + self.proxies.remove(proxy_index); } pub(crate) fn update_aabbs( diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 22bba1b..3a259ae 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,14 +1,25 @@ use crate::data::arena::Arena; +use crate::data::pubsub::PubSub; use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::Collider; +use crate::geometry::{Collider, ColliderGraphIndex}; use std::ops::{Index, IndexMut}; /// The unique identifier of a collider added to a collider set. pub type ColliderHandle = crate::data::arena::Index; +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct RemovedCollider { + pub handle: ColliderHandle, + pub(crate) contact_graph_index: ColliderGraphIndex, + pub(crate) proximity_graph_index: ColliderGraphIndex, + pub(crate) proxy_index: usize, +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A set of colliders that can be handled by a physics `World`. pub struct ColliderSet { + pub(crate) removed_colliders: PubSub, pub(crate) colliders: Arena, } @@ -16,6 +27,7 @@ impl ColliderSet { /// Create a new empty set of colliders. pub fn new() -> Self { ColliderSet { + removed_colliders: PubSub::new(), colliders: Arena::new(), } } @@ -60,8 +72,35 @@ impl ColliderSet { handle } - pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option { - self.colliders.remove(handle) + /// Remove a collider from this set and update its parent accordingly. + pub fn remove( + &mut self, + handle: ColliderHandle, + bodies: &mut RigidBodySet, + ) -> Option { + let collider = self.colliders.remove(handle)?; + + /* + * Delete the collider from its parent body. + */ + if let Some(mut parent) = bodies.get_mut_internal(collider.parent) { + parent.remove_collider_internal(handle, &collider); + bodies.wake_up(collider.parent, true); + } + + /* + * Publish removal. + */ + let message = RemovedCollider { + handle, + contact_graph_index: collider.contact_graph_index, + proximity_graph_index: collider.proximity_graph_index, + proxy_index: collider.proxy_index, + }; + + self.removed_colliders.publish(message); + + Some(collider) } /// Gets the collider with the given handle without a known generation. diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 406727f..4456961 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -45,6 +45,7 @@ pub type RayIntersection = ncollide::query::RayIntersection; pub(crate) use self::ball::WBall; pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections}; pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent; +pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; #[cfg(feature = "dim2")] diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 016da33..60c5e1a 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -14,13 +14,16 @@ use crate::geometry::proximity_detector::{ // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair, + BroadPhasePairEvent, Collider, ColliderGraphIndex, ColliderHandle, ContactEvent, + ProximityEvent, ProximityPair, RemovedCollider, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] //use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::data::pubsub::PubSubCursor; use crate::ncollide::query::Proximity; use crate::pipeline::EventHandler; +use std::collections::HashMap; //use simba::simd::SimdValue; /// The narrow-phase responsible for computing precise contact information between colliders. @@ -28,6 +31,7 @@ use crate::pipeline::EventHandler; pub struct NarrowPhase { contact_graph: InteractionGraph, proximity_graph: InteractionGraph, + removed_colliders: Option>, // ball_ball: Vec, // Workspace: Vec<*mut ContactPair>, // shape_shape: Vec, // Workspace: Vec<*mut ContactPair>, // ball_ball_prox: Vec, // Workspace: Vec<*mut ProximityPair>, @@ -42,6 +46,7 @@ impl NarrowPhase { Self { contact_graph: InteractionGraph::new(), proximity_graph: InteractionGraph::new(), + removed_colliders: None, // ball_ball: Vec::new(), // shape_shape: Vec::new(), // ball_ball_prox: Vec::new(), @@ -73,45 +78,84 @@ impl NarrowPhase { // &mut self.contact_graph.interactions // } - pub(crate) fn remove_colliders( + /// Maintain the narrow-phase internal state by taking collider removal into account. + pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) { + // Ensure we already subscribed. + if self.removed_colliders.is_none() { + self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + } + + let mut cursor = self.removed_colliders.take().unwrap(); + + // TODO: avoid these hash-maps. + // They are necessary to handle the swap-remove done internally + // by the contact/proximity graphs when a node is removed. + let mut prox_id_remap = HashMap::new(); + let mut contact_id_remap = HashMap::new(); + + for i in 0.. { + if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) { + let proximity_graph_id = prox_id_remap + .get(&collider.handle) + .copied() + .unwrap_or(collider.proximity_graph_index); + let contact_graph_id = contact_id_remap + .get(&collider.handle) + .copied() + .unwrap_or(collider.contact_graph_index); + + self.remove_collider( + proximity_graph_id, + contact_graph_id, + colliders, + bodies, + &mut prox_id_remap, + &mut contact_id_remap, + ); + } else { + break; + } + } + + colliders.removed_colliders.ack(&mut cursor); + self.removed_colliders = Some(cursor); + } + + pub(crate) fn remove_collider<'a>( &mut self, - handles: &[ColliderHandle], + proximity_graph_id: ColliderGraphIndex, + contact_graph_id: ColliderGraphIndex, colliders: &mut ColliderSet, bodies: &mut RigidBodySet, + prox_id_remap: &mut HashMap, + contact_id_remap: &mut HashMap, ) { - for handle in handles { - if let Some(collider) = colliders.get(*handle) { - let proximity_graph_id = collider.proximity_graph_index; - let contact_graph_id = collider.contact_graph_index; + // 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, true) + } - // 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, true) - } + if let Some(parent) = colliders.get(b).map(|c| c.parent) { + bodies.wake_up(parent, true) + } + } - if let Some(parent) = colliders.get(b).map(|c| c.parent) { - bodies.wake_up(parent, true) - } - } + // We have to manage the fact that one other collider will + // have its graph index changed because of the node's swap-remove. + if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) { + if let Some(replacement) = colliders.get_mut(replacement) { + replacement.proximity_graph_index = proximity_graph_id; + } else { + prox_id_remap.insert(replacement, proximity_graph_id); + } + } - // We have to manage the fact that one other collider will - // have its graph index changed because of the node's swap-remove. - if let Some(replacement) = self - .proximity_graph - .remove_node(proximity_graph_id) - .and_then(|h| colliders.get_mut(h)) - { - replacement.proximity_graph_index = proximity_graph_id; - } - - if let Some(replacement) = self - .contact_graph - .remove_node(contact_graph_id) - .and_then(|h| colliders.get_mut(h)) - { - replacement.contact_graph_index = contact_graph_id; - } + if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) { + if let Some(replacement) = colliders.get_mut(replacement) { + replacement.contact_graph_index = contact_graph_id; + } else { + contact_id_remap.insert(replacement, contact_graph_id); } } } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index f30dae7..2283fa7 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -84,28 +84,4 @@ impl CollisionPipeline { bodies.modified_inactive_set.clear(); } - - /// Remove a rigid-body and all its associated data. - pub fn remove_rigid_body( - &mut self, - handle: RigidBodyHandle, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - ) -> Option { - // Remove the body. - let body = bodies.remove_internal(handle)?; - - // Remove this rigid-body from the broad-phase and narrow-phase. - broad_phase.remove_colliders(&body.colliders, colliders); - narrow_phase.remove_colliders(&body.colliders, colliders, bodies); - - // Remove all colliders attached to this body. - for collider in &body.colliders { - colliders.remove_internal(*collider); - } - - Some(body) - } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 3fcd1af..7185c62 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -1,6 +1,7 @@ //! Physics pipeline structures. use crate::counters::Counters; +use crate::data::pubsub::PubSubCursor; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; @@ -8,7 +9,7 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandl use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet, - ContactManifoldIndex, NarrowPhase, + ContactManifoldIndex, NarrowPhase, RemovedCollider, }; use crate::math::Vector; use crate::pipeline::EventHandler; @@ -59,6 +60,18 @@ impl PhysicsPipeline { } } + /// Remove this. + pub fn maintain( + &mut self, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) { + // broad_phase.maintain(colliders); + // narrow_phase.maintain(colliders, bodies); + } + /// Executes one timestep of the physics simulation. pub fn step( &mut self, @@ -73,6 +86,7 @@ impl PhysicsPipeline { ) { // println!("Step"); self.counters.step_started(); + self.maintain(broad_phase, narrow_phase, bodies, colliders); bodies.maintain_active_set(); // Update kinematic bodies velocities. @@ -249,55 +263,6 @@ impl PhysicsPipeline { bodies.modified_inactive_set.clear(); self.counters.step_completed(); } - - /// Remove a collider and all its associated data. - pub fn remove_collider( - &mut self, - handle: ColliderHandle, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - ) -> Option { - broad_phase.remove_colliders(&[handle], colliders); - narrow_phase.remove_colliders(&[handle], colliders, bodies); - let collider = colliders.remove_internal(handle)?; - - if let Some(parent) = bodies.get_mut_internal(collider.parent) { - parent.remove_collider_internal(handle, &collider); - bodies.wake_up(collider.parent, true); - } - - Some(collider) - } - - /// Remove a rigid-body and all its associated data. - pub fn remove_rigid_body( - &mut self, - handle: RigidBodyHandle, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - joints: &mut JointSet, - ) -> Option { - // Remove the body. - let body = bodies.remove_internal(handle)?; - - // Remove this rigid-body from the broad-phase and narrow-phase. - broad_phase.remove_colliders(&body.colliders, colliders); - narrow_phase.remove_colliders(&body.colliders, colliders, bodies); - - // Remove all joints attached to this body. - joints.remove_rigid_body(body.joint_graph_index, bodies); - - // Remove all colliders attached to this body. - for collider in &body.colliders { - colliders.remove_internal(*collider); - } - - Some(body) - } } #[cfg(test)] diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 9f63438..869fa41 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -813,13 +813,9 @@ impl Testbed { let num_to_delete = (colliders.len() / 10).max(1); for to_delete in &colliders[..num_to_delete] { - self.physics.pipeline.remove_collider( - to_delete[0], - &mut self.physics.broad_phase, - &mut self.physics.narrow_phase, - &mut self.physics.bodies, - &mut self.physics.colliders, - ); + self.physics + .colliders + .remove(to_delete[0], &mut self.physics.bodies); } } WindowEvent::Key(Key::D, Action::Release, _) => { @@ -833,11 +829,8 @@ impl Testbed { .collect(); let num_to_delete = (dynamic_bodies.len() / 10).max(1); for to_delete in &dynamic_bodies[..num_to_delete] { - self.physics.pipeline.remove_rigid_body( + self.physics.bodies.remove( *to_delete, - &mut self.physics.broad_phase, - &mut self.physics.narrow_phase, - &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, ); From 5d95368bce1d18f0dbec295f8ee787c1b54be0ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 5 Oct 2020 19:13:09 +0200 Subject: [PATCH 17/29] Testbed: fix snapshot restoration. --- src_testbed/testbed.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 869fa41..c145af5 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -1304,9 +1304,9 @@ impl State for Testbed { if let Ok(w) = snapshot.restore() { self.clear(window); self.graphics.clear(window); + self.set_world(w.3, w.4, w.5); self.physics.broad_phase = w.1; self.physics.narrow_phase = w.2; - self.set_world(w.3, w.4, w.5); self.state.timestep_id = w.0; } } From 721db2d49e06de38a16320425f77986b308445dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 10:02:21 +0200 Subject: [PATCH 18/29] Fix crash when deleting a collider. --- .../heightfield_shape_contact_generator.rs | 8 ++++---- src/pipeline/physics_pipeline.rs | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index f59a94b..04afc65 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -71,10 +71,10 @@ fn do_generate_contacts( } else { manifold.subshape_index_pair.1 }; - println!( - "Restoring for {} [chosen with {:?}]", - subshape_id, manifold.subshape_index_pair - ); + // println!( + // "Restoring for {} [chosen with {:?}]", + // subshape_id, manifold.subshape_index_pair + // ); // Use dummy shapes for the dispatch. #[cfg(feature = "dim2")] diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 7185c62..f00fa8d 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -68,8 +68,8 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, ) { - // broad_phase.maintain(colliders); - // narrow_phase.maintain(colliders, bodies); + broad_phase.maintain(colliders); + narrow_phase.maintain(colliders, bodies); } /// Executes one timestep of the physics simulation. From 8e432b298bd71df7d1b776b77c15713d9bea280e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 10:46:59 +0200 Subject: [PATCH 19/29] Make the WQuadTree more generic and use it as the trimesh acceleration structure. --- src/geometry/broad_phase.rs | 2 +- src/geometry/collider_set.rs | 2 +- .../trimesh_shape_contact_generator.rs | 12 +- .../trimesh_shape_proximity_detector.rs | 12 +- src/geometry/trimesh.rs | 53 +++---- src/geometry/waabb.rs | 8 +- src/geometry/wquadtree.rs | 146 +++++++++++++----- src/pipeline/query_pipeline.rs | 9 +- 8 files changed, 146 insertions(+), 98 deletions(-) diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs index a43b7af..a1333d0 100644 --- a/src/geometry/broad_phase.rs +++ b/src/geometry/broad_phase.rs @@ -144,7 +144,7 @@ impl WAABBHierarchy { if let Some(waabb2) = level.get(*i) { // NOTE: using `intersect.bitmask()` and performing bit comparisons // is much more efficient than testing if each intersect.extract(i) is true. - let intersect = waabb1.intersects_lanewise(waabb2); + let intersect = waabb1.intersects(waabb2); let bitmask = intersect.bitmask(); for j in 0..SIMD_WIDTH { diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 3a259ae..dbdd2a4 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -38,7 +38,7 @@ impl ColliderSet { } /// Iterate through all the colliders on this set. - pub fn iter(&self) -> impl Iterator { + pub fn iter(&self) -> impl ExactSizeIterator { self.colliders.iter() } diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 78f26bb..d759ae3 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -5,7 +5,7 @@ use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyI use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { - interferences: WAABBHierarchyIntersections, + interferences: Vec, local_aabb2: AABB, old_interferences: Vec, old_manifolds: Vec, @@ -14,7 +14,7 @@ pub struct TrimeshShapeContactGeneratorWorkspace { impl TrimeshShapeContactGeneratorWorkspace { pub fn new() -> Self { Self { - interferences: WAABBHierarchyIntersections::new(), + interferences: Vec::new(), local_aabb2: AABB::new_invalid(), old_interferences: Vec::new(), old_manifolds: Vec::new(), @@ -74,7 +74,7 @@ fn do_generate_contacts( let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? std::mem::swap( &mut workspace.old_interferences, - workspace.interferences.computed_interferences_mut(), + &mut workspace.interferences, ); std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds); ctxt.pair.manifolds.clear(); @@ -108,16 +108,14 @@ fn do_generate_contacts( // workspace.old_manifolds.len() // ); - trimesh1 - .waabbs() - .compute_interferences_with(local_aabb2, &mut workspace.interferences); + workspace.interferences = trimesh1.waabbs().intersect_aabb(&local_aabb2); workspace.local_aabb2 = local_aabb2; } /* * Dispatch to the specific solver by keeping the previous manifold if we already had one. */ - let new_interferences = workspace.interferences.computed_interferences(); + let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut old_manifolds_it = workspace.old_manifolds.drain(..); diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index 3dd7381..0a3ff44 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -5,7 +5,7 @@ use crate::geometry::{Collider, Proximity, Shape, Trimesh, WAABBHierarchyInterse use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { - interferences: WAABBHierarchyIntersections, + interferences: Vec, local_aabb2: AABB, old_interferences: Vec, } @@ -13,7 +13,7 @@ pub struct TrimeshShapeProximityDetectorWorkspace { impl TrimeshShapeProximityDetectorWorkspace { pub fn new() -> Self { Self { - interferences: WAABBHierarchyIntersections::new(), + interferences: Vec::new(), local_aabb2: AABB::new_invalid(), old_interferences: Vec::new(), } @@ -67,19 +67,17 @@ fn do_detect_proximity( let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? std::mem::swap( &mut workspace.old_interferences, - &mut workspace.interferences.computed_interferences_mut(), + &mut workspace.interferences, ); - trimesh1 - .waabbs() - .compute_interferences_with(local_aabb2, &mut workspace.interferences); + workspace.interferences = trimesh1.waabbs().intersect_aabb(&local_aabb2); workspace.local_aabb2 = local_aabb2; } /* * Dispatch to the specific solver by keeping the previous manifold if we already had one. */ - let new_interferences = workspace.interferences.computed_interferences(); + let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut best_proximity = Proximity::Disjoint; diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index 62731b6..f9e6034 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,4 +1,4 @@ -use crate::geometry::{Triangle, WAABBHierarchy}; +use crate::geometry::{Triangle, WQuadtree}; use crate::math::{Isometry, Point}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; @@ -7,7 +7,7 @@ use ncollide::bounding_volume::{HasBoundingVolume, AABB}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A triangle mesh. pub struct Trimesh { - waabb_tree: WAABBHierarchy, + wquadtree: WQuadtree, aabb: AABB, vertices: Vec>, indices: Vec>, @@ -25,41 +25,24 @@ impl Trimesh { "A triangle mesh must contain at least one triangle." ); - // z-sort the indices. - // indices.sort_unstable_by(|idx, jdx| { - // let ti = Triangle::new( - // vertices[idx[0] as usize], - // vertices[idx[1] as usize], - // vertices[idx[2] as usize], - // ); - // let tj = Triangle::new( - // vertices[jdx[0] as usize], - // vertices[jdx[1] as usize], - // vertices[jdx[2] as usize], - // ); - // let center_i = (ti.a.coords + ti.b.coords + ti.c.coords) / 3.0; - // let center_j = (tj.a.coords + tj.b.coords + tj.c.coords) / 3.0; - // crate::geometry::z_cmp_floats(center_i.as_slice(), center_j.as_slice()) - // .unwrap_or(std::cmp::Ordering::Equal) - // }); let aabb = AABB::from_points(&vertices); + let data = indices.iter().enumerate().map(|(i, idx)| { + let aabb = Triangle::new( + vertices[idx[0] as usize], + vertices[idx[1] as usize], + vertices[idx[2] as usize], + ) + .local_bounding_volume(); + (i, aabb) + }); - let aabbs: Vec<_> = indices - .iter() - .map(|idx| { - Triangle::new( - vertices[idx[0] as usize], - vertices[idx[1] as usize], - vertices[idx[2] as usize], - ) - .local_bounding_volume() - }) - .collect(); - - let waabb_tree = WAABBHierarchy::new(&aabbs); + let mut wquadtree = WQuadtree::new(); + // NOTE: we apply no dilation factor because we won't + // update this tree dynamically. + wquadtree.clear_and_rebuild(data, 0.0); Self { - waabb_tree, + wquadtree, aabb, vertices, indices, @@ -71,8 +54,8 @@ impl Trimesh { self.aabb.transform_by(pos) } - pub(crate) fn waabbs(&self) -> &WAABBHierarchy { - &self.waabb_tree + pub(crate) fn waabbs(&self) -> &WQuadtree { + &self.wquadtree } /// The number of triangles forming this mesh. diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 1706370..cc420d9 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -156,7 +156,7 @@ impl WAABB { } #[cfg(feature = "dim2")] - pub fn contains_lanewise(&self, other: &WAABB) -> SimdBool { + pub fn contains(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.mins.x) & self.mins.y.simd_le(other.mins.y) & self.maxs.x.simd_ge(other.maxs.x) @@ -164,7 +164,7 @@ impl WAABB { } #[cfg(feature = "dim3")] - pub fn contains_lanewise(&self, other: &WAABB) -> SimdBool { + pub fn contains(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.mins.x) & self.mins.y.simd_le(other.mins.y) & self.mins.z.simd_le(other.mins.z) @@ -174,7 +174,7 @@ impl WAABB { } #[cfg(feature = "dim2")] - pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { + pub fn intersects(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) & other.mins.x.simd_le(self.maxs.x) & self.mins.y.simd_le(other.maxs.y) @@ -182,7 +182,7 @@ impl WAABB { } #[cfg(feature = "dim3")] - pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { + pub fn intersects(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) & other.mins.x.simd_le(self.maxs.x) & self.mins.y.simd_le(other.maxs.y) diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 17ecffe..627ad9a 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -7,6 +7,31 @@ use simba::simd::{SimdBool, SimdValue}; use std::collections::VecDeque; use std::ops::Range; +pub trait IndexedData: Copy { + fn default() -> Self; + fn index(&self) -> usize; +} + +impl IndexedData for usize { + fn default() -> Self { + u32::MAX as usize + } + + fn index(&self) -> usize { + *self + } +} + +impl IndexedData for ColliderHandle { + fn default() -> Self { + ColliderSet::invalid_handle() + } + + fn index(&self) -> usize { + self.into_raw_parts().0 + } +} + #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct NodeIndex { @@ -41,38 +66,32 @@ struct WQuadtreeNode { #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct WQuadtreeProxy { +struct WQuadtreeProxy { node: NodeIndex, - handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? + data: T, // The collider data. TODO: only set the collider generation here? } -impl WQuadtreeProxy { +impl WQuadtreeProxy { fn invalid() -> Self { Self { node: NodeIndex::invalid(), - handle: ColliderSet::invalid_handle(), + data: T::default(), } } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct WQuadtree { +#[derive(Clone, Debug)] +pub struct WQuadtree { nodes: Vec, dirty_nodes: VecDeque, - proxies: Vec, + proxies: Vec>, } -impl WQuadtree { - pub fn new() -> Self { - WQuadtree { - nodes: Vec::new(), - dirty_nodes: VecDeque::new(), - proxies: Vec::new(), - } - } - - pub fn pre_update(&mut self, handle: ColliderHandle) { - let id = handle.into_raw_parts().0; +// FIXME: this should be generic too. +impl WQuadtree { + pub fn pre_update(&mut self, data: ColliderHandle) { + let id = data.into_raw_parts().0; let node_id = self.proxies[id].node.index; let node = &mut self.nodes[node_id as usize]; if !node.dirty { @@ -86,7 +105,7 @@ impl WQuadtree { let dilation_factor = SimdFloat::splat(dilation_factor); while let Some(id) = self.dirty_nodes.pop_front() { - // NOTE: this will handle the case where we reach the root of the tree. + // NOTE: this will data the case where we reach the root of the tree. if let Some(node) = self.nodes.get(id as usize) { // Compute the new WAABB. let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; @@ -94,7 +113,7 @@ impl WQuadtree { if node.leaf { // We are in a leaf: compute the colliders' AABBs. if let Some(proxy) = self.proxies.get(*child_id as usize) { - let collider = &colliders[proxy.handle]; + let collider = &colliders[proxy.data]; *new_aabb = collider.compute_aabb(); } } else { @@ -107,7 +126,7 @@ impl WQuadtree { let node = &mut self.nodes[id as usize]; let new_waabb = WAABB::from(new_aabbs); - if !node.waabb.contains_lanewise(&new_waabb).all() { + if !node.waabb.contains(&new_waabb).all() { node.waabb = new_waabb; node.waabb.dilate_by_factor(dilation_factor); self.dirty_nodes.push_back(node.parent.index); @@ -116,31 +135,40 @@ impl WQuadtree { } } } +} - pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet, dilation_factor: f32) { +impl WQuadtree { + pub fn new() -> Self { + WQuadtree { + nodes: Vec::new(), + dirty_nodes: VecDeque::new(), + proxies: Vec::new(), + } + } + + pub fn clear_and_rebuild( + &mut self, + data: impl ExactSizeIterator, + dilation_factor: f32, + ) { self.nodes.clear(); self.proxies.clear(); // Create proxies. - let mut indices = Vec::with_capacity(colliders.len()); - self.proxies = vec![WQuadtreeProxy::invalid(); colliders.len()]; + let mut indices = Vec::with_capacity(data.len()); + let mut aabbs = vec![AABB::new_invalid(); data.len()]; + self.proxies = vec![WQuadtreeProxy::invalid(); data.len()]; - for (handle, collider) in colliders.iter() { - let index = handle.into_raw_parts().0; + for (data, aabb) in data { + let index = data.index(); if index >= self.proxies.len() { self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); + aabbs.resize(index + 1, AABB::new_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(); + self.proxies[index].data = data; aabbs[index] = aabb; + indices.push(index); } // Build the tree recursively. @@ -279,7 +307,47 @@ impl WQuadtree { (id, my_aabb) } - pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { + // FIXME: implement a visitor pattern to merge intersect_aabb + // and intersect_ray into a single method. + pub fn intersect_aabb(&self, aabb: &AABB) -> Vec { + let mut res = Vec::new(); + + if self.nodes.is_empty() { + return res; + } + + // Special case for the root. + let mut stack = vec![0u32]; + let waabb = WAABB::splat(*aabb); + while let Some(inode) = stack.pop() { + let node = self.nodes[inode as usize]; + let intersections = node.waabb.intersects(&waabb); + let bitmask = intersections.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 { + if node.leaf { + // We found a leaf! + // Unfortunately, invalid AABBs return a intersection as well. + if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { + res.push(proxy.data); + } + } else { + // Internal node, visit the child. + // Unfortunately, we have this check because invalid AABBs + // return a intersection as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push(node.children[ii]); + } + } + } + } + } + + res + } + + pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { let mut res = Vec::new(); if self.nodes.is_empty() { @@ -301,7 +369,7 @@ impl WQuadtree { // 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); + res.push(proxy.data); } } else { // Internal node, visit the child. @@ -324,14 +392,14 @@ struct WQuadtreeIncrementalBuilderStep { parent: NodeIndex, } -struct WQuadtreeIncrementalBuilder { - quadtree: WQuadtree, +struct WQuadtreeIncrementalBuilder { + quadtree: WQuadtree, to_insert: Vec, aabbs: Vec, indices: Vec, } -impl WQuadtreeIncrementalBuilder { +impl WQuadtreeIncrementalBuilder { pub fn new() -> Self { Self { quadtree: WQuadtree::new(), diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index ce4e063..10aa7ed 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -8,7 +8,7 @@ use ncollide::bounding_volume::BoundingVolume; /// A pipeline for performing queries on all the colliders of a scene. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct QueryPipeline { - quadtree: WQuadtree, + quadtree: WQuadtree, tree_built: bool, dilation_factor: f32, } @@ -32,9 +32,10 @@ impl QueryPipeline { /// Update the acceleration structure on the query pipeline. pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { if !self.tree_built { - self.quadtree - .clear_and_rebuild(colliders, self.dilation_factor); - // self.tree_built = true; // FIXME: uncomment this once we handle insertion/removals properly. + let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + // FIXME: uncomment this once we handle insertion/removals properly. + // self.tree_built = true; return; } From cf86ee40a199dc84f7b8dd045e57b3a15bca79fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 11:21:39 +0200 Subject: [PATCH 20/29] Use the WQuadtree for the exhaustive ray-cast too. --- src/geometry/wquadtree.rs | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 627ad9a..fe1ba2a 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -309,11 +309,9 @@ impl WQuadtree { // FIXME: implement a visitor pattern to merge intersect_aabb // and intersect_ray into a single method. - pub fn intersect_aabb(&self, aabb: &AABB) -> Vec { - let mut res = Vec::new(); - + pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec) { if self.nodes.is_empty() { - return res; + return; } // Special case for the root. @@ -330,7 +328,7 @@ impl WQuadtree { // We found a leaf! // Unfortunately, invalid AABBs return a intersection as well. if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { - res.push(proxy.data); + out.push(proxy.data); } } else { // Internal node, visit the child. @@ -343,15 +341,11 @@ impl WQuadtree { } } } - - res } - pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec { - let mut res = Vec::new(); - + pub fn cast_ray(&self, ray: &Ray, max_toi: f32, out: &mut Vec) { if self.nodes.is_empty() { - return res; + return; } // Special case for the root. @@ -369,7 +363,7 @@ impl WQuadtree { // 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.data); + out.push(proxy.data); } } else { // Internal node, visit the child. @@ -382,8 +376,6 @@ impl WQuadtree { } } } - - res } } From 17c31bcc57ec8d037ba6cc6ab5f7cfb6fa4bb09b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 11:22:51 +0200 Subject: [PATCH 21/29] WQuadtree query: reduce the amount of allocations. --- .../trimesh_shape_contact_generator.rs | 5 ++++- .../trimesh_shape_proximity_detector.rs | 5 ++++- src/pipeline/query_pipeline.rs | 20 +++++++++---------- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index d759ae3..7dc0054 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -108,7 +108,10 @@ fn do_generate_contacts( // workspace.old_manifolds.len() // ); - workspace.interferences = trimesh1.waabbs().intersect_aabb(&local_aabb2); + workspace.interferences.clear(); + trimesh1 + .waabbs() + .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index 0a3ff44..1fc5e1e 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -70,7 +70,10 @@ fn do_detect_proximity( &mut workspace.interferences, ); - workspace.interferences = trimesh1.waabbs().intersect_aabb(&local_aabb2); + workspace.interferences.clear(); + trimesh1 + .waabbs() + .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 10aa7ed..c5f0ded 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -64,15 +64,10 @@ impl QueryPipeline { ray: &Ray, max_toi: f32, ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { - // let t0 = instant::now(); - let inter = self.quadtree.cast_ray(ray, max_toi); - // println!( - // "Found {} interferences in time {}.", - // inter.len(), - // instant::now() - t0 - // ); + // TODO: avoid allocation? + let mut inter = Vec::new(); + self.quadtree.cast_ray(ray, max_toi, &mut inter); - // let t0 = instant::now(); let mut best = f32::MAX; let mut result = None; @@ -85,7 +80,6 @@ impl QueryPipeline { } } } - // println!("Cast time: {}", instant::now() - t0); result } @@ -107,8 +101,12 @@ impl QueryPipeline { 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() { + // TODO: avoid allocation? + let mut inter = Vec::new(); + self.quadtree.cast_ray(ray, max_toi, &mut inter); + + for handle in inter { + let collider = &colliders[handle]; if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { if !callback(handle, collider, inter) { return; From 60c4d01e0a756ce0af142ac698fd0adcb8c22042 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 11:37:48 +0200 Subject: [PATCH 22/29] Completely remove the WAABBHierarchy structure. It is now replaced by the WQuadtree. --- src/geometry/broad_phase.rs | 255 ------------------ src/geometry/broad_phase_multi_sap.rs | 37 ++- .../trimesh_shape_contact_generator.rs | 2 +- src/geometry/mod.rs | 4 +- .../trimesh_shape_proximity_detector.rs | 2 +- 5 files changed, 39 insertions(+), 261 deletions(-) delete mode 100644 src/geometry/broad_phase.rs diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs deleted file mode 100644 index a1333d0..0000000 --- a/src/geometry/broad_phase.rs +++ /dev/null @@ -1,255 +0,0 @@ -use crate::geometry::ColliderHandle; -use ncollide::bounding_volume::AABB; -#[cfg(feature = "simd-is-enabled")] -use { - crate::geometry::WAABB, - crate::math::{Point, SIMD_WIDTH}, - crate::utils::WVec, - simba::simd::SimdBool as _, -}; - -#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct ColliderPair { - pub collider1: ColliderHandle, - pub collider2: ColliderHandle, -} - -impl ColliderPair { - pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { - ColliderPair { - collider1, - collider2, - } - } - - pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { - if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { - Self::new(collider1, collider2) - } else { - Self::new(collider2, collider1) - } - } - - pub fn swap(self) -> Self { - Self::new(self.collider2, self.collider1) - } - - pub fn zero() -> Self { - Self { - collider1: ColliderHandle::from_raw_parts(0, 0), - collider2: ColliderHandle::from_raw_parts(0, 0), - } - } -} - -pub struct WAABBHierarchyIntersections { - curr_level_interferences: Vec, - next_level_interferences: Vec, -} - -impl WAABBHierarchyIntersections { - pub fn new() -> Self { - Self { - curr_level_interferences: Vec::new(), - next_level_interferences: Vec::new(), - } - } - - pub fn computed_interferences(&self) -> &[usize] { - &self.curr_level_interferences[..] - } - - pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec { - &mut self.curr_level_interferences - } -} - -#[cfg(feature = "simd-is-enabled")] -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct WAABBHierarchy { - levels: Vec>, -} - -#[cfg(feature = "simd-is-enabled")] -impl WAABBHierarchy { - pub fn new(aabbs: &[AABB]) -> Self { - let mut waabbs: Vec<_> = aabbs - .chunks_exact(SIMD_WIDTH) - .map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH])) - .collect(); - - if aabbs.len() % SIMD_WIDTH != 0 { - let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH; - let last_i = aabbs.len() - 1; - let last_waabb = - WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]); - waabbs.push(last_waabb); - } - - let mut levels = vec![waabbs]; - - loop { - let last_level = levels.last().unwrap(); - let mut next_level = Vec::new(); - - for chunk in last_level.chunks_exact(SIMD_WIDTH) { - let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]); - let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]); - next_level.push(WAABB::new(mins, maxs)); - } - - // Deal with the last non-exact chunk. - if last_level.len() % SIMD_WIDTH != 0 { - let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH; - let last_id = last_level.len() - 1; - let mins = array![|ii| last_level[(first_id + ii).min(last_id)] - .mins - .horizontal_inf(); SIMD_WIDTH]; - let maxs = array![|ii| last_level[(first_id + ii).min(last_id)] - .maxs - .horizontal_sup(); SIMD_WIDTH]; - - let mins = Point::from(mins); - let maxs = Point::from(maxs); - next_level.push(WAABB::new(mins, maxs)); - } - - if next_level.len() == 1 { - levels.push(next_level); - break; - } - - levels.push(next_level); - } - - Self { levels } - } - - pub fn compute_interferences_with( - &self, - aabb: AABB, - workspace: &mut WAABBHierarchyIntersections, - ) { - let waabb1 = WAABB::splat(aabb); - workspace.next_level_interferences.clear(); - workspace.curr_level_interferences.clear(); - workspace.curr_level_interferences.push(0); - - for level in self.levels.iter().rev() { - for i in &workspace.curr_level_interferences { - // This `if let` handle the case when `*i` is out of bounds because - // the initial number of aabbs was not a power of SIMD_WIDTH. - if let Some(waabb2) = level.get(*i) { - // NOTE: using `intersect.bitmask()` and performing bit comparisons - // is much more efficient than testing if each intersect.extract(i) is true. - let intersect = waabb1.intersects(waabb2); - let bitmask = intersect.bitmask(); - - for j in 0..SIMD_WIDTH { - if (bitmask & (1 << j)) != 0 { - workspace.next_level_interferences.push(i * SIMD_WIDTH + j) - } - } - } - } - - std::mem::swap( - &mut workspace.curr_level_interferences, - &mut workspace.next_level_interferences, - ); - workspace.next_level_interferences.clear(); - } - } -} - -#[cfg(not(feature = "simd-is-enabled"))] -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct WAABBHierarchy { - levels: Vec>>, -} - -#[cfg(not(feature = "simd-is-enabled"))] -impl WAABBHierarchy { - const GROUP_SIZE: usize = 4; - - pub fn new(aabbs: &[AABB]) -> Self { - use ncollide::bounding_volume::BoundingVolume; - - let mut levels = vec![aabbs.to_vec()]; - - loop { - let last_level = levels.last().unwrap(); - let mut next_level = Vec::new(); - - for chunk in last_level.chunks(Self::GROUP_SIZE) { - let mut merged = chunk[0]; - for aabb in &chunk[1..] { - merged.merge(aabb) - } - - next_level.push(merged); - } - - if next_level.len() == 1 { - levels.push(next_level); - break; - } - - levels.push(next_level); - } - - Self { levels } - } - - pub fn compute_interferences_with( - &self, - aabb1: AABB, - workspace: &mut WAABBHierarchyIntersections, - ) { - use ncollide::bounding_volume::BoundingVolume; - - workspace.next_level_interferences.clear(); - workspace.curr_level_interferences.clear(); - workspace.curr_level_interferences.push(0); - - for level in self.levels[1..].iter().rev() { - for i in &workspace.curr_level_interferences { - for j in 0..Self::GROUP_SIZE { - if let Some(aabb2) = level.get(*i + j) { - if aabb1.intersects(aabb2) { - workspace - .next_level_interferences - .push((i + j) * Self::GROUP_SIZE) - } - } - } - } - - std::mem::swap( - &mut workspace.curr_level_interferences, - &mut workspace.next_level_interferences, - ); - workspace.next_level_interferences.clear(); - } - - // Last level. - for i in &workspace.curr_level_interferences { - for j in 0..Self::GROUP_SIZE { - if let Some(aabb2) = self.levels[0].get(*i + j) { - if aabb1.intersects(aabb2) { - workspace.next_level_interferences.push(i + j) - } - } - } - } - - std::mem::swap( - &mut workspace.curr_level_interferences, - &mut workspace.next_level_interferences, - ); - workspace.next_level_interferences.clear(); - } -} diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 5c80e0e..0add089 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,6 +1,6 @@ use crate::data::pubsub::PubSubCursor; use crate::dynamics::RigidBodySet; -use crate::geometry::{Collider, ColliderHandle, ColliderPair, ColliderSet, RemovedCollider}; +use crate::geometry::{Collider, ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; #[cfg(feature = "enhanced-determinism")] use crate::utils::FxHashMap32 as HashMap; @@ -16,6 +16,41 @@ const NEXT_FREE_SENTINEL: u32 = u32::MAX; const SENTINEL_VALUE: f32 = f32::MAX; const CELL_WIDTH: f32 = 20.0; +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderPair { + pub collider1: ColliderHandle, + pub collider2: ColliderHandle, +} + +impl ColliderPair { + pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + ColliderPair { + collider1, + collider2, + } + } + + pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { + Self::new(collider1, collider2) + } else { + Self::new(collider2, collider1) + } + } + + pub fn swap(self) -> Self { + Self::new(self.collider2, self.collider1) + } + + pub fn zero() -> Self { + Self { + collider1: ColliderHandle::from_raw_parts(0, 0), + collider2: ColliderHandle::from_raw_parts(0, 0), + } + } +} + pub enum BroadPhasePairEvent { AddPair(ColliderPair), DeletePair(ColliderPair), diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 7dc0054..52ba9b7 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyIntersections}; +use crate::geometry::{Collider, ContactManifold, Shape, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4456961..562f962 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -43,8 +43,7 @@ pub type RayIntersection = ncollide::query::RayIntersection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; -pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections}; -pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent; +pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; @@ -58,7 +57,6 @@ pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; mod ball; -mod broad_phase; mod broad_phase_multi_sap; mod capsule; mod collider; diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index 1fc5e1e..edf3085 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, Shape, Trimesh, WAABBHierarchyIntersections}; +use crate::geometry::{Collider, Proximity, Shape, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { From 7c92848383b9f76bde010f00683a44453b2b456a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 14:17:18 +0200 Subject: [PATCH 23/29] Implement RayCast for the Trimesh. --- src/geometry/collider.rs | 4 +++- src/geometry/trimesh.rs | 52 +++++++++++++++++++++++++++++++++++++++- 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 183446b..23345f7 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -132,7 +132,9 @@ impl Shape { Shape::Triangle(triangle) => { triangle.toi_and_normal_with_ray(position, ray, max_toi, true) } - Shape::Trimesh(_trimesh) => None, + Shape::Trimesh(trimesh) => { + trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) + } Shape::HeightField(heightfield) => { heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) } diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index f9e6034..dd8cb4b 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,5 +1,6 @@ -use crate::geometry::{Triangle, WQuadtree}; +use crate::geometry::{Ray, RayIntersection, Triangle, WQuadtree}; use crate::math::{Isometry, Point}; +use crate::ncollide::query::RayCast; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; @@ -103,3 +104,52 @@ impl Trimesh { } } } + +impl RayCast for Trimesh { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option { + // FIXME: do a best-first search. + let mut intersections = Vec::new(); + let ls_ray = ray.inverse_transform_by(m); + self.wquadtree + .cast_ray(&ls_ray, max_toi, &mut intersections); + let mut best: Option = None; + + for inter in intersections { + let tri = self.triangle(inter); + if let Some(inter) = tri.toi_and_normal_with_ray(m, ray, max_toi, solid) { + if let Some(curr) = &mut best { + if curr.toi > inter.toi { + *curr = inter; + } + } else { + best = Some(inter); + } + } + } + + best + } + + fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: f32) -> bool { + // FIXME: do a best-first search. + let mut intersections = Vec::new(); + let ls_ray = ray.inverse_transform_by(m); + self.wquadtree + .cast_ray(&ls_ray, max_toi, &mut intersections); + + for inter in intersections { + let tri = self.triangle(inter); + if tri.intersects_ray(m, ray, max_toi) { + return true; + } + } + + false + } +} From 682ff61f94931ef205a9f81e7d00417ac88537c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 15:23:48 +0200 Subject: [PATCH 24/29] Don't let the PubSub internal offsets overflow + fix some warnings. --- src/data/pubsub.rs | 67 ++++++++++++++++++++++----- src/dynamics/rigid_body_set.rs | 2 - src/geometry/broad_phase_multi_sap.rs | 6 +-- src/geometry/collider_set.rs | 2 +- src/geometry/narrow_phase.rs | 8 ++-- src/geometry/waabb.rs | 6 +-- src/geometry/wquadtree.rs | 3 ++ src/pipeline/collision_pipeline.rs | 2 +- src/pipeline/physics_pipeline.rs | 6 +-- src/pipeline/query_pipeline.rs | 6 +-- src_testbed/engine.rs | 2 +- 11 files changed, 73 insertions(+), 37 deletions(-) diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs index 1ac7498..0a3432a 100644 --- a/src/data/pubsub.rs +++ b/src/data/pubsub.rs @@ -3,16 +3,28 @@ use serde::export::PhantomData; use std::collections::VecDeque; -/// The position of a subscriber on a pub-sub queue. +/// A permanent subscription to a pub-sub queue. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct PubSubCursor { - // Index of the next message to read. +pub struct Subscription { + // Position on the cursor array. id: u32, - next: u32, _phantom: PhantomData, } -impl PubSubCursor { +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct PubSubCursor { + // Position on the offset array. + id: u32, + // Index of the next message to read. + // NOTE: Having this here is not actually necessary because + // this value is supposed to be equal to `offsets[self.id]`. + // However, we keep it because it lets us avoid one lookup + // on the `offsets` array inside of message-polling loops + // based on `read_ith`. + next: u32, +} + +impl PubSubCursor { fn id(&self, num_deleted: u32) -> usize { (self.id - num_deleted) as usize } @@ -29,6 +41,7 @@ pub struct PubSub { deleted_offsets: u32, messages: VecDeque, offsets: VecDeque, + cursors: Vec, } impl PubSub { @@ -39,9 +52,24 @@ impl PubSub { deleted_messages: 0, messages: VecDeque::new(), offsets: VecDeque::new(), + cursors: Vec::new(), } } + fn reset_shifts(&mut self) { + for offset in &mut self.offsets { + *offset -= self.deleted_messages; + } + + for cursor in &mut self.cursors { + cursor.id -= self.deleted_offsets; + cursor.next -= self.deleted_messages; + } + + self.deleted_offsets = 0; + self.deleted_messages = 0; + } + /// Publish a new message. pub fn publish(&mut self, message: T) { if self.offsets.is_empty() { @@ -55,25 +83,33 @@ impl PubSub { /// Subscribe to the queue. /// /// A subscription cannot be cancelled. - pub fn subscribe(&mut self) -> PubSubCursor { + #[must_use] + pub fn subscribe(&mut self) -> Subscription { let cursor = PubSubCursor { next: self.messages.len() as u32 + self.deleted_messages, id: self.offsets.len() as u32 + self.deleted_offsets, + }; + + let subscription = Subscription { + id: self.cursors.len() as u32, _phantom: PhantomData, }; self.offsets.push_back(cursor.next); - cursor + self.cursors.push(cursor); + subscription } /// Read the i-th message not yet read by the given subsciber. - pub fn read_ith(&self, cursor: &PubSubCursor, i: usize) -> Option<&T> { + pub fn read_ith(&self, sub: &Subscription, i: usize) -> Option<&T> { + let cursor = &self.cursors[sub.id as usize]; self.messages .get(cursor.next(self.deleted_messages) as usize + i) } /// Get the messages not yet read by the given subscriber. - pub fn read(&self, cursor: &PubSubCursor) -> impl Iterator { + pub fn read(&self, sub: &Subscription) -> impl Iterator { + let cursor = &self.cursors[sub.id as usize]; let next = cursor.next(self.deleted_messages); // TODO: use self.queue.range(next..) once it is stabilised. @@ -86,11 +122,14 @@ impl PubSub { /// Makes the given subscribe acknowledge all the messages in the queue. /// /// A subscriber cannot read acknowledged messages any more. - pub fn ack(&mut self, cursor: &mut PubSubCursor) { + pub fn ack(&mut self, sub: &Subscription) { // Update the cursor. - cursor.next = self.messages.len() as u32 + self.deleted_messages; + let cursor = &mut self.cursors[sub.id as usize]; + self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX; cursor.id = self.offsets.len() as u32 + self.deleted_offsets; + + cursor.next = self.messages.len() as u32 + self.deleted_messages; self.offsets.push_back(cursor.next); // Now clear the messages we don't need to @@ -110,6 +149,12 @@ impl PubSub { } self.deleted_messages += num_to_delete; + + if self.deleted_messages > u32::MAX / 2 || self.deleted_offsets > u32::MAX / 2 { + // Don't let the deleted_* shifts grow indefinitely otherwise + // they will end up overflowing, breaking everything. + self.reset_shifts(); + } } } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 7906083..83f1c51 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,11 +2,9 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::data::pubsub::PubSub; use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody}; 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. diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 0add089..d85eae3 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,6 +1,6 @@ -use crate::data::pubsub::PubSubCursor; +use crate::data::pubsub::Subscription; use crate::dynamics::RigidBodySet; -use crate::geometry::{Collider, ColliderHandle, ColliderSet, RemovedCollider}; +use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; #[cfg(feature = "enhanced-determinism")] use crate::utils::FxHashMap32 as HashMap; @@ -417,7 +417,7 @@ impl SAPRegion { pub struct BroadPhase { proxies: Proxies, regions: HashMap, SAPRegion>, - removed_colliders: Option>, + removed_colliders: Option>, deleted_any: bool, // We could think serializing this workspace is useless. // It turns out is is important to serialize at least its capacity diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index dbdd2a4..5ac9658 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -83,7 +83,7 @@ impl ColliderSet { /* * Delete the collider from its parent body. */ - if let Some(mut parent) = bodies.get_mut_internal(collider.parent) { + if let Some(parent) = bodies.get_mut_internal(collider.parent) { parent.remove_collider_internal(handle, &collider); bodies.wake_up(collider.parent, true); } diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 60c5e1a..ebe0a79 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -14,13 +14,13 @@ use crate::geometry::proximity_detector::{ // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, Collider, ColliderGraphIndex, ColliderHandle, ContactEvent, - ProximityEvent, ProximityPair, RemovedCollider, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent, + ProximityPair, RemovedCollider, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] //use crate::math::{SimdFloat, SIMD_WIDTH}; -use crate::data::pubsub::PubSubCursor; +use crate::data::pubsub::Subscription; use crate::ncollide::query::Proximity; use crate::pipeline::EventHandler; use std::collections::HashMap; @@ -31,7 +31,7 @@ use std::collections::HashMap; pub struct NarrowPhase { contact_graph: InteractionGraph, proximity_graph: InteractionGraph, - removed_colliders: Option>, + removed_colliders: Option>, // ball_ball: Vec, // Workspace: Vec<*mut ContactPair>, // shape_shape: Vec, // Workspace: Vec<*mut ContactPair>, // ball_ball_prox: Vec, // Workspace: Vec<*mut ProximityPair>, diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index cc420d9..645ac04 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -93,10 +93,6 @@ impl<'de> serde::Deserialize<'de> for WAABB { } impl WAABB { - pub fn new(mins: Point, maxs: Point) -> Self { - Self { mins, maxs } - } - pub fn new_invalid() -> Self { Self::splat(AABB::new_invalid()) } @@ -132,7 +128,7 @@ impl WAABB { 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])); + 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 = diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index fe1ba2a..233ebd1 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -379,11 +379,13 @@ impl WQuadtree { } } +#[allow(dead_code)] struct WQuadtreeIncrementalBuilderStep { range: Range, parent: NodeIndex, } +#[allow(dead_code)] struct WQuadtreeIncrementalBuilder { quadtree: WQuadtree, to_insert: Vec, @@ -391,6 +393,7 @@ struct WQuadtreeIncrementalBuilder { indices: Vec, } +#[allow(dead_code)] impl WQuadtreeIncrementalBuilder { pub fn new() -> Self { Self { diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 2283fa7..5a19e52 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,6 +1,6 @@ //! Physics pipeline structures. -use crate::dynamics::{JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{JointSet, RigidBodySet}; use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; use crate::pipeline::EventHandler; diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f00fa8d..4a39f79 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -1,15 +1,13 @@ //! Physics pipeline structures. use crate::counters::Counters; -use crate::data::pubsub::PubSubCursor; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; -use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet, - ContactManifoldIndex, NarrowPhase, RemovedCollider, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, }; use crate::math::Vector; use crate::pipeline::EventHandler; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index c5f0ded..32f59fc 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,9 +1,5 @@ use crate::dynamics::RigidBodySet; -use crate::geometry::{ - Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree, AABB, WAABB, -}; -use crate::math::{Point, Vector}; -use ncollide::bounding_volume::BoundingVolume; +use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree}; /// A pipeline for performing queries on all the colliders of a scene. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 27a8510..217da48 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -609,7 +609,7 @@ impl GraphicsManager { } } - pub fn draw(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) { + pub fn draw(&mut self, _bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) { // use kiss3d::camera::Camera; // println!( // "camera eye {:?}, at: {:?}", From c5a8e03df7b3009b24e7c272fafcc49b4bc4e829 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 15:23:58 +0200 Subject: [PATCH 25/29] Use simba ^0.2.1 --- Cargo.toml | 1 + build/rapier2d/Cargo.toml | 2 +- build/rapier3d/Cargo.toml | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index b4ce57e..944fa40 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,6 +4,7 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark [patch.crates-io] #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" } +#simba = { path = "../simba" } [profile.release] #debug = true diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index ec3a026..890dfe2 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -37,7 +37,7 @@ instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.22" ncollide2d = "0.24" -simba = "0.2" +simba = "^0.2.1" approx = "0.3" rayon = { version = "1", optional = true } crossbeam = "0.7" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index dcf5aca..298d655 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -37,7 +37,7 @@ instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.22" ncollide3d = "0.24" -simba = "0.2" +simba = "^0.2.1" approx = "0.3" rayon = { version = "1", optional = true } crossbeam = "0.7" From e87b73a2a20fee1ed333d564ba46dbf1c3ca75e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 15:49:22 +0200 Subject: [PATCH 26/29] Fix compilation in 2D. --- CHANGELOG | 6 +++++- examples2d/add_remove2.rs | 11 +++-------- src/geometry/collider.rs | 3 ++- src/geometry/trimesh.rs | 10 ++++++++-- src/geometry/wquadtree.rs | 6 +++++- src/pipeline/physics_pipeline.rs | 16 ++-------------- src_testbed/box2d_backend.rs | 12 ++++++------ src_testbed/testbed.rs | 2 +- 8 files changed, 32 insertions(+), 34 deletions(-) diff --git a/CHANGELOG b/CHANGELOG index 6c01cce..70b20e5 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -1,6 +1,10 @@ ## v0.2.0 - WIP +The most significant change on this version is the addition of the `QueryPipeline` responsible for performing +scene-wide queries. So far only ray-casting has been implemented. -- Add `PhysicsPipeline::remove_collider(...)` to remove a collider from the `ColliderSet`. +- Add `ColliderSet::remove(...)` to remove a collider from the `ColliderSet`. +- Replace `PhysicsPipeline::remove_rigid_body` by `RigidBodySet::remove`. +- The `JointSet.iter()` now returns an iterator yielding `(JointHandle, &Joint)` instead of just `&Joint`. - Add `ColliderDesc::translation(...)` to set the translation of a collider relative to the rigid-body it is attached to. - Add `ColliderDesc::rotation(...)` to set the rotation of a collider relative to the rigid-body it is attached to. - Add `ColliderDesc::position(...)` to set the position of a collider relative to the rigid-body it is attached to. diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs index 3a32a95..67075fe 100644 --- a/examples2d/add_remove2.rs +++ b/examples2d/add_remove2.rs @@ -28,14 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { .map(|e| e.0) .collect(); for handle in to_remove { - physics.pipeline.remove_rigid_body( - handle, - &mut physics.broad_phase, - &mut physics.narrow_phase, - &mut physics.bodies, - &mut physics.colliders, - &mut physics.joints, - ); + physics + .bodies + .remove(handle, &mut physics.colliders, &mut physics.joints); graphics.remove_body_nodes(window, handle); } }); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 23345f7..7c293b6 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -124,7 +124,7 @@ impl Shape { } Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), #[cfg(feature = "dim2")] - Shape::Triangle(triangle) => { + Shape::Triangle(_) | Shape::Trimesh(_) => { // This is not implemented yet in 2D. None } @@ -132,6 +132,7 @@ impl Shape { Shape::Triangle(triangle) => { triangle.toi_and_normal_with_ray(position, ray, max_toi, true) } + #[cfg(feature = "dim3")] Shape::Trimesh(trimesh) => { trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) } diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index dd8cb4b..b6e23e7 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,9 +1,14 @@ -use crate::geometry::{Ray, RayIntersection, Triangle, WQuadtree}; +use crate::geometry::{Triangle, WQuadtree}; use crate::math::{Isometry, Point}; -use crate::ncollide::query::RayCast; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +#[cfg(feature = "dim3")] +use { + crate::geometry::{Ray, RayIntersection}, + ncollide::query::RayCast, +}; + #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A triangle mesh. @@ -105,6 +110,7 @@ impl Trimesh { } } +#[cfg(feature = "dim3")] impl RayCast for Trimesh { fn toi_and_normal_with_ray( &self, diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index 233ebd1..fce04eb 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -1,6 +1,8 @@ use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB}; use crate::geometry::{WRay, WAABB}; -use crate::math::{Point, Vector}; +use crate::math::Point; +#[cfg(feature = "dim3")] +use crate::math::Vector; use crate::simd::{SimdFloat, SIMD_WIDTH}; use ncollide::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; @@ -252,6 +254,7 @@ impl WQuadtree { // Find the axis with minimum variance. This is the axis along // which we are **not** subdividing our set. + #[allow(unused_mut)] // Does not need to be mutable in 2D. let mut subdiv_dims = [0, 1]; #[cfg(feature = "dim3")] { @@ -466,6 +469,7 @@ impl WQuadtreeIncrementalBuilder { // Find the axis with minimum variance. This is the axis along // which we are **not** subdividing our set. + #[allow(unused_mut)] // Does not need to be mutable in 2D. let mut subdiv_dims = [0, 1]; #[cfg(feature = "dim3")] { diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 4a39f79..462b341 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -58,18 +58,6 @@ impl PhysicsPipeline { } } - /// Remove this. - pub fn maintain( - &mut self, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - ) { - broad_phase.maintain(colliders); - narrow_phase.maintain(colliders, bodies); - } - /// Executes one timestep of the physics simulation. pub fn step( &mut self, @@ -82,9 +70,9 @@ impl PhysicsPipeline { joints: &mut JointSet, events: &dyn EventHandler, ) { - // println!("Step"); self.counters.step_started(); - self.maintain(broad_phase, narrow_phase, bodies, colliders); + broad_phase.maintain(colliders); + narrow_phase.maintain(colliders, bodies); bodies.maintain_active_set(); // Update kinematic bodies velocities. diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index 07cef1e..c25ff1f 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -98,10 +98,10 @@ impl Box2dWorld { fn insert_joints(&mut self, joints: &JointSet) { for joint in joints.iter() { - let body_a = self.rapier2box2d[&joint.body1]; - let body_b = self.rapier2box2d[&joint.body2]; + let body_a = self.rapier2box2d[&joint.1.body1]; + let body_b = self.rapier2box2d[&joint.1.body2]; - match &joint.params { + match &joint.1.params { JointParams::BallJoint(params) => { let def = RevoluteJointDef { body_a, @@ -158,7 +158,7 @@ impl Box2dWorld { } fn create_fixture(collider: &Collider, body: &mut b2::MetaBody) { - let center = na_vec_to_b2_vec(collider.delta().translation.vector); + let center = na_vec_to_b2_vec(collider.position_wrt_parent().translation.vector); let mut fixture_def = b2::FixtureDef::new(); fixture_def.restitution = 0.0; @@ -182,7 +182,7 @@ impl Box2dWorld { let points: Vec<_> = poly .vertices() .iter() - .map(|p| collider.delta() * p) + .map(|p| collider.position_wrt_parent() * p) .map(|p| na_vec_to_b2_vec(p.coords)) .collect(); let b2_shape = b2::PolygonShape::new_with(&points); @@ -229,7 +229,7 @@ impl Box2dWorld { for coll_handle in body.colliders() { let collider = &mut colliders[*coll_handle]; - collider.set_position_debug(pos * collider.delta()); + collider.set_position_debug(pos * collider.position_wrt_parent()); } } } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index c145af5..3d7fd7d 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -1160,7 +1160,7 @@ impl Testbed { } #[cfg(feature = "dim2")] - fn highlight_hovered_body(&mut self, window: &Window) { + fn highlight_hovered_body(&mut self, _window: &Window) { // Do nothing for now. } From 8c388687935b0f09a6a03b9de96fc80d15dd67c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 16:02:15 +0200 Subject: [PATCH 27/29] Fix bogus PhantomData import. --- src/data/pubsub.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs index 0a3432a..b2c9e27 100644 --- a/src/data/pubsub.rs +++ b/src/data/pubsub.rs @@ -1,7 +1,7 @@ //! Publish-subscribe mechanism for internal events. -use serde::export::PhantomData; use std::collections::VecDeque; +use std::marker::PhantomData; /// A permanent subscription to a pub-sub queue. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] From fc72aa6845a21b3af5d862968eddd3116036ac42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 16:32:24 +0200 Subject: [PATCH 28/29] Fix examples. --- src/geometry/broad_phase_multi_sap.rs | 14 ++------- src/pipeline/physics_pipeline.rs | 42 ++++++++++----------------- 2 files changed, 18 insertions(+), 38 deletions(-) diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index d85eae3..8e69d24 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -700,16 +700,13 @@ impl BroadPhase { mod test { use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; - use crate::pipeline::PhysicsPipeline; #[test] fn test_add_update_remove() { let mut broad_phase = BroadPhase::new(); - let mut narrow_phase = NarrowPhase::new(); let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); let mut joints = JointSet::new(); - let mut pipeline = PhysicsPipeline::new(); let rb = RigidBodyBuilder::new_dynamic().build(); let co = ColliderBuilder::ball(0.5).build(); @@ -718,15 +715,8 @@ mod test { broad_phase.update_aabbs(0.0, &bodies, &mut colliders); - pipeline.remove_rigid_body( - hrb, - &mut broad_phase, - &mut narrow_phase, - &mut bodies, - &mut colliders, - &mut joints, - ); - + bodies.remove(hrb, &mut colliders, &mut joints); + broad_phase.maintain(&mut colliders); broad_phase.update_aabbs(0.0, &bodies, &mut colliders); // Create another body. diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 462b341..47fd260 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -316,14 +316,7 @@ mod test { 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, - ); + bodies.remove(*h, &mut colliders, &mut joints); } pipeline.step( @@ -342,30 +335,27 @@ mod test { fn rigid_body_removal_snapshot_handle_determinism() { 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 set = RigidBodySet::new(); + let mut bodies = RigidBodySet::new(); let rb = RigidBodyBuilder::new_dynamic().build(); - let h1 = set.insert(rb.clone()); - let h2 = set.insert(rb.clone()); - let h3 = set.insert(rb.clone()); + let h1 = bodies.insert(rb.clone()); + let h2 = bodies.insert(rb.clone()); + let h3 = bodies.insert(rb.clone()); - pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); - pipeline.remove_rigid_body(h3, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); - pipeline.remove_rigid_body(h2, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + bodies.remove(h1, &mut colliders, &mut joints); + bodies.remove(h3, &mut colliders, &mut joints); + bodies.remove(h2, &mut colliders, &mut joints); - let ser_set = bincode::serialize(&set).unwrap(); - let mut set2: RigidBodySet = bincode::deserialize(&ser_set).unwrap(); + let ser_bodies = bincode::serialize(&bodies).unwrap(); + let mut bodies2: RigidBodySet = bincode::deserialize(&ser_bodies).unwrap(); - let h1a = set.insert(rb.clone()); - let h2a = set.insert(rb.clone()); - let h3a = set.insert(rb.clone()); + let h1a = bodies.insert(rb.clone()); + let h2a = bodies.insert(rb.clone()); + let h3a = bodies.insert(rb.clone()); - let h1b = set2.insert(rb.clone()); - let h2b = set2.insert(rb.clone()); - let h3b = set2.insert(rb.clone()); + let h1b = bodies2.insert(rb.clone()); + let h2b = bodies2.insert(rb.clone()); + let h3b = bodies2.insert(rb.clone()); assert_eq!(h1a, h1b); assert_eq!(h2a, h2b); From 25b8486ebf8bdfa0d165300a30877293e9e40c51 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 6 Oct 2020 16:32:12 +0200 Subject: [PATCH 29/29] CI: Conditionally set the commit for the bench message. --- .github/workflows/rapier-ci-bench.yml | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/.github/workflows/rapier-ci-bench.yml b/.github/workflows/rapier-ci-bench.yml index 89ed247..9cce1b2 100644 --- a/.github/workflows/rapier-ci-bench.yml +++ b/.github/workflows/rapier-ci-bench.yml @@ -9,20 +9,23 @@ on: jobs: send-bench-message: + env: + BENCHBOT_AMQP_USER: ${{ secrets.BENCHBOT_AMQP_USER }} + BENCHBOT_AMQP_PASS: ${{ secrets.BENCHBOT_AMQP_PASS }} + BENCHBOT_AMQP_VHOST: ${{ secrets.BENCHBOT_AMQP_VHOST }} + BENCHBOT_AMQP_HOST: ${{ secrets.BENCHBOT_AMQP_HOST }} + BENCHBOT_TARGET_REPO: ${{ github.repository }} + BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }} + BENCHBOT_SHA: ${{ github.sha }} + BENCHBOT_HEAD_REF: ${{github.head_ref}} runs-on: ubuntu-latest steps: - - name: Get the latest commit on PR - id: get-latest-commit - uses: ActionsRML/get-PR-latest-commit@v1 + - name: Find commit SHA + if: github.ref == 'refs/heads/master' + run: | + echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA" - name: Send 3D bench message shell: bash - env: - BENCHBOT_AMQP_USER: ${{ secrets.BENCHBOT_AMQP_USER }} - BENCHBOT_AMQP_PASS: ${{ secrets.BENCHBOT_AMQP_PASS }} - BENCHBOT_AMQP_VHOST: ${{ secrets.BENCHBOT_AMQP_VHOST }} - BENCHBOT_AMQP_HOST: ${{ secrets.BENCHBOT_AMQP_HOST }} - BENCHBOT_TARGET_REPO: ${{ github.repository }} - BENCHBOT_TARGET_COMMIT: ${{ steps.get-latest-commit.outputs.latest_commit_sha }} run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS -i -H "content-type:application/json" -X POST https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish