#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody, RigidBodyChanges};
use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
use parry::partitioning::IndexedData;
use std::ops::{Index, IndexMut};
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index);
impl RigidBodyHandle {
pub fn into_raw_parts(self) -> (usize, u64) {
self.0.into_raw_parts()
}
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
}
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
crate::INVALID_USIZE,
crate::INVALID_U64,
))
}
}
impl IndexedData for RigidBodyHandle {
fn default() -> Self {
Self(IndexedData::default())
}
fn index(&self) -> usize {
self.0.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 fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
BodyPair { body1, body2 }
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
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,
pub(crate) modified_bodies: Vec<RigidBodyHandle>,
pub(crate) modified_all_bodies: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
stack: Vec<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,
modified_bodies: Vec::new(),
modified_all_bodies: false,
can_sleep: Vec::new(),
stack: Vec::new(),
}
}
pub fn len(&self) -> usize {
self.bodies.len()
}
pub fn is_empty(&self) -> bool {
self.bodies.is_empty()
}
pub fn contains(&self, handle: RigidBodyHandle) -> bool {
self.bodies.contains(handle.0)
}
pub fn insert(&mut self, mut rb: RigidBody) -> RigidBodyHandle {
rb.reset_internal_references();
rb.changes.set(RigidBodyChanges::all(), true);
let handle = RigidBodyHandle(self.bodies.insert(rb));
self.modified_bodies.push(handle);
let rb = &mut self.bodies[handle.0];
if rb.is_kinematic() {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_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.0)?;
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.0].active_set_id = rb.active_set_id;
}
}
}
for collider in &rb.colliders {
colliders.remove(*collider, self, false);
}
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.0) {
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)
.map(|(b, h)| (b, RigidBodyHandle(h)))
}
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
let (rb, handle) = self.bodies.get_unknown_gen_mut(i)?;
let handle = RigidBodyHandle(handle);
Self::mark_as_modified(
handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Some((rb, handle))
}
pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
self.bodies.get(handle.0)
}
fn mark_as_modified(
handle: RigidBodyHandle,
rb: &mut RigidBody,
modified_bodies: &mut Vec<RigidBodyHandle>,
modified_all_bodies: bool,
) {
if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) {
rb.changes = RigidBodyChanges::MODIFIED;
modified_bodies.push(handle);
}
}
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle.0)?;
Self::mark_as_modified(
handle,
result,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Some(result)
}
pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
self.bodies.get_mut(handle.0)
}
pub(crate) fn get_mut_internal_with_modification_tracking(
&mut self,
handle: RigidBodyHandle,
) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle.0)?;
Self::mark_as_modified(
handle,
result,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Some(result)
}
pub(crate) fn get2_mut_internal(
&mut self,
h1: RigidBodyHandle,
h2: RigidBodyHandle,
) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
self.bodies.get2_mut(h1.0, h2.0)
}
pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
}
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
self.modified_bodies.clear();
self.modified_all_bodies = true;
self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b))
}
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.0)?)))
}
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.0)?)))
}
#[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.0)?)))
}
#[inline(always)]
#[cfg(not(feature = "dev-remove-slow-accessors"))]
pub fn foreach_active_dynamic_body_mut(
&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.0) {
Self::mark_as_modified(
*handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
f(*handle, rb)
}
}
}
#[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.0) {
f(*handle, rb)
}
}
for handle in &self.active_kinematic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
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.0) {
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.0) {
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.0) {
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.0) {
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)]
}
fn maintain_one(
bodies: &mut Arena<RigidBody>,
colliders: &mut ColliderSet,
handle: RigidBodyHandle,
modified_inactive_set: &mut Vec<RigidBodyHandle>,
active_kinematic_set: &mut Vec<RigidBodyHandle>,
active_dynamic_set: &mut Vec<RigidBodyHandle>,
) {
enum FinalAction {
UpdateActiveKinematicSetId,
UpdateActiveDynamicSetId,
}
if let Some(rb) = bodies.get_mut(handle.0) {
let mut final_action = None;
if rb.changes.contains(RigidBodyChanges::BODY_STATUS) {
match rb.body_status() {
BodyStatus::Dynamic => {
if active_kinematic_set.get(rb.active_set_id) == Some(&handle) {
active_kinematic_set.swap_remove(rb.active_set_id);
final_action =
Some((FinalAction::UpdateActiveKinematicSetId, rb.active_set_id));
}
rb.wake_up(true);
rb.changes.set(RigidBodyChanges::SLEEP, true);
}
BodyStatus::Kinematic => {
if active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
active_dynamic_set.swap_remove(rb.active_set_id);
final_action =
Some((FinalAction::UpdateActiveDynamicSetId, rb.active_set_id));
}
if active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = active_kinematic_set.len();
active_kinematic_set.push(handle);
}
}
BodyStatus::Static => {}
}
}
if rb.changes.contains(RigidBodyChanges::POSITION)
|| rb.changes.contains(RigidBodyChanges::COLLIDERS)
{
rb.update_colliders_positions(colliders);
if rb.is_static() {
modified_inactive_set.push(handle);
}
if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = active_kinematic_set.len();
active_kinematic_set.push(handle);
}
}
if rb.changes.contains(RigidBodyChanges::SLEEP)
&& !rb.is_sleeping()
&& rb.is_dynamic()
&& active_dynamic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = active_dynamic_set.len();
active_dynamic_set.push(handle);
}
rb.changes = RigidBodyChanges::empty();
if let Some((action, id)) = final_action {
let active_set = match action {
FinalAction::UpdateActiveKinematicSetId => active_kinematic_set,
FinalAction::UpdateActiveDynamicSetId => active_dynamic_set,
};
if id < active_set.len() {
if let Some(rb2) = bodies.get_mut(active_set[id].0) {
rb2.active_set_id = id;
}
}
}
}
}
pub(crate) fn handle_user_changes(&mut self, colliders: &mut ColliderSet) {
if self.modified_all_bodies {
for (handle, _) in self.bodies.iter_mut() {
self.modified_bodies.push(RigidBodyHandle(handle));
}
}
for handle in self.modified_bodies.drain(..) {
Self::maintain_one(
&mut self.bodies,
colliders,
handle,
&mut self.modified_inactive_set,
&mut self.active_kinematic_set,
&mut self.active_dynamic_set,
)
}
if self.modified_all_bodies {
self.modified_bodies.shrink_to_fit();
self.modified_all_bodies = false;
}
}
pub(crate) fn update_active_set_with_contacts(
&mut self,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
joint_graph: &InteractionGraph<RigidBodyHandle, 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.0];
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_bodies(
rb: &RigidBody,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
stack: &mut Vec<RigidBodyHandle>,
) {
for collider_handle in &rb.colliders {
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts {
for manifold in &inter.2.manifolds {
if !manifold.data.solver_contacts.is_empty() {
let other = crate::utils::select_other(
(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.0];
if !rb.is_moving() {
continue;
}
push_contacting_bodies(rb, colliders, narrow_phase, &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.0];
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_bodies(rb, colliders, narrow_phase, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::select_other((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.0];
if b.activation.sleeping {
b.sleep();
}
}
}
}
impl Index<RigidBodyHandle> for RigidBodySet {
type Output = RigidBody;
fn index(&self, index: RigidBodyHandle) -> &RigidBody {
&self.bodies[index.0]
}
}
#[cfg(not(feature = "dev-remove-slow-accessors"))]
impl IndexMut<RigidBodyHandle> for RigidBodySet {
fn index_mut(&mut self, handle: RigidBodyHandle) -> &mut RigidBody {
let rb = &mut self.bodies[handle.0];
Self::mark_as_modified(
handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
rb
}
}