path: root/src/geometry/broad_phase_multi_sap/broad_phase.rs

use super::{
    BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
};
use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
use crate::geometry::{
    ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape,
};
use crate::math::Real;
use crate::utils::IndexMut2;
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::HashMap;

use crate::data::{BundleSet, ComponentSet, ComponentSetMut};

/// A broad-phase combining a Hierarchical Grid and Sweep-and-Prune.
///
/// The basic Sweep-and-Prune (SAP) algorithm has one significant flaws:
/// the interactions between far-away objects. This means that objects
/// that are very far away will still have some of their endpoints swapped
/// within the SAP data-structure. This results in poor scaling because this
/// results in lots of swapping between endpoints of AABBs that won't ever
/// actually interact.
///
/// The first optimization to address this problem is to use the Multi-SAP
/// method. This basically combines an SAP with a grid. The grid subdivides
/// the spaces into equally-sized subspaces (grid cells). Each subspace, which we call
/// a "region" contains an SAP instance (i.e. there SAP axes responsible for
/// collecting endpoints and swapping them when they move to detect interaction pairs).
/// Each AABB is inserted in all the regions it intersects.
/// This prevents the far-away problem because two objects that are far away will
/// be located on different regions. So their endpoints will never meed.
///
/// However, the Multi-SAP approach has one notable problem: the region size must
/// be chosen wisely. It could be user-defined, but that's makes it more difficult
/// to use (for the end-user). Or it can be given a fixed value. Using a fixed
/// value may result in large objects intersecting lots of regions, resulting in
/// poor performances and very high memory usage.
///
/// So a solution to that large-objects problem is the Multi-SAP approach is to
/// replace the grid by a hierarchical grid. A hierarchical grid is composed of
/// several layers. And each layer have different region sizes. For example all
/// the regions on layer 0 will have the size 1x1x1. All the regions on the layer
/// 1 will have the size 10x10x10, etc. That way, a given AABB will be inserted
/// on the layer that has regions big enough to avoid the large-object problem.
/// For example a 20x20x20 object will be inserted in the layer with region
/// of size 10x10x10, resulting in only 8 regions being intersect by the AABB.
/// (If it was inserted in the layer with regions of size 1x1x1, it would have intersected
/// 8000 regions, which is a problem performancewise.)
///
/// We call this new method the Hierarchical-SAP.
///
/// Now with the Hierarchical-SAP, we can update each layer independently from one another.
/// However, objects belonging to different layers will never be detected as intersecting that
/// way. So we need a way to do inter-layer interference detection. There is a lot ways of doing
/// this: performing inter-layer Multi-Box-Pruning passes is one example (but this is not what we do).
/// In our implementation, we do the following:
/// - The AABB bounds of each region of the layer `n` are inserted into the corresponding larger region
///   of the layer `n + 1`.
/// - When an AABB in the region of the layer `n + 1` intersects the AABB corresponding to one of the
///   regions at the smaller layer `n`, we add that AABB to that smaller region.
/// So in the end it means that a given AABB will be inserted into all the region it intersects at
/// the layer `n`. And it will also be inserted into all the regions it intersects at the smaller layers
/// (the layers `< n`), but only for the regions that already exist (so we don't have to discretize
/// our AABB into the layers `< n`). This involves a fair amount of bookkeeping unfortunately, but
/// this has the benefit of keep the overall complexity of the algorithm O(1) in the typical specially
/// coherent scenario.
///
/// From an implementation point-of-view, our hierarchical SAP is implemented with the following structures:
/// - There is one `SAPLayer` per layer of the hierarchical grid.
/// - Each `SAPLayer` contains multiple `SAPRegion` (each being a region of the grid represented by that layer).
/// - Each `SAPRegion` contains three `SAPAxis`, representing the "classical" SAP algorithm running on this region.
/// - Each `SAPAxis` maintains a sorted list of `SAPEndpoints` representing the endpoints of the AABBs intersecting
///   the bounds on the `SAPRegion` containing this `SAPAxis`.
/// - A set of `SAPProxy` are maintained separately. It contains the AABBs of all the colliders managed by this
///   broad-phase, as well as the AABBs of all the regions part of this broad-phase.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhase {
    proxies: SAPProxies,
    layers: Vec<SAPLayer>,
    smallest_layer: u8,
    largest_layer: u8,
    deleted_any: bool,
    // NOTE: we maintain this hashmap to simplify collider removal.
    //       This information is also present in the ColliderProxyId
    //       component. However if that component is removed, we need
    //       a way to access it to do some cleanup.
    //       Note that we could just remove the ColliderProxyId component
    //       altogether but that would be slow because of the need to
    //       always access this hashmap. Instead, we access this hashmap
    //       only when the collider has been added/removed.
    //       Another alternative would be to remove ColliderProxyId and
    //       just use a Coarena. But this seems like it could use too
    //       much memory.
    colliders_proxy_ids: HashMap<ColliderHandle, SAPProxyIndex>,
    #[cfg_attr(feature = "serde-serialize", serde(skip))]
    region_pool: SAPRegionPool, // To avoid repeated allocations.
    // We could think serializing this workspace is useless.
    // It turns out is is important to serialize at least its capacity
    // and restore this capacity when deserializing the hashmap.
    // This is because the order of future elements inserted into the
    // hashmap depends on its capacity (because the internal bucket indices
    // depend on this capacity). So not restoring this capacity may alter
    // the order at which future elements are reported. This will in turn
    // alter the order at which the pairs are registered in the narrow-phase,
    // thus altering the order of the contact manifold. In the end, this
    // alters the order of the resolution of contacts, resulting in
    // diverging simulation after restoration of a snapshot.
    #[cfg_attr(
        feature = "serde-serialize",
        serde(
            serialize_with = "parry::utils::hashmap::serialize_hashmap_capacity",
            deserialize_with = "parry::utils::hashmap::deserialize_hashmap_capacity"
        )
    )]
    reporting: HashMap<(u32, u32), bool>, // Workspace
}

