#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
use crossbeam::channel::{Receiver, Sender};
use std::ops::{Deref, DerefMut, Index, IndexMut};
pub struct RigidBodyMut<'a> {
rb: &'a mut RigidBody,
was_sleeping: bool,
handle: RigidBodyHandle,
sender: &'a Sender<RigidBodyHandle>,
}
impl<'a> RigidBodyMut<'a> {
fn new(
handle: RigidBodyHandle,
rb: &'a mut RigidBody,
sender: &'a Sender<RigidBodyHandle>,
) -> Self {
Self {
was_sleeping: rb.is_sleeping(),
handle,
sender,
rb,
}
}
}
impl<'a> Deref for RigidBodyMut<'a> {
type Target = RigidBody;
fn deref(&self) -> &RigidBody {
&*self.rb
}
}
impl<'a> DerefMut for RigidBodyMut<'a> {
fn deref_mut(&mut self) -> &mut RigidBody {
self.rb
}
}
impl<'a> Drop for RigidBodyMut<'a> {
fn drop(&mut self) {
if self.was_sleeping && !self.rb.is_sleeping() {
self.sender.send(self.handle).unwrap();
}
}
}
pub type RigidBodyHandle = crate::data::arena::Index;
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct BodyPair {
pub body1: RigidBodyHandle,
pub body2: RigidBodyHandle,
}
impl BodyPair {
pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
BodyPair { body1, body2 }
}
pub(crate) fn swap(self) -> Self {
Self::new(self.body2, self.body1)
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct RigidBodySet {
pub(crate) bodies: Arena<RigidBody>,
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
active_set_timestamp: u32,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
stack: Vec<RigidBodyHandle>,
#[cfg_attr(
feature = "serde-serialize",
serde(skip, default = "crossbeam::channel::unbounded")
)]
activation_channel: (Sender<RigidBodyHandle>, Receiver<RigidBodyHandle>),
}
impl RigidBodySet {
pub fn new() -> Self {
RigidBodySet {
bodies: Arena::new(),
active_dynamic_set: Vec::new(),
active_kinematic_set: Vec::new(),
modified_inactive_set: Vec::new(),
active_islands: Vec::new(),
active_set_timestamp: 0,
can_sleep: Vec::new(),
stack: Vec::new(),
activation_channel: crossbeam::channel::unbounded(),
}
}
pub fn invalid_handle() -> RigidBodyHandle {
RigidBodyHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
}
pub fn len(&self) -> usize {
self.bodies.len()
}
pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
let mut rb = &mut self.bodies[handle];
match rb.body_status {
BodyStatus::Dynamic | BodyStatus::Static => {
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
BodyStatus::Kinematic => {
if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
}
}
}
pub fn contains(&self, handle: RigidBodyHandle) -> bool {
self.bodies.contains(handle)
}
pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
let handle = self.bodies.insert(rb);
let rb = &mut self.bodies[handle];
if !rb.is_sleeping() && rb.is_dynamic() {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
if rb.is_kinematic() {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
if !rb.is_dynamic() {
self.modified_inactive_set.push(handle);
}
handle
}
pub fn remove(
&mut self,
handle: RigidBodyHandle,
colliders: &mut ColliderSet,
joints: &mut JointSet,
) -> Option<RigidBody> {
let rb = self.bodies.remove(handle)?;
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
for active_set in &mut active_sets {
if active_set.get(rb.active_set_id) == Some(&handle) {
active_set.swap_remove(rb.active_set_id);
if let Some(replacement) = active_set.get(rb.active_set_id) {
self.bodies[*replacement].active_set_id = rb.active_set_id;
}
}
}
for collider in &rb.colliders {
colliders.remove(*collider, self);
}
joints.remove_rigid_body(rb.joint_graph_index, self);
Some(rb)
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
if let Some(rb) = self.bodies.get_mut(handle) {
if rb.is_dynamic() {
rb.wake_up(strong);
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
}
}
pub fn get_unknown_gen(&self, i: usize) -> Option<(&RigidBody, RigidBodyHandle)> {
self.bodies.get_unknown_gen(i)
}
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> {
let sender = &self.activation_channel.0;
self.bodies
.get_unknown_gen_mut(i)
.map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
}
pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
self.bodies.get(handle)
}
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<RigidBodyMut> {
let sender = &self.activation_channel.0;
self.bodies
.get_mut(handle)
.map(|rb| RigidBodyMut::new(handle, rb, sender))
}
pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
self.bodies.get_mut(handle)
}
pub(crate) fn get2_mut_internal(
&mut self,
h1: RigidBodyHandle,
h2: RigidBodyHandle,
) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
self.bodies.get2_mut(h1, h2)
}
pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
self.bodies.iter()
}
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyMut)> {
let sender = &self.activation_channel.0;
self.bodies
.iter_mut()
.map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
}
pub fn iter_active_kinematic<'a>(
&'a self,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let bodies: &'a _ = &self.bodies;
self.active_kinematic_set
.iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?)))
}
pub fn iter_active_dynamic<'a>(
&'a self,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let bodies: &'a _ = &self.bodies;
self.active_dynamic_set
.iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?)))
}
#[cfg(not(feature = "parallel"))]
pub(crate) fn iter_active_island<'a>(
&'a self,
island_id: usize,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
let bodies: &'a _ = &self.bodies;
self.active_dynamic_set[island_range]
.iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?)))
}
#[inline(always)]
pub(crate) fn foreach_active_body_mut_internal(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(*handle) {
f(*handle, rb)
}
}
for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(*handle) {
f(*handle, rb)
}
}
}
#[inline(always)]
pub(crate) fn foreach_active_dynamic_body_mut_internal(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(*handle) {
f(*handle, rb)
}
}
}
#[inline(always)]
pub(crate) fn foreach_active_kinematic_body_mut_internal(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(*handle) {
f(*handle, rb)
}
}
}
#[inline(always)]
#[cfg(not(feature = "parallel"))]
pub(crate) fn foreach_active_island_body_mut_internal(
&mut self,
island_id: usize,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
for handle in &self.active_dynamic_set[island_range] {
if let Some(rb) = self.bodies.get_mut(*handle) {
f(*handle, rb)
}
}
}
#[cfg(feature = "parallel")]
#[inline(always)]
#[allow(dead_code)]
pub(crate) fn foreach_active_island_body_mut_internal_parallel(
&mut self,
island_id: usize,
f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
) {
use std::sync::atomic::Ordering;
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
self.active_dynamic_set[island_range]
.par_iter()
.for_each_init(
|| bodies.load(Ordering::Relaxed),
|bodies, handle| {
let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
if let Some(rb) = bodies.get_mut(*handle) {
f(*handle, rb)
}
},
);
}
pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
self.active_islands[island_id]..self.active_islands[island_id + 1]
}
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
&self.active_dynamic_set[self.active_island_range(island_id)]
}
pub(crate) fn maintain_active_set(&mut self) {
for handle in self.activation_channel.1.try_iter() {
if let Some(rb) = self.bodies.get_mut(handle) {
if !rb.is_sleeping()
&& rb.is_dynamic()
&& self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
}
}
pub(crate) fn update_active_set_with_contacts(
&mut self,
colliders: &ColliderSet,
contact_graph: &InteractionGraph<ContactPair>,
joint_graph: &InteractionGraph<Joint>,
min_island_size: usize,
) {
assert!(
min_island_size > 0,
"The minimum island size must be at least 1."
);
self.active_set_timestamp += 1;
self.stack.clear();
self.can_sleep.clear();
for h in self.active_dynamic_set.drain(..).rev() {
let rb = &mut self.bodies[h];
rb.update_energy();
if rb.activation.energy <= rb.activation.threshold {
rb.activation.sleeping = true;
self.can_sleep.push(h);
} else {
self.stack.push(h);
}
}
#[inline(always)]
fn push_contacting_colliders(
rb: &RigidBody,
colliders: &ColliderSet,
contact_graph: &InteractionGraph<ContactPair>,
stack: &mut Vec<ColliderHandle>,
) {
for collider_handle in &rb.colliders {
let collider = &colliders[*collider_handle];
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other =
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
let other_body = colliders[other].parent;
stack.push(other_body);
break;
}
}
}
}
}
for h in self.active_kinematic_set.iter() {
let rb = &self.bodies[*h];
if !rb.is_moving() {
continue;
}
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
}
self.active_islands.clear();
self.active_islands.push(0);
let mut island_marker = self.stack.len().max(1) - 1;
while let Some(handle) = self.stack.pop() {
let rb = &mut self.bodies[handle];
if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
continue;
}
if self.stack.len() < island_marker {
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
self.active_islands.push(self.active_dynamic_set.len());
}
island_marker = self.stack.len();
}
rb.wake_up(false);
rb.active_island_id = self.active_islands.len() - 1;
rb.active_set_id = self.active_dynamic_set.len();
rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
rb.active_set_timestamp = self.active_set_timestamp;
self.active_dynamic_set.push(handle);
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::other_handle((inter.0, inter.1), handle);
self.stack.push(other);
}
}
self.active_islands.push(self.active_dynamic_set.len());
for h in &self.can_sleep {
let b = &mut self.bodies[*h];
if b.activation.sleeping {
b.sleep();
}
}
}
}
impl Index<RigidBodyHandle> for RigidBodySet {
type Output = RigidBody;
fn index(&self, index: RigidBodyHandle) -> &RigidBody {
&self.bodies[index]
}
}
impl IndexMut<RigidBodyHandle> for RigidBodySet {
fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
&mut self.bodies[index]
}
}