diff options
| -rw-r--r-- | examples3d/debug_big_colliders3.rs | 23 | ||||
| -rw-r--r-- | examples3d/debug_infinite_fall3.rs | 8 | ||||
| -rw-r--r-- | examples3d/fountain3.rs | 14 | ||||
| -rw-r--r-- | examples3d/primitives3.rs | 48 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/broad_phase.rs | 23 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_axis.rs | 83 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_layer.rs | 47 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_proxy.rs | 28 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_region.rs | 60 | ||||
| -rw-r--r-- | src/geometry/broad_phase_multi_sap/sap_utils.rs | 12 |
10 files changed, 211 insertions, 135 deletions
diff --git a/examples3d/debug_big_colliders3.rs b/examples3d/debug_big_colliders3.rs index c2b62e2..956f36a 100644 --- a/examples3d/debug_big_colliders3.rs +++ b/examples3d/debug_big_colliders3.rs @@ -1,6 +1,6 @@ -use na::Point3; +use na::{Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; -use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, HalfSpace, SharedShape}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -14,30 +14,27 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let ground_size = 100.0; - let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static().build(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size) - .friction(1.5) - .build(); + let halfspace = SharedShape::new(HalfSpace::new(Vector3::y_axis())); + let collider = ColliderBuilder::new(halfspace).build(); colliders.insert(collider, handle, &mut bodies); let mut curr_y = 0.0; - let mut curr_width = 1_000.0; + let mut curr_width = 10_000.0; - for _ in 0..6 { - curr_y += curr_width; + for _ in 0..12 { + let curr_height = 0.1f32.min(curr_width); + curr_y += curr_height * 4.0; let rigid_body = RigidBodyBuilder::new_dynamic() .translation(0.0, curr_y, 0.0) .build(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(curr_width, curr_width, curr_width).build(); + let collider = ColliderBuilder::cuboid(curr_width, curr_height, curr_width).build(); colliders.insert(collider, handle, &mut bodies); - curr_width /= 10.0; + curr_width /= 5.0; } /* diff --git a/examples3d/debug_infinite_fall3.rs b/examples3d/debug_infinite_fall3.rs index 2a85e37..e70a386 100644 --- a/examples3d/debug_infinite_fall3.rs +++ b/examples3d/debug_infinite_fall3.rs @@ -1,3 +1,4 @@ +use na::Point3; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -17,7 +18,7 @@ pub fn init_world(testbed: &mut Testbed) { let ground_height = 2.1; let rigid_body = RigidBodyBuilder::new_static() - .translation(0.0, -ground_height, 0.0) + .translation(0.0, 4.0, 0.0) .build(); let handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); @@ -26,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) { let rad = 1.0; // Build the dynamic box rigid body. let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(0.0, 3.0 * rad, 0.0) + .translation(0.0, 7.0 * rad, 0.0) .can_sleep(false) .build(); let handle = bodies.insert(rigid_body); @@ -34,7 +35,7 @@ pub fn init_world(testbed: &mut Testbed) { colliders.insert(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(0.0, 5.0 * rad, 0.0) + .translation(0.0, 2.0 * rad, 0.0) .can_sleep(false) .build(); let handle = bodies.insert(rigid_body); @@ -44,6 +45,7 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ + testbed.look_at(Point3::new(100.0, -10.0, 100.0), Point3::origin()); testbed.set_world(bodies, colliders, joints); } diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index e39ff2a..6c80562 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) { * Ground */ let ground_size = 100.1; - let ground_height = 2.1; + let ground_height = 2.1; // 16.0; let rigid_body = RigidBodyBuilder::new_static() .translation(0.0, -ground_height, 0.0) @@ -64,10 +64,6 @@ pub fn init_world(testbed: &mut Testbed) { physics .bodies .remove(*handle, &mut physics.colliders, &mut physics.joints); - physics.broad_phase.maintain(&mut physics.colliders); - physics - .narrow_phase - .maintain(&mut physics.colliders, &mut physics.bodies); if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { graphics.remove_body_nodes(window, *handle); @@ -80,10 +76,10 @@ pub fn init_world(testbed: &mut Testbed) { * Set up the testbed. */ testbed.set_world(bodies, colliders, joints); - testbed - .physics_state_mut() - .integration_parameters - .velocity_based_erp = 0.2; + // testbed + // .physics_state_mut() + // .integration_parameters + // .velocity_based_erp = 0.2; testbed.look_at(Point3::new(-30.0, 4.0, -30.0), Point3::new(0.0, 1.0, 0.0)); } diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index a9ae176..db15341 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -12,6 +12,19 @@ pub fn init_world(testbed: &mut Testbed) { let joints = JointSet::new(); /* + * Ground + */ + let ground_size = 100.1; + let ground_height = 2.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* * Create the cubes */ let num = 8; @@ -26,11 +39,11 @@ pub fn init_world(testbed: &mut Testbed) { let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; - for j in 0usize..1 { - for i in 0..1 { - for k in 0usize..1 { + for j in 0usize..20 { + for i in 0..num { + for k in 0usize..num { let x = i as f32 * shiftx - centerx + offset; - let y = j as f32 * shifty + centery - rad; + let y = j as f32 * shifty + centery + 3.0; let z = k as f32 * shiftz - centerz + offset; // Build the rigid body. @@ -55,36 +68,9 @@ pub fn init_world(testbed: &mut Testbed) { } /* - * Ground - */ - testbed.add_callback( - move |mut window, mut graphics, physics, events, run_state| { - if run_state.timestep_id == 10 { - let ground_size = 100.1; - let ground_height = 2.1; - - let rigid_body = RigidBodyBuilder::new_static() - .translation(0.0, -ground_height, 0.0) - .build(); - let handle = physics.bodies.insert(rigid_body); - let collider = - ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); - physics - .colliders - .insert(collider, handle, &mut physics.bodies); - - if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) { - graphics.add(window, handle, &physics.bodies, &physics.colliders); - } - } - }, - ); - - /* * Set up the testbed. */ testbed.set_world(bodies, colliders, joints); - testbed.physics_state_mut().gravity.fill(0.0); testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); } diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index dd533af..18d394f 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -213,7 +213,9 @@ impl BroadPhase { */ let cursor = self.removed_colliders.as_ref().unwrap(); for collider in colliders.removed_colliders.read(&cursor) { - self.proxies.remove(collider.proxy_index); + if collider.proxy_index != crate::INVALID_U32 { + self.proxies.remove(collider.proxy_index); + } } colliders.removed_colliders.ack(&cursor); } @@ -222,7 +224,7 @@ impl BroadPhase { if let Some(larger_layer) = self.layers[layer_id as usize].larger_layer { // Remove all the region endpoints from the larger layer. // They will be automatically replaced by the new layer's regions. - self.layers[larger_layer as usize].delete_all_region_endpoints(&mut self.proxies); + self.layers[larger_layer as usize].unregister_all_subregions(&mut self.proxies); } if let Some(smaller_layer) = self.layers[layer_id as usize].smaller_layer { @@ -305,6 +307,7 @@ impl BroadPhase { } } + /// Updates the broad-phase, taking into account the new collider positions. pub fn update( &mut self, prediction_distance: Real, @@ -324,7 +327,9 @@ impl BroadPhase { { for handle in &bodies[*body_handle].colliders { let collider = &mut colliders[*handle]; - let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); + let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); + aabb.mins = super::clamp_point(aabb.mins); + aabb.maxs = super::clamp_point(aabb.maxs); let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { proxy.aabb = aabb; @@ -334,7 +339,7 @@ impl BroadPhase { let layer_id = self.ensure_layer_exists(layer_depth); // Create the proxy. - let proxy = SAPProxy::collider(*handle, aabb, layer_id); + let proxy = SAPProxy::collider(*handle, aabb, layer_id, layer_depth); collider.proxy_index = self.proxies.insert(proxy); layer_id }; @@ -443,13 +448,19 @@ impl BroadPhase { (SAPProxyData::Collider(_), SAPProxyData::Region(_)) => { if *colliding { // Add the collider to the subregion. - proxy2.data.as_region_mut().preupdate_proxy(*proxy_id1); + proxy2 + .data + .as_region_mut() + .preupdate_proxy(*proxy_id1, false); } } (SAPProxyData::Region(_), SAPProxyData::Collider(_)) => { if *colliding { // Add the collider to the subregion. - proxy1.data.as_region_mut().preupdate_proxy(*proxy_id2); + proxy1 + .data + .as_region_mut() + .preupdate_proxy(*proxy_id2, false); } } (SAPProxyData::Region(_), SAPProxyData::Region(_)) => { diff --git a/src/geometry/broad_phase_multi_sap/sap_axis.rs b/src/geometry/broad_phase_multi_sap/sap_axis.rs index 2e2e613..a6b62ae 100644 --- a/src/geometry/broad_phase_multi_sap/sap_axis.rs +++ b/src/geometry/broad_phase_multi_sap/sap_axis.rs @@ -29,6 +29,13 @@ impl SAPAxis { } } + pub fn clear(&mut self) { + self.new_endpoints.clear(); + self.endpoints.clear(); + self.endpoints.push(SAPEndpoint::start_sentinel()); + self.endpoints.push(SAPEndpoint::end_sentinel()); + } + pub fn batch_insert( &mut self, dim: usize, @@ -115,14 +122,29 @@ impl SAPAxis { } } - pub fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> usize { - let mut deleted = 0; + /// Removes from this axis all the endpoints that are out of bounds from this axis. + /// + /// Returns the number of deleted proxies as well as the number of proxies deleted + /// such that `proxy.layer_depth <= layer_depth`. + pub fn delete_out_of_bounds_proxies( + &self, + proxies: &SAPProxies, + existing_proxies: &mut BitVec, + layer_depth: i8, + ) -> (usize, usize) { + let mut num_subproper_proxies_deleted = 0; + let mut num_proxies_deleted = 0; for endpoint in &self.endpoints { if endpoint.value < self.min_bound { - let proxy_idx = endpoint.proxy() as usize; - if endpoint.is_end() && existing_proxies[proxy_idx] { - existing_proxies.set(proxy_idx, false); - deleted += 1; + let proxy_id = endpoint.proxy(); + if endpoint.is_end() && existing_proxies[proxy_id as usize] { + existing_proxies.set(proxy_id as usize, false); + + if proxies[proxy_id].layer_depth <= layer_depth { + num_subproper_proxies_deleted += 1; + } + + num_proxies_deleted += 1; } } else { break; @@ -131,17 +153,22 @@ impl SAPAxis { for endpoint in self.endpoints.iter().rev() { if endpoint.value > self.max_bound { - let proxy_idx = endpoint.proxy() as usize; - if endpoint.is_start() && existing_proxies[proxy_idx] { - existing_proxies.set(endpoint.proxy() as usize, false); - deleted += 1; + let proxy_id = endpoint.proxy(); + if endpoint.is_start() && existing_proxies[proxy_id as usize] { + existing_proxies.set(proxy_id as usize, false); + + if proxies[proxy_id].layer_depth <= layer_depth { + num_subproper_proxies_deleted += 1; + } + + num_proxies_deleted += 1; } } else { break; } } - deleted + (num_proxies_deleted, num_subproper_proxies_deleted) } pub fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) { @@ -149,26 +176,39 @@ impl SAPAxis { .retain(|endpt| endpt.is_sentinel() || existing_proxies[endpt.proxy() as usize]) } + /// Removes from this axis all the endpoints corresponding to a proxy with an AABB mins/maxs values + /// equal to DELETED_AABB_VALUE, indicating that the endpoints should be deleted. + /// + /// Returns the number of deleted proxies such that `proxy.layer_depth <= layer_depth`. pub fn delete_deleted_proxies_and_endpoints_after_subregion_removal( &mut self, proxies: &SAPProxies, existing_proxies: &mut BitVec, + layer_depth: i8, ) -> usize { - let mut num_deleted = 0; + let mut num_subproper_proxies_deleted = 0; self.endpoints.retain(|endpt| { - if !endpt.is_sentinel() && proxies[endpt.proxy()].aabb.mins.x == DELETED_AABB_VALUE { - if existing_proxies.get(endpt.proxy() as usize) == Some(true) { - existing_proxies.set(endpt.proxy() as usize, false); - num_deleted += 1; + if !endpt.is_sentinel() { + let proxy = &proxies[endpt.proxy()]; + + if proxy.aabb.mins.x == DELETED_AABB_VALUE { + if existing_proxies.get(endpt.proxy() as usize) == Some(true) { + existing_proxies.set(endpt.proxy() as usize, false); + + if proxy.layer_depth <= layer_depth { + num_subproper_proxies_deleted += 1; + } + } + + return false; } - false - } else { - true } + + true }); - num_deleted + num_subproper_proxies_deleted } pub fn update_endpoints( @@ -197,7 +237,8 @@ impl SAPAxis { if endpoint_j.is_end() { // Report start collision. - if aabb_i.intersects(&proxies[endpoint_j.proxy()].aabb) { + let proxy_j = &proxies[endpoint_j.proxy()]; + if aabb_i.intersects(&proxy_j.aabb) { let pair = super::sort2(endpoint_i.proxy(), endpoint_j.proxy()); reporting.insert(pair, true); } diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs index 2a65ffc..8242cca 100644 --- a/src/geometry/broad_phase_multi_sap/sap_layer.rs +++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs @@ -38,10 +38,18 @@ impl SAPLayer { } } - pub fn delete_all_region_endpoints(&mut self, proxies: &mut SAPProxies) { + pub fn unregister_all_subregions(&mut self, proxies: &mut SAPProxies) { for region_id in self.regions.values() { if let Some(mut region) = proxies[*region_id].data.take_region() { region.delete_all_region_endpoints(proxies); + + for subregion in region.subregions.drain(..) { + proxies[subregion] + .data + .as_region_mut() + .id_in_parent_subregion = crate::INVALID_U32; + } + proxies[*region_id].data.set_region(region); } } @@ -77,14 +85,17 @@ impl SAPLayer { pool: &mut SAPRegionPool, ) { if let Some(proxy) = proxies.get(proxy_id) { - let region_key = super::point_key(proxy.aabb.center(), self.region_width); - let region_id = self.ensure_region_exists(region_key, proxies, pool); - let region = proxies[region_id].data.as_region_mut(); - let id_in_parent_subregion = region.register_subregion(proxy_id); - proxies[proxy_id] - .data - .as_region_mut() - .id_in_parent_subregion = id_in_parent_subregion as u32; + if proxy.data.as_region().id_in_parent_subregion == crate::INVALID_U32 { + let region_key = super::point_key(proxy.aabb.center(), self.region_width); + let region_id = self.ensure_region_exists(region_key, proxies, pool); + let region = proxies[region_id].data.as_region_mut(); + + let id_in_parent_subregion = region.register_subregion(proxy_id); + proxies[proxy_id] + .data + .as_region_mut() + .id_in_parent_subregion = id_in_parent_subregion as u32; + } } } @@ -106,9 +117,10 @@ impl SAPLayer { region.needs_update_after_subregion_removal = true; } - region + let removed = region .subregions .swap_remove(id_in_parent_subregion as usize); // Remove the subregion index from the subregion list. + assert_eq!(removed, proxy_id); // Re-adjust the id_in_parent_subregion of the subregion that was swapped in place // of the deleted one. @@ -137,7 +149,8 @@ impl SAPLayer { Entry::Vacant(vacant) => { let region_bounds = super::region_aabb(region_key, self.region_width); let region = SAPRegion::recycle_or_new(region_bounds, pool); - let region_proxy = SAPProxy::subregion(region, region_bounds, self.layer_id); + let region_proxy = + SAPProxy::subregion(region, region_bounds, self.layer_id, self.depth); let region_proxy_id = proxies.insert(region_proxy); self.created_regions.push(region_proxy_id as u32); let _ = vacant.insert(region_proxy_id); @@ -172,7 +185,7 @@ impl SAPLayer { let region_key = Point::new(i, j, _k); let region_id = self.ensure_region_exists(region_key, proxies, pool); let region = proxies[region_id].data.as_region_mut(); - region.preupdate_proxy(proxy_id); + region.preupdate_proxy(proxy_id, true); } } } @@ -222,14 +235,14 @@ impl SAPLayer { for (point, region_id) in &self.regions { if let Some(mut region) = proxies[*region_id].data.take_region() { // Update the region. - region.update(proxies, reporting); + region.update(proxies, self.depth, reporting); // Mark all subregions as to-be-updated. for subregion_id in ®ion.subregions { proxies[*subregion_id].data.as_region_mut().mark_as_dirty(); } - if region.proxy_count == 0 { + if region.subproper_proxy_count == 0 { self.regions_to_potentially_remove.push(*point); } @@ -257,10 +270,10 @@ impl SAPLayer { if let Some(proxy) = proxies.get_mut(*region_id.get()) { // First, we need to remove the endpoints of the deleted subregions. let mut region = proxy.data.take_region().unwrap(); - region.update_after_subregion_removal(proxies); + region.update_after_subregion_removal(proxies, self.depth); - // Check if we actually can delete this region. - if region.proxy_count == 0 { + // Check if we can actually delete this region. + if region.subproper_proxy_count == 0 { let region_id = region_id.remove(); // We can delete this region. So we need to tell the larger diff --git a/src/geometry/broad_phase_multi_sap/sap_proxy.rs b/src/geometry/broad_phase_multi_sap/sap_proxy.rs index 8c01f58..ed834bd 100644 --- a/src/geometry/broad_phase_multi_sap/sap_proxy.rs +++ b/src/geometry/broad_phase_multi_sap/sap_proxy.rs @@ -14,19 +14,19 @@ pub enum SAPProxyData { } impl SAPProxyData { - pub fn is_subregion(&self) -> bool { + pub fn is_region(&self) -> bool { match self { SAPProxyData::Region(_) => true, _ => false, } } - // pub fn as_region(&self) -> &SAPRegion { - // match self { - // SAPProxyData::Region(r) => r.as_ref().unwrap(), - // _ => panic!("Invalid proxy type."), - // } - // } + pub fn as_region(&self) -> &SAPRegion { + match self { + SAPProxyData::Region(r) => r.as_ref().unwrap(), + _ => panic!("Invalid proxy type."), + } + } pub fn as_region_mut(&mut self) -> &mut SAPRegion { match self { @@ -53,25 +53,29 @@ pub struct SAPProxy { pub data: SAPProxyData, pub aabb: AABB, pub next_free: SAPProxyIndex, + // TODO: pack the layer_id and layer_depth into a single u16? pub layer_id: u8, + pub layer_depth: i8, } impl SAPProxy { - pub fn collider(handle: ColliderHandle, aabb: AABB, layer_id: u8) -> Self { + pub fn collider(handle: ColliderHandle, aabb: AABB, layer_id: u8, layer_depth: i8) -> Self { Self { data: SAPProxyData::Collider(handle), aabb, next_free: NEXT_FREE_SENTINEL, layer_id, + layer_depth, } } - pub fn subregion(subregion: Box<SAPRegion>, aabb: AABB, layer_id: u8) -> Self { + pub fn subregion(subregion: Box<SAPRegion>, aabb: AABB, layer_id: u8, layer_depth: i8) -> Self { Self { data: SAPProxyData::Region(Some(subregion)), aabb, next_free: NEXT_FREE_SENTINEL, layer_id, + layer_depth, } } } @@ -92,7 +96,7 @@ impl SAPProxies { } pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex { - if self.first_free != NEXT_FREE_SENTINEL { + let result = if self.first_free != NEXT_FREE_SENTINEL { let proxy_id = self.first_free; self.first_free = self.elements[proxy_id as usize].next_free; self.elements[proxy_id as usize] = proxy; @@ -100,7 +104,9 @@ impl SAPProxies { } else { self.elements.push(proxy); self.elements.len() as u32 - 1 - } + }; + + result } pub fn remove(&mut self, proxy_id: SAPProxyIndex) { diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs index c442664..3d0834c 100644 --- a/src/geometry/broad_phase_multi_sap/sap_region.rs +++ b/src/geometry/broad_phase_multi_sap/sap_region.rs @@ -18,7 +18,10 @@ pub struct SAPRegion { pub id_in_parent_subregion: u32, pub update_count: u8, pub needs_update_after_subregion_removal: bool, - pub proxy_count: usize, + // Number of proxies (added to this region) that originates + // from the layer at depth <= the depth of the layer containing + // this region. + pub subproper_proxy_count: usize, } impl SAPRegion { @@ -37,26 +40,27 @@ impl SAPRegion { id_in_parent_subregion: crate::INVALID_U32, update_count: 0, needs_update_after_subregion_removal: false, - proxy_count: 0, + subproper_proxy_count: 0, } } pub fn recycle(bounds: AABB, mut old: Box<Self>) -> Box<Self> { // Correct the bounds - for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) { - axis.min_bound = bound; - } - for (axis, &bound) in old.axes.iter_mut().zip(bounds.maxs.iter()) { - axis.max_bound = bound; + for i in 0..DIM { + // Make sure the axis is empty (it may still contain + // some old endpoints from non-proper proxies. + old.axes[i].clear(); + old.axes[i].min_bound = bounds.mins[i]; + old.axes[i].max_bound = bounds.maxs[i]; } old.update_count = 0; + old.existing_proxies.clear(); + old.id_in_parent_subregion = crate::INVALID_U32; // The rest of the fields should be "empty" - assert_eq!(old.proxy_count, 0); + assert_eq!(old.subproper_proxy_count, 0); assert!(old.to_insert.is_empty()); - debug_assert!(!old.existing_proxies.any()); - assert!(old.axes.iter().all(|ax| ax.endpoints.len() == 2)); old } @@ -75,7 +79,7 @@ impl SAPRegion { axis.endpoints.retain(|e| { if let Some(proxy) = proxies.get(e.proxy()) { existing_proxies.set(e.proxy() as usize, false); - !proxy.data.is_subregion() + !proxy.data.is_region() } else { true } @@ -97,11 +101,11 @@ impl SAPRegion { pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize { let subregion_index = self.subregions.len(); self.subregions.push(proxy_id); - self.preupdate_proxy(proxy_id); + self.preupdate_proxy(proxy_id, true); subregion_index } - pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex) -> bool { + pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool { let mask_len = self.existing_proxies.len(); if proxy_id as usize >= mask_len { self.existing_proxies @@ -111,7 +115,11 @@ impl SAPRegion { if !self.existing_proxies[proxy_id as usize] { self.to_insert.push(proxy_id); self.existing_proxies.set(proxy_id as usize, true); - self.proxy_count += 1; + + if is_subproper_proxy { + self.subproper_proxy_count += 1; + } + false } else { // Here we need a second update if all proxies exit this region. In this case, we need @@ -122,31 +130,41 @@ impl SAPRegion { } } - pub fn update_after_subregion_removal(&mut self, proxies: &SAPProxies) { + pub fn update_after_subregion_removal(&mut self, proxies: &SAPProxies, layer_depth: i8) { if self.needs_update_after_subregion_removal { for axis in &mut self.axes { - self.proxy_count -= axis + self.subproper_proxy_count -= axis .delete_deleted_proxies_and_endpoints_after_subregion_removal( proxies, &mut self.existing_proxies, + layer_depth, ); } self.needs_update_after_subregion_removal = false; } } - pub fn update(&mut self, proxies: &SAPProxies, reporting: &mut HashMap<(u32, u32), bool>) { + pub fn update( + &mut self, + proxies: &SAPProxies, + layer_depth: i8, + reporting: &mut HashMap<(u32, u32), bool>, + ) { if self.update_count > 0 { // Update endpoints. - let mut deleted = 0; + let mut total_deleted = 0; + let mut total_deleted_subproper = 0; for dim in 0..DIM { self.axes[dim].update_endpoints(dim, proxies, reporting); - deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies); + let (num_deleted, num_deleted_subproper) = self.axes[dim] + .delete_out_of_bounds_proxies(proxies, &mut self.existing_proxies, layer_depth); + total_deleted += num_deleted; + total_deleted_subproper += num_deleted_subproper; } - if deleted > 0 { - self.proxy_count -= deleted; + if total_deleted > 0 { + self.subproper_proxy_count -= total_deleted_subproper; for dim in 0..DIM { self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } diff --git a/src/geometry/broad_phase_multi_sap/sap_utils.rs b/src/geometry/broad_phase_multi_sap/sap_utils.rs index b471b4d..ac84926 100644 --- a/src/geometry/broad_phase_multi_sap/sap_utils.rs +++ b/src/geometry/broad_phase_multi_sap/sap_utils.rs @@ -5,6 +5,7 @@ pub(crate) const NUM_SENTINELS: usize = 1; pub(crate) const NEXT_FREE_SENTINEL: u32 = u32::MAX; pub(crate) const SENTINEL_VALUE: Real = Real::MAX; pub(crate) const DELETED_AABB_VALUE: Real = SENTINEL_VALUE / 2.0; +pub(crate) const MAX_AABB_EXTENT: Real = SENTINEL_VALUE / 4.0; pub(crate) const REGION_WIDTH_BASE: Real = 1.0; pub(crate) const REGION_WIDTH_POWER_BASIS: Real = 5.0; @@ -18,6 +19,10 @@ pub(crate) fn sort2(a: u32, b: u32) -> (u32, u32) { } } +pub(crate) fn clamp_point(point: Point<Real>) -> Point<Real> { + point.map(|e| na::clamp(e, -MAX_AABB_EXTENT, MAX_AABB_EXTENT)) +} + pub(crate) fn point_key(point: Point<Real>, region_width: Real) -> Point<i32> { (point / region_width) .coords @@ -32,7 +37,7 @@ pub(crate) fn region_aabb(index: Point<i32>, region_width: Real) -> AABB { } pub(crate) fn region_width(depth: i8) -> Real { - REGION_WIDTH_BASE * REGION_WIDTH_POWER_BASIS.powi(depth as i32) + (REGION_WIDTH_BASE * REGION_WIDTH_POWER_BASIS.powi(depth as i32)).min(MAX_AABB_EXTENT) } /// Computes the depth of the layer the given AABB should be part of. @@ -49,8 +54,9 @@ pub(crate) fn layer_containing_aabb(aabb: &AABB) -> i8 { const NUM_ELEMENTS_PER_DIMENSION: Real = 10.0; let width = 2.0 * aabb.half_extents().norm() * NUM_ELEMENTS_PER_DIMENSION; - // TODO: handle overflows in the f32 -> i8 conversion. (width / REGION_WIDTH_BASE) .log(REGION_WIDTH_POWER_BASIS) - .round() as i8 + .round() + .max(i8::MIN as Real) + .min(i8::MAX as Real) as i8 } |