path: root/src/geometry/broad_phase_multi_sap.rs

use crate::data::pubsub::Subscription;
use crate::dynamics::{IslandSet, RigidBodySet};
use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
use crate::math::{Point, Real, Vector, DIM};
use bit_vec::BitVec;
use parry::bounding_volume::{BoundingVolume, AABB};
use parry::utils::hashmap::HashMap;
use std::cmp::Ordering;
use std::ops::{Index, IndexMut};

const NUM_SENTINELS: usize = 1;
const NEXT_FREE_SENTINEL: u32 = u32::MAX;
const SENTINEL_VALUE: Real = Real::MAX;
const CELL_WIDTH: Real = 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),
}

fn sort2(a: u32, b: u32) -> (u32, u32) {
    assert_ne!(a, b);

    if a < b {
        (a, b)
    } else {
        (b, a)
    }
}

fn point_key(point: Point<Real>) -> Point<i32> {
    (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into()
}

fn region_aabb(index: Point<i32>) -> AABB {
    let mins = index.coords.map(|i| i as Real * CELL_WIDTH).into();
    let maxs = mins + Vector::repeat(CELL_WIDTH);
    AABB::new(mins, maxs)
}

#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct Endpoint {
    value: Real,
    packed_flag_proxy: u32,
}

const START_FLAG_MASK: u32 = 0b1 << 31;
const PROXY_MASK: u32 = u32::MAX ^ START_FLAG_MASK;
const START_SENTINEL_TAG: u32 = u32::MAX;
const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK;

impl Endpoint {
    pub fn start_endpoint(value: Real, proxy: u32) -> Self {
        Self {
            value,
            packed_flag_proxy: proxy | START_FLAG_MASK,
        }
    }

    pub fn end_endpoint(value: Real, proxy: u32) -> Self {
        Self {
            value,
            packed_flag_proxy: proxy & PROXY_MASK,
        }
    }

    pub fn start_sentinel() -> Self {
        Self {
            value: -SENTINEL_VALUE,
            packed_flag_proxy: START_SENTINEL_TAG,
        }
    }

    pub fn end_sentinel() -> Self {
        Self {
            value: SENTINEL_VALUE,
            packed_flag_proxy: END_SENTINEL_TAG,
        }
    }

    pub fn is_sentinel(self) -> bool {
        self.packed_flag_proxy & PROXY_MASK == PROXY_MASK
    }

    pub fn proxy(self) -> u32 {
        self.packed_flag_proxy & PROXY_MASK
    }

    pub fn is_start(self) -> bool {
        (self.packed_flag_proxy & START_FLAG_MASK) != 0
    }

    pub fn is_end(self) -> bool {
        (self.packed_flag_proxy & START_FLAG_MASK) == 0
    }
}

#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
struct SAPAxis {
    min_bound: Real,
    max_bound: Real,
    endpoints: Vec<Endpoint>,
    #[cfg_attr(feature = "serde-serialize", serde(skip))]
    new_endpoints: Vec<(Endpoint, usize)>, // Workspace
}

impl SAPAxis {
    fn new(min_bound: Real, max_bound: Real) -> Self {
        assert!(min_bound <= max_bound);

        Self {
            min_bound,
            max_bound,
            endpoints: vec![Endpoint::start_sentinel(), Endpoint::end_sentinel()],
            new_endpoints: Vec::new(),
        }
    }

    fn batch_insert(
        &mut self,
        dim: usize,
        new_proxies: &[usize],
        proxies: &Proxies,
        reporting: Option<&mut