impl BroadPhase {
    /// Create a new empty broad-phase.
    pub fn new() -> Self {
        BroadPhase {
            proxies: SAPProxies::new(),
            layers: Vec::new(),
            smallest_layer: 0,
            largest_layer: 0,
            region_pool: Vec::new(),
            reporting: HashMap::default(),
            colliders_proxy_ids: HashMap::default(),
            deleted_any: false,
        }
    }

    /// Maintain the broad-phase internal state by taking collider removal into account.
    ///
    /// For each colliders marked as removed, we make their containing layer mark
    /// its proxy as pre-deleted. The actual proxy removal will happen at the end
    /// of the `BroadPhase::update`.
    fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
        // For each removed collider, remove the corresponding proxy.
        for removed in removed_colliders {
            if let Some(proxy_id) = self.colliders_proxy_ids.get(removed).copied() {
                self.predelete_proxy(proxy_id);
            }
        }
    }

    /// Pre-deletes a proxy from this broad-phase.
    ///
    /// The removal of a proxy is a semi-lazy process. It will mark
    /// the proxy as predeleted, and will set its AABB as +infinity.
    /// After this method has been called with all the proxies to
    /// remove, the `complete_removal` method MUST be called to
    /// complete the removal of these proxies, by actually removing them
    /// from all the relevant layers/regions/axes.
    fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) {
        if proxy_index == crate::INVALID_U32 {
            // This collider has not been added to the broad-phase yet.
            return;
        }

        let proxy = &mut self.proxies[proxy_index];
        let layer = &mut self.layers[proxy.layer_id as usize];
        // Let the layer know that the proxy is being deleted.
        layer.predelete_proxy(&mut self.proxies, proxy_index);
    }

    /// Completes the removal of the deleted proxies.
    ///
    /// If `self.predelete_proxy` was called, then this `complete_removals`
    /// method must be called to complete the removals.
    ///
    /// This method will actually remove from the proxy list all the proxies
    /// marked as deletable by `self.predelete_proxy`, making their proxy
    /// handles re-usable by new proxies.
    fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) {
        // If there is no layer, there is nothing to remove.
        if self.layers.is_empty() {
            return;
        }

        // This is a bottom-up pass:
        // - Complete the removal on the layer `n`. This may cause so regions to be deleted.
        // - Continue with the layer `n + 1`. This will delete from `n + 1` all the proxies
        //   of the regions originating from `n`.
        // This bottom-up approach will propagate region removal from the smallest layer up
        // to the largest layer.
        let mut curr_layer_id = self.smallest_layer;

        loop {
            let curr_layer = &mut self.layers[curr_layer_id as usize];

            if let Some(larger_layer_id) = curr_layer.larger_layer {
                let (curr_layer, larger_layer) = self
                    .layers
                    .index_mut2(curr_layer_id as usize, larger_layer_id as usize);
                curr_layer.complete_removals(
                    Some(larger_layer),
                    &mut self.proxies,
                    &mut self.region_pool,
                );

                // NOTE: we don't care about reporting pairs.
                self.reporting.clear();
                curr_layer_id = larger_layer_id;
            } else {
                curr_layer.complete_removals(None, &mut self.proxies, &mut self.region_pool);

                // NOTE: we don't care about reporting pairs.
                self.reporting.clear();
                break;
            }
        }

        /*
         * Actually remove the colliders proxies.
         */
        for removed in removed_colliders {
            if let Some(proxy_id) = self.colliders_proxy_ids.remove(removed) {
                if proxy_id != crate::INVALID_U32 {
                    self.proxies.remove(proxy_id);
                }
            }
        }
    }

    /// Finalize the insertion of the layer identified by `layer_id`.
    ///
    /// This will:
    /// - Remove all the subregion proxies from the larger layer.
    /// - Pre-insert all the smaller layer's region proxies into this layer.
    fn finalize_layer_insertion(&mut self, layer_id: u8) {
        // Remove all the region endpoints from the larger layer.
        // They will be automatically replaced by the new layer's regions.
        if let Some(larger_layer) = self.layers[layer_id as usize].larger_layer {
            self.layers[larger_layer as usize].unregister_all_subregions(&mut self.proxies);
        }

        // Add all the regions from the smaller layer to the new layer.
        // This will result in new regions to be created in the new layer.
        // These new regions will automatically propagate to the larger layers in
        // the Phase 3 of `Self::update`.
        if let Some(smaller_layer) = self.layers[layer_id as usize].smaller_layer <