path: root/src/dynamics/island_set.rs

use crate::data::Coarena;
use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderSet, NarrowPhase};
use crate::utils;
use std::collections::VecDeque;

#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct Island {
    bodies: Vec<RigidBodyHandle>,
}

impl Island {
    pub fn new() -> Self {
        Self { bodies: vec![] }
    }

    pub fn len(&self) -> usize {
        self.bodies.len()
    }

    pub fn bodies(&self) -> &[RigidBodyHandle] {
        &self.bodies
    }
}

#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct IslandSet {
    // We can't store this in the rigid-bodies because that would
    // cause borrowing issues during the traversal.
    traversal_body_timestamps: Coarena<u64>,
    islands: Vec<Island>,
    active_island: usize,
    islands_to_extract: VecDeque<RigidBodyHandle>,
    islands_extraction_queue: Vec<RigidBodyHandle>,
    // NOTE: this value must never be 0, otherwise some
    // invariants used during the traversal can break.
    traversal_timestamp: u64,
}

impl IslandSet {
    pub fn new() -> Self {
        IslandSet {
            traversal_body_timestamps: Coarena::new(),
            islands: vec![Island::new()],
            active_island: 0,
            islands_to_extract: VecDeque::new(),
            islands_extraction_queue: vec![],
            traversal_timestamp: 1,
        }
    }

    pub fn num_active_islands(&self) -> usize {
        1
    }

    pub fn is_island_sleeping(&self, island_id: usize) -> bool {
        island_id != self.active_island
    }

    pub fn active_bodies(&self) -> &[RigidBodyHandle] {
        &self.islands[self.active_island].bodies
    }

    pub fn incremental_update(&mut self, narrow_phase: &NarrowPhase) {}

    pub fn add_rigid_body(&mut self, handle: RigidBodyHandle, body: &mut RigidBody) {
        assert_eq!(body.island_id, crate::INVALID_USIZE);

        if !body.is_dynamic() {
            return;
        }

        self.traversal_body_timestamps
            .ensure_element_exists(handle.0, 0);

        if body.can_sleep() {
            body.island_id = self.islands.len();
            body.island_offset = 0;
            self.islands.push(Island {
                bodies: vec![handle],
            })
        } else {
            let mut active_island = &mut self.islands[self.active_island];
            body.island_id = self.active_island;
            body.island_offset = active_island.bodies.len();
            active_island.bodies.push(handle);
        }
    }

    pub fn contact_started(
        &mut self,
        bodies: &mut RigidBodySet,
        mut h1: RigidBodyHandle,
        mut h2: RigidBodyHandle,
    ) {
        let island1 = bodies[h1].island_id;
        let island2 = bodies[h2].island_id;

        self.wake_up_island(bodies, island1);
        self.wake_up_island(bodies, island2);
    }

    pub fn body_sleep_state_changed(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle) {
        let body = &mut bodies[handle];
        let island_id = body.island_id;

        if island_id == crate::INVALID_USIZE {
            self.add_rigid_body(handle, body);
        } else if island_id == self.active_island && body.can_sleep() {
            // The body is sleeping now.
            self.islands_to_extract.push_back(handle);
        } else if island_id != self.active_island && !body.can_sleep() {
            // The body is awake now.
            self.wake_up_island(bodies, island_id);
        }
    }

    #[inline(always)]
    fn push_contacting_bodies(
        handle: RigidBodyHandle,
        bodies: &mut RigidBodySet,
        colliders: &ColliderSet,
        narrow_phase: &NarrowPhase,
        queue: &mut Vec<RigidBodyHandle>,
        traversal_body_timestamps: &mut Coarena<u64>,
        first_traversal_timestamp: u64,
        traversal_timestamp: u64,
    ) -> bool {
        for collider_handle in &bodies[handle].colliders {
            if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
                for inter in contacts {
                    let other = crate::utils::select_other((inter.0, inter.1), *collider_handle);
                    let other_body_handle = colliders[other].parent;
                    let other_body = &bodies[other_body_handle];

                    if other_body.is_dynamic() {
                        if !other_body.can_sleep() {
                            return false;
                        }

                        let other_body_timestamp =
                            &mut traversal_body_timestamps[other_body_handle.0];

                        if *other_body_timestamp >= first_traversal_timestamp
                            && *other_body_timestamp < traversal_timestamp
                        {
                            // We already saw this body during another traversal this frame.
                            // So we don't need to continue any further.
                            return false;
                        }

                        if *other_body_timestamp != traversal_timestamp {
                            *other_body_timestamp = traversal_timestamp;
                            queue.push(other_body_handle);
                        }
                    }
                }
            }
        }

        true
    }

    pub fn update_sleeping_islands(
        &mut self,
        bodies: &mut RigidBodySet,
        colliders: &mut ColliderSet,
        narrow_phase: &NarrowPhase,
    ) {
        let first_traversal_timestamp = self.traversal_timestamp + 1;
        let mut island_found = false;

        while let Some(handle) = self.islands_to_extract.pop_front() {
            self.traversal_timestamp += 1;
            island_found = self.extract_sleeping_island(
                bodies,
                colliders,
                narrow_phase,
                handle,
                first_traversal_timestamp,
            ) || island_found;

            // Make sure our `traversal_timestamp` remains correct
            // even if it overflows (can happen in very long-running
            // simulations).
            if self.traversal_timestamp == u64::MAX - 1 {
                // Stop the work now because the overflowing timestamp
                // will cause issues with the traversal. Forcibly reset all
                // the timestamps and continue next frame.
                for body in bodies.iter_mut_internal() {
                    body.1.traversal_timestamp = 0;
                }
                self.traversal_timestamp = 1;
                break;
            }
        }

        if island_found {
            // Now we need to remove from the active set all the bodies
            // we moved to a sleeping island.
            let mut i = 0;
            let mut new_len = 0;
            let active_island = &mut self.islands[self.active_island];

            while i < active_island.bodies.len() {
                let handle = active_island.bodies[i];
                let body = &mut bodies[handle];

                if body.island_id == self.active_island {
                    // This body still belongs to this island.
                    // Update its offset.
                    body.island_offset = new_len;
                    active_island.bodies[new_len] = handle;
                    new_len += 1;
                }

                i += 1;
            }

            active_island.bodies.truncate(new_len);
        }
    }

    fn extract_sleeping_island(
        &mut self,
        bodies: &mut RigidBodySet,
        colliders: &mut ColliderSet,
        narrow_phase: &NarrowPhase,
        handle: RigidBodyHandle,
        first_traversal_timestamp: u64,
    ) -> bool {
        // Attempt to extract a sleeping island by traversing the
        // contact graph starting with the given rigid-body.
        if let Some(body) = bodies.get(handle) {
            if body.is_dynamic() && body.can_sleep() {
                // Perform a breadth-first search of the contact graph starting
                // with this body. We stop the search when an active body is
                // found (meaning that the island cannot sleep), or if we
                // extracted the whole island.
                // We do a breadth-first search in order to increase our chances
                // to find a non-sleeping body quickly: if this body just started
                // sleeping, chances are that a non-sleeping body is close.
                self.islands_extraction_queue.clear();
                self.islands_extraction_queue.push