From 3c85a6ac41397cf95199933c6a93909bc070a844 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 8 Sep 2020 21:18:17 +0200 Subject: Start implementing ray-casting. This adds a QueryPipeline structure responsible for scene queries. Currently this structure is able to perform a brute-force ray-cast. This commit also includes the beginning of implementation of a SIMD-based acceleration structure which will be used for these scene queries in the future. --- src/pipeline/mod.rs | 2 + src/pipeline/query_pipeline.rs | 306 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 308 insertions(+) create mode 100644 src/pipeline/query_pipeline.rs (limited to 'src/pipeline') diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 6298d18..287de9d 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -3,7 +3,9 @@ pub use collision_pipeline::CollisionPipeline; pub use event_handler::{ChannelEventCollector, EventHandler}; pub use physics_pipeline::PhysicsPipeline; +pub use query_pipeline::QueryPipeline; mod collision_pipeline; mod event_handler; mod physics_pipeline; +mod query_pipeline; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs new file mode 100644 index 0000000..15caaef --- /dev/null +++ b/src/pipeline/query_pipeline.rs @@ -0,0 +1,306 @@ +use crate::dynamics::RigidBodySet; +use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, AABB, WAABB}; +use crate::math::{Point, Vector}; +use ncollide::bounding_volume::BoundingVolume; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +struct NodeIndex { + index: u32, // Index of the addressed node in the `children` array. + lane: u8, // SIMD lane of the addressed node. +} + +impl NodeIndex { + fn new(index: u32, lane: u8) -> Self { + Self { index, lane } + } + + fn invalid() -> Self { + Self { + index: u32::MAX, + lane: 0, + } + } +} + +#[derive(Copy, Clone, Debug)] +struct WAABBHierarchyNodeChildren { + waabb: WAABB, + // Index of the children of the 4 nodes represented by self. + // If this is a leaf, it contains the proxy ids instead. + grand_children: [u32; 4], + parent: NodeIndex, + leaf: bool, // TODO: pack this with the NodexIndex.lane? +} + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +struct ColliderNodeIndex { + node: NodeIndex, + handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here? +} + +impl ColliderNodeIndex { + fn invalid() -> Self { + Self { + node: NodeIndex::invalid(), + handle: ColliderSet::invalid_handle(), + } + } +} + +struct WAABBHierarchy { + children: Vec, + dirty: Vec, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism. + proxies: Vec, +} + +impl WAABBHierarchy { + pub fn new() -> Self { + WAABBHierarchy { + children: Vec::new(), + dirty: Vec::new(), + proxies: Vec::new(), + } + } + + pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) { + self.children.clear(); + self.dirty.clear(); + self.proxies.clear(); + + // Create proxies. + let mut indices = Vec::with_capacity(colliders.len()); + let mut proxies = vec![ColliderNodeIndex::invalid(); colliders.len()]; + for (handle, collider) in colliders.iter() { + let index = handle.into_raw_parts().0; + if proxies.len() < handle.into_raw_parts().0 { + proxies.resize(index + 1, ColliderNodeIndex::invalid()); + } + + proxies[index].handle = handle; + indices.push(index); + } + + // Compute AABBs. + let mut aabbs = vec![AABB::new_invalid(); proxies.len()]; + for (handle, collider) in colliders.iter() { + let index = handle.into_raw_parts().0; + let aabb = collider.compute_aabb(); + aabbs[index] = aabb; + } + + // Build the tree recursively. + let root_node = WAABBHierarchyNodeChildren { + waabb: WAABB::new_invalid(), + grand_children: [1; 4], + parent: NodeIndex::invalid(), + leaf: false, + }; + + self.children.push(root_node); + let root_id = NodeIndex::new(0, 0); + let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id); + self.children[0].waabb = WAABB::splat(aabb); + } + + fn do_recurse_build( + &mut self, + indices: &mut [usize], + aabbs: &[AABB], + parent: NodeIndex, + ) -> (u32, AABB) { + // Leaf case. + if indices.len() <= 4 { + let my_id = self.children.len(); + let mut my_aabb = AABB::new_invalid(); + let mut leaf_aabbs = [AABB::new_invalid(); 4]; + let mut proxy_ids = [u32::MAX; 4]; + + for (k, id) in indices.iter().enumerate() { + my_aabb.merge(&aabbs[*id]); + leaf_aabbs[k] = aabbs[*id]; + proxy_ids[k] = *id as u32; + } + + let node = WAABBHierarchyNodeChildren { + waabb: WAABB::from(leaf_aabbs), + grand_children: proxy_ids, + parent, + leaf: true, + }; + + self.children.push(node); + return (my_id as u32, my_aabb); + } + + // Compute the center and variance along each dimension. + // In 3D we compute the variance to not-subdivide the dimension with lowest variance. + // Therefore variance computation is not needed in 2D because we only have 2 dimension + // to split in the first place. + let mut center = Point::origin(); + #[cfg(feature = "dim3")] + let mut variance = Vector::zeros(); + + let denom = 1.0 / (indices.len() as f32); + + for i in &*indices { + let coords = aabbs[*i].center().coords; + center += coords * denom; + #[cfg(feature = "dim3")] + { + variance += coords.component_mul(&coords) * denom; + } + } + + #[cfg(feature = "dim3")] + { + variance = variance - center.coords.component_mul(¢er.coords); + } + + // Find the axis with minimum variance. This is the axis along + // which we are **not** subdividing our set. + let mut subdiv_dims = [0, 1]; + #[cfg(feature = "dim3")] + { + let min = variance.imin(); + subdiv_dims[0] = (min + 1) % 3; + subdiv_dims[1] = (min + 2) % 3; + } + + // Split the set along the two subdiv_dims dimensions. + // TODO: should we split wrt. the median instead of the average? + // TODO: we should ensure each subslice contains at least 4 elements each (or less if + // indices has less than 16 elements in the first place. + let (left, right) = split_indices_wrt_dim(indices, &aabbs, subdiv_dims[0]); + let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, subdiv_dims[1]); + let (right_bottom, right_top) = split_indices_wrt_dim(right, &aabbs, subdiv_dims[1]); + + let node = WAABBHierarchyNodeChildren { + waabb: WAABB::new_invalid(), + grand_children: [0; 4], // Will be set after the recursive call + parent, + leaf: false, + }; + + let id = self.children.len() as u32; + self.children.push(node); + + // Recurse! + let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0)); + let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1)); + let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2)); + let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3)); + + // Now we know the indices of the grand-children. + self.children[id as usize].grand_children = [a.0, b.0, c.0, d.0]; + self.children[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]); + + // TODO: will this chain of .merged be properly optimized? + let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&c.1); + (id, my_aabb) + } +} + +fn split_indices_wrt_dim<'a>( + indices: &'a mut [usize], + aabbs: &[AABB], + dim: usize, +) -> (&'a mut [usize], &'a mut [usize]) { + let mut icurr = 0; + let mut ilast = indices.len() - 1; + + // The loop condition we can just do 0..indices.len() + // instead of the test icurr < ilast because we know + // we will iterate exactly once per index. + for _ in 0..indices.len() { + let i = indices[icurr]; + let center = aabbs[i].center(); + + if center[dim] > center[dim] { + indices.swap(icurr, ilast); + ilast -= 1; + } else { + icurr += 1; + } + } + + indices.split_at_mut(icurr) +} + +/// A pipeline for performing queries on all the colliders of a scene. +pub struct QueryPipeline { + // hierarchy: WAABBHierarchy, +} + +impl Default for QueryPipeline { + fn default() -> Self { + Self::new() + } +} + +impl QueryPipeline { + /// Initializes an empty query pipeline. + pub fn new() -> Self { + Self { + // hierarchy: WAABBHierarchy::new(), + } + } + + /// Update the acceleration structure on the query pipeline. + pub fn update(&mut self, _bodies: &mut RigidBodySet, _colliders: &mut ColliderSet) {} + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + pub fn cast_ray<'a>( + &self, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: f32, + ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { + let mut best = f32::MAX; + let mut result = None; + + // FIXME: this is a brute-force approach. + for (handle, collider) in colliders.iter() { + if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if inter.toi < best { + best = inter.toi; + result = Some((handle, collider, inter)); + } + } + } + + result + } + + /// Find the all intersections between a ray and a set of collider and passes them to a callback. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + /// - `callback`: function executed on each collider for which a ray intersection has been found. + /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, + /// this method will exit early, ignory any further raycast. + pub fn interferences_with_ray<'a>( + &self, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: f32, + mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, + ) { + // FIXME: this is a brute-force approach. + for (handle, collider) in colliders.iter() { + if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if !callback(handle, collider, inter) { + return; + } + } + } + } +} -- cgit From e16b7722be23f7b6627bd54e174d7782d33c53fe Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Sep 2020 22:22:55 +0200 Subject: Fix crash caused by the removal of a kinematic body. --- src/pipeline/physics_pipeline.rs | 46 ++++++++++++++++++++++++++++++++++------ 1 file changed, 40 insertions(+), 6 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 8449477..192ca9a 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -298,8 +298,9 @@ impl PhysicsPipeline { #[cfg(test)] mod test { - use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; - use crate::geometry::{BroadPhase, ColliderSet, NarrowPhase}; + use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet}; + use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; + use crate::math::Vector; use crate::pipeline::PhysicsPipeline; #[test] @@ -310,12 +311,45 @@ mod test { let mut bf = BroadPhase::new(); let mut nf = NarrowPhase::new(); - let mut set = RigidBodySet::new(); - let rb = RigidBodyBuilder::new_dynamic().build(); + let mut bodies = RigidBodySet::new(); // Check that removing the body right after inserting it works. - let h1 = set.insert(rb.clone()); - pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + // We add two dynamic bodies, one kinematic body and one static body before removing + // them. This include a non-regression test where deleting a kimenatic body crashes. + let rb = RigidBodyBuilder::new_dynamic().build(); + let h1 = bodies.insert(rb.clone()); + let h2 = bodies.insert(rb.clone()); + + // The same but with a kinematic body. + let rb = RigidBodyBuilder::new_kinematic().build(); + let h3 = bodies.insert(rb.clone()); + + // The same but with a static body. + let rb = RigidBodyBuilder::new_static().build(); + let h4 = bodies.insert(rb.clone()); + + let to_delete = [h1, h2, h3, h4]; + for h in &to_delete { + pipeline.remove_rigid_body( + *h, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + ); + } + + pipeline.step( + &Vector::zeros(), + &IntegrationParameters::default(), + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + &(), + ); } #[test] -- cgit From 7b8e322446ffa36e3f47078e23eb61ef423175dc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 10:43:20 +0200 Subject: Make kinematic bodies properly wake up dynamic bodies. --- src/pipeline/collision_pipeline.rs | 2 +- src/pipeline/physics_pipeline.rs | 49 ++++++++++++++++++++++++++++++++------ 2 files changed, 43 insertions(+), 8 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 0184295..f30dae7 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -50,7 +50,7 @@ impl CollisionPipeline { self.broad_phase_events.clear(); broad_phase.find_pairs(&mut self.broad_phase_events); - narrow_phase.register_pairs(colliders, &self.broad_phase_events, events); + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events); narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 192ca9a..3fcd1af 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -75,6 +75,15 @@ impl PhysicsPipeline { self.counters.step_started(); bodies.maintain_active_set(); + // Update kinematic bodies velocities. + // TODO: what is the best place for this? It should at least be + // located before the island computation because we test the velocity + // there to determine if this kinematic body should wake-up dynamic + // bodies it is touching. + bodies.foreach_active_kinematic_body_mut_internal(|_, body| { + body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + }); + self.counters.stages.collision_detection_time.start(); self.counters.cd.broad_phase_time.start(); self.broadphase_collider_pairs.clear(); @@ -91,7 +100,7 @@ impl PhysicsPipeline { broad_phase.find_pairs(&mut self.broad_phase_events); // println!("Find pairs time: {}", instant::now() - t); - narrow_phase.register_pairs(colliders, &self.broad_phase_events, events); + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); self.counters.cd.broad_phase_time.pause(); // println!("Num contact pairs: {}", pairs.len()); @@ -122,11 +131,6 @@ impl PhysicsPipeline { ); self.counters.stages.island_construction_time.pause(); - // Update kinematic bodies velocities. - bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); - }); - if self.manifold_indices.len() < bodies.num_islands() { self.manifold_indices .resize(bodies.num_islands(), Vec::new()); @@ -261,7 +265,7 @@ impl PhysicsPipeline { if let Some(parent) = bodies.get_mut_internal(collider.parent) { parent.remove_collider_internal(handle, &collider); - bodies.wake_up(collider.parent); + bodies.wake_up(collider.parent, true); } Some(collider) @@ -303,6 +307,37 @@ mod test { use crate::math::Vector; use crate::pipeline::PhysicsPipeline; + #[test] + fn kinematic_and_static_contact_crash() { + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + let mut pipeline = PhysicsPipeline::new(); + let mut bf = BroadPhase::new(); + let mut nf = NarrowPhase::new(); + let mut bodies = RigidBodySet::new(); + + let rb = RigidBodyBuilder::new_static().build(); + let h1 = bodies.insert(rb.clone()); + let co = ColliderBuilder::ball(10.0).build(); + colliders.insert(co.clone(), h1, &mut bodies); + + // The same but with a kinematic body. + let rb = RigidBodyBuilder::new_kinematic().build(); + let h2 = bodies.insert(rb.clone()); + colliders.insert(co, h2, &mut bodies); + + pipeline.step( + &Vector::zeros(), + &IntegrationParameters::default(), + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + &(), + ); + } + #[test] fn rigid_body_removal_before_step() { let mut colliders = ColliderSet::new(); -- cgit From 2dda0e5ce48ed0d93b4b0fa3098ba08f59a50a0a Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 17:26:57 +0200 Subject: Complete the WQuadtree construction and ray-cast. --- src/pipeline/query_pipeline.rs | 245 ++++------------------------------------- 1 file changed, 19 insertions(+), 226 deletions(-) (limited to 'src/pipeline') 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 } -- cgit From a7d77a01447d2b77694b2a957d000790af60b383 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 22 Sep 2020 15:29:29 +0200 Subject: Add non-topological WQuadtree update. --- src/pipeline/query_pipeline.rs | 34 ++++++++++++++++++++++++++-------- 1 file changed, 26 insertions(+), 8 deletions(-) (limited to 'src/pipeline') 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(), -- cgit From 52bbcc79fe1989f391e17b8c240a886bd8529d1f Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 22 Sep 2020 18:46:57 +0200 Subject: Query pipeline: aggressive workaround until we properly support collider addition/removal. --- src/pipeline/query_pipeline.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/pipeline') 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; } -- cgit From c031f96ac548645932c5605bfc17869618e9212b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 28 Sep 2020 10:04:56 +0200 Subject: Fix compilation when parallelism is not enabled. --- src/pipeline/query_pipeline.rs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src/pipeline') 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 } -- cgit From 2d0a888484dd296cc785caf978252dd97b58e10a Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 16:51:50 +0200 Subject: Make the query pipeline serializable. --- src/pipeline/query_pipeline.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/pipeline') 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 // ); -- cgit From 93aa7b6e1e8cbfd73542ed10ad5c26ae0a8b9848 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 19:04:18 +0200 Subject: Use the publish-subscribe mechanism to handle collider removals across pipelines. --- src/pipeline/collision_pipeline.rs | 24 -------------- src/pipeline/physics_pipeline.rs | 65 +++++++++----------------------------- 2 files changed, 15 insertions(+), 74 deletions(-) (limited to 'src/pipeline') 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)] -- cgit From 721db2d49e06de38a16320425f77986b308445dc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 10:02:21 +0200 Subject: Fix crash when deleting a collider. --- src/pipeline/physics_pipeline.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/pipeline') 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. -- cgit From 8e432b298bd71df7d1b776b77c15713d9bea280e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 10:46:59 +0200 Subject: Make the WQuadTree more generic and use it as the trimesh acceleration structure. --- src/pipeline/query_pipeline.rs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src/pipeline') 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; } -- cgit From 17c31bcc57ec8d037ba6cc6ab5f7cfb6fa4bb09b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 11:22:51 +0200 Subject: WQuadtree query: reduce the amount of allocations. --- src/pipeline/query_pipeline.rs | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) (limited to 'src/pipeline') 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; -- cgit From 682ff61f94931ef205a9f81e7d00417ac88537c1 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 15:23:48 +0200 Subject: Don't let the PubSub internal offsets overflow + fix some warnings. --- src/pipeline/collision_pipeline.rs | 2 +- src/pipeline/physics_pipeline.rs | 6 ++---- src/pipeline/query_pipeline.rs | 6 +----- 3 files changed, 4 insertions(+), 10 deletions(-) (limited to 'src/pipeline') 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))] -- cgit From e87b73a2a20fee1ed333d564ba46dbf1c3ca75e2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 15:49:22 +0200 Subject: Fix compilation in 2D. --- src/pipeline/physics_pipeline.rs | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) (limited to 'src/pipeline') 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. -- cgit From fc72aa6845a21b3af5d862968eddd3116036ac42 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 16:32:24 +0200 Subject: Fix examples. --- src/pipeline/physics_pipeline.rs | 42 +++++++++++++++------------------------- 1 file changed, 16 insertions(+), 26 deletions(-) (limited to 'src/pipeline') 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); -- cgit