Split rigid-bodies and colliders into multiple components
This commit is contained in:
Crozet Sébastien
@@ -1,48 +1,19 @@
|
||||
#[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 crate::data::{Arena, ComponentSet, ComponentSetMut, ComponentSetOption};
|
||||
use crate::dynamics::{
|
||||
IslandManager, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, RigidBodyHandle,
|
||||
RigidBodyType,
|
||||
};
|
||||
use crate::dynamics::{
|
||||
JointSet, RigidBody, RigidBodyCcd, RigidBodyChanges, RigidBodyDamping, RigidBodyForces,
|
||||
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
|
||||
};
|
||||
use crate::geometry::ColliderSet;
|
||||
use parry::partitioning::IndexedData;
|
||||
use std::ops::{Index, IndexMut};
|
||||
|
||||
/// The unique handle of a rigid body added to a `RigidBodySet`.
|
||||
#[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 {
|
||||
/// Converts this handle into its (index, generation) components.
|
||||
pub fn into_raw_parts(self) -> (usize, u64) {
|
||||
self.0.into_raw_parts()
|
||||
}
|
||||
|
||||
/// Reconstructs an handle from its (index, generation) components.
|
||||
pub fn from_raw_parts(id: usize, generation: u64) -> Self {
|
||||
Self(crate::data::arena::Index::from_raw_parts(id, generation))
|
||||
}
|
||||
|
||||
/// An always-invalid rigid-body handle.
|
||||
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))]
|
||||
/// A pair of rigid body handles.
|
||||
@@ -69,38 +40,75 @@ pub struct RigidBodySet {
|
||||
// parallelism because the `Receiver` breaks the Sync impl.
|
||||
// Could we avoid this?
|
||||
pub(crate) bodies: Arena<RigidBody>,
|
||||
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
|
||||
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
|
||||
// Set of inactive bodies which have been modified.
|
||||
// This typically include static bodies which have been modified.
|
||||
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>, // Workspace.
|
||||
#[cfg_attr(feature = "serde-serialize", serde(skip))]
|
||||
stack: Vec<RigidBodyHandle>, // Workspace.
|
||||
}
|
||||
|
||||
macro_rules! impl_field_component_set(
|
||||
($T: ty, $field: ident) => {
|
||||
impl ComponentSetOption<$T> for RigidBodySet {
|
||||
fn get(&self, handle: crate::data::Index) -> Option<&$T> {
|
||||
self.get(RigidBodyHandle(handle)).map(|b| &b.$field)
|
||||
}
|
||||
}
|
||||
|
||||
impl ComponentSet<$T> for RigidBodySet {
|
||||
fn size_hint(&self) -> usize {
|
||||
self.len()
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn for_each(&self, mut f: impl FnMut(crate::data::Index, &$T)) {
|
||||
for (handle, body) in self.bodies.iter() {
|
||||
f(handle, &body.$field)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ComponentSetMut<$T> for RigidBodySet {
|
||||
fn set_internal(&mut self, handle: crate::data::Index, val: $T) {
|
||||
if let Some(rb) = self.get_mut_internal(RigidBodyHandle(handle)) {
|
||||
rb.$field = val;
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn map_mut_internal<Result>(
|
||||
&mut self,
|
||||
handle: crate::data::Index,
|
||||
f: impl FnOnce(&mut $T) -> Result,
|
||||
) -> Option<Result> {
|
||||
self.get_mut_internal(RigidBodyHandle(handle)).map(|rb| f(&mut rb.$field))
|
||||
}
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
impl_field_component_set!(RigidBodyPosition, rb_pos);
|
||||
impl_field_component_set!(RigidBodyMassProps, rb_mprops);
|
||||
impl_field_component_set!(RigidBodyVelocity, rb_vels);
|
||||
impl_field_component_set!(RigidBodyDamping, rb_damping);
|
||||
impl_field_component_set!(RigidBodyForces, rb_forces);
|
||||
impl_field_component_set!(RigidBodyCcd, rb_ccd);
|
||||
impl_field_component_set!(RigidBodyIds, rb_ids);
|
||||
impl_field_component_set!(RigidBodyType, rb_type);
|
||||
impl_field_component_set!(RigidBodyActivation, rb_activation);
|
||||
impl_field_component_set!(RigidBodyColliders, rb_colliders);
|
||||
impl_field_component_set!(RigidBodyDominance, rb_dominance);
|
||||
impl_field_component_set!(RigidBodyChanges, changes);
|
||||
|
||||
impl RigidBodySet {
|
||||
/// Create a new empty set of rigid bodies.
|
||||
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(crate) fn take_modified(&mut self) -> Vec<RigidBodyHandle> {
|
||||
std::mem::replace(&mut self.modified_bodies, vec![])
|
||||
}
|
||||
|
||||
/// The number of rigid bodies on this set.
|
||||
pub fn len(&self) -> usize {
|
||||
self.bodies.len()
|
||||
@@ -121,18 +129,10 @@ impl RigidBodySet {
|
||||
// Make sure the internal links are reset, they may not be
|
||||
// if this rigid-body was obtained by cloning another one.
|
||||
rb.reset_internal_references();
|
||||
rb.changes.set(RigidBodyChanges::all(), true);
|
||||
rb.changes_mut_internal().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
|
||||
}
|
||||
|
||||
@@ -140,6 +140,7 @@ impl RigidBodySet {
|
||||
pub fn remove(
|
||||
&mut self,
|
||||
handle: RigidBodyHandle,
|
||||
islands: &mut IslandManager,
|
||||
colliders: &mut ColliderSet,
|
||||
joints: &mut JointSet,
|
||||
) -> Option<RigidBody> {
|
||||
@@ -147,55 +148,23 @@ impl RigidBodySet {
|
||||
/*
|
||||
* Update active sets.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
islands.rigid_body_removed(handle, &rb.rb_ids, self);
|
||||
|
||||
/*
|
||||
* Remove colliders attached to this rigid-body.
|
||||
*/
|
||||
for collider in &rb.colliders {
|
||||
colliders.remove(*collider, self, false);
|
||||
for collider in rb.colliders() {
|
||||
colliders.remove(*collider, islands, self, false);
|
||||
}
|
||||
|
||||
/*
|
||||
* Remove joints attached to this rigid-body.
|
||||
*/
|
||||
joints.remove_rigid_body(rb.joint_graph_index, self);
|
||||
joints.remove_rigid_body(rb.rb_ids.joint_graph_index, islands, self);
|
||||
|
||||
Some(rb)
|
||||
}
|
||||
|
||||
pub(crate) fn num_islands(&self) -> usize {
|
||||
self.active_islands.len() - 1
|
||||
}
|
||||
|
||||
/// Forces the specified rigid-body to wake up if it is dynamic.
|
||||
///
|
||||
/// If `strong` is `true` then it is assured that the rigid-body will
|
||||
/// remain awake during multiple subsequent timesteps.
|
||||
pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
|
||||
if let Some(rb) = self.bodies.get_mut(handle.0) {
|
||||
// TODO: what about kinematic bodies?
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets the rigid-body with the given handle without a known generation.
|
||||
///
|
||||
/// This is useful when you know you want the rigid-body at position `i` but
|
||||
@@ -224,12 +193,7 @@ impl RigidBodySet {
|
||||
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,
|
||||
);
|
||||
Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
|
||||
Some((rb, handle))
|
||||
}
|
||||
|
||||
@@ -238,14 +202,13 @@ impl RigidBodySet {
|
||||
self.bodies.get(handle.0)
|
||||
}
|
||||
|
||||
fn mark_as_modified(
|
||||
pub(crate) 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;
|
||||
if !rb.changes().contains(RigidBodyChanges::MODIFIED) {
|
||||
*rb.changes_mut_internal() = RigidBodyChanges::MODIFIED;
|
||||
modified_bodies.push(handle);
|
||||
}
|
||||
}
|
||||
@@ -254,12 +217,7 @@ impl RigidBodySet {
|
||||
#[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,
|
||||
);
|
||||
Self::mark_as_modified(handle, result, &mut self.modified_bodies);
|
||||
Some(result)
|
||||
}
|
||||
|
||||
@@ -274,23 +232,10 @@ impl RigidBodySet {
|
||||
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,
|
||||
);
|
||||
Self::mark_as_modified(handle, result, &mut self.modified_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)
|
||||
}
|
||||
|
||||
/// Iterates through all the rigid-bodies on this set.
|
||||
pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
|
||||
self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
|
||||
@@ -300,431 +245,11 @@ impl RigidBodySet {
|
||||
#[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))
|
||||
}
|
||||
|
||||
/// Iter through all the active kinematic rigid-bodies on this set.
|
||||
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)?)))
|
||||
}
|
||||
|
||||
/// Iter through all the active dynamic rigid-bodies on this set.
|
||||
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)?)))
|
||||
}
|
||||
|
||||
/// Applies the given function on all the active dynamic rigid-bodies
|
||||
/// contained by this set.
|
||||
#[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_dynamic_set(&self) -> &[RigidBodyHandle] {
|
||||
// &self.active_dynamic_set
|
||||
// }
|
||||
|
||||
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)]
|
||||
}
|
||||
|
||||
// Utility function to avoid some borrowing issue in the `maintain` method.
|
||||
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;
|
||||
|
||||
// The body's status changed. We need to make sure
|
||||
// it is on the correct active set.
|
||||
if rb.changes.contains(RigidBodyChanges::BODY_STATUS) {
|
||||
match rb.body_status() {
|
||||
BodyStatus::Dynamic => {
|
||||
// Remove from the active kinematic set if it was there.
|
||||
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));
|
||||
}
|
||||
|
||||
// Add to the active dynamic set.
|
||||
rb.wake_up(true);
|
||||
// Make sure the sleep change flag is set (even if for some
|
||||
// reasons the rigid-body was already awake) to make
|
||||
// sure the code handling sleeping change adds the body to
|
||||
// the active_dynamic_set.
|
||||
rb.changes.set(RigidBodyChanges::SLEEP, true);
|
||||
}
|
||||
BodyStatus::Kinematic => {
|
||||
// Remove from the active dynamic set if it was there.
|
||||
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));
|
||||
}
|
||||
|
||||
// Add to the active kinematic set.
|
||||
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 => {}
|
||||
}
|
||||
}
|
||||
|
||||
// Update the positions of the colliders.
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// Push the body to the active set if it is not
|
||||
// sleeping and if it is not already inside of the active set.
|
||||
if rb.changes.contains(RigidBodyChanges::SLEEP)
|
||||
&& !rb.is_sleeping() // May happen if the body was put to sleep manually.
|
||||
&& rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
|
||||
&& active_dynamic_set.get(rb.active_set_id) != Some(&handle)
|
||||
{
|
||||
rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
|
||||
active_dynamic_set.push(handle);
|
||||
}
|
||||
|
||||
rb.changes = RigidBodyChanges::empty();
|
||||
|
||||
// Adjust some ids, if needed.
|
||||
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 {
|
||||
// Unfortunately, we have to push all the bodies to `modified_bodies`
|
||||
// instead of just calling `maintain_one` on each element i
|
||||
// `self.bodies.iter_mut()` because otherwise it would be difficult to
|
||||
// handle the final change of active_set_id in Self::maintain_one
|
||||
// (because it has to modify another rigid-body because of the swap-remove.
|
||||
// So this causes borrowing problems if we do this while iterating
|
||||
// through self.bodies.iter_mut()).
|
||||
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(); // save some memory.
|
||||
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."
|
||||
);
|
||||
|
||||
// Update the energy of every rigid body and
|
||||
// keep only those that may not sleep.
|
||||
// let t = instant::now();
|
||||
self.active_set_timestamp += 1;
|
||||
self.stack.clear();
|
||||
self.can_sleep.clear();
|
||||
|
||||
// NOTE: the `.rev()` is here so that two successive timesteps preserve
|
||||
// the order of the bodies in the `active_dynamic_set` vec. This reversal
|
||||
// does not seem to affect performances nor stability. However it makes
|
||||
// debugging slightly nicer so we keep this rev.
|
||||
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 {
|
||||
// Mark them as sleeping for now. This will
|
||||
// be set to false during the graph traversal
|
||||
// if it should not be put to sleep.
|
||||
rb.activation.sleeping = true;
|
||||
self.can_sleep.push(h);
|
||||
} else {
|
||||
self.stack.push(h);
|
||||
}
|
||||
}
|
||||
|
||||
// Read all the contacts and push objects touching touching this rigid-body.
|
||||
#[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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Now iterate on all active kinematic bodies and push all the bodies
|
||||
// touching them to the stack so they can be woken up.
|
||||
for h in self.active_kinematic_set.iter() {
|
||||
let rb = &self.bodies[h.0];
|
||||
|
||||
if !rb.is_moving() {
|
||||
// If the kinematic body does not move, it does not have
|
||||
// to wake up any dynamic body.
|
||||
continue;
|
||||
}
|
||||
|
||||
push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
|
||||
}
|
||||
|
||||
// println!("Selection: {}", instant::now() - t);
|
||||
|
||||
// let t = instant::now();
|
||||
// Propagation of awake state and awake island computation through the
|
||||
// traversal of the interaction graph.
|
||||
self.active_islands.clear();
|
||||
self.active_islands.push(0);
|
||||
|
||||
// The max avoid underflow when the stack is empty.
|
||||
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() {
|
||||
// We already visited this body and its neighbors.
|
||||
// Also, we don't propagate awake state through static bodies.
|
||||
continue;
|
||||
}
|
||||
|
||||
if self.stack.len() < island_marker {
|
||||
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
|
||||
>= min_island_size
|
||||
{
|
||||
// We are starting a new island.
|
||||
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);
|
||||
|
||||
// Transmit the active state to all the rigid-bodies with colliders
|
||||
// in contact or joined with this collider.
|
||||
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());
|
||||
// println!(
|
||||
// "Extraction: {}, num islands: {}",
|
||||
// instant::now() - t,
|
||||
// self.active_islands.len() - 1
|
||||
// );
|
||||
|
||||
// Actually put to sleep bodies which have not been detected as awake.
|
||||
// let t = instant::now();
|
||||
for h in &self.can_sleep {
|
||||
let b = &mut self.bodies[h.0];
|
||||
if b.activation.sleeping {
|
||||
b.sleep();
|
||||
}
|
||||
}
|
||||
// println!("Activation: {}", instant::now() - t);
|
||||
let modified_bodies = &mut self.modified_bodies;
|
||||
self.bodies.iter_mut().map(move |(h, b)| {
|
||||
modified_bodies.push(RigidBodyHandle(h));
|
||||
(RigidBodyHandle(h), b)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
@@ -736,16 +261,19 @@ impl Index<RigidBodyHandle> for RigidBodySet {
|
||||
}
|
||||
}
|
||||
|
||||
impl Index<crate::data::Index> for RigidBodySet {
|
||||
type Output = RigidBody;
|
||||
|
||||
fn index(&self, index: crate::data::Index) -> &RigidBody {
|
||||
&self.bodies[index]
|
||||
}
|
||||
}
|
||||
|
||||
#[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,
|
||||
);
|
||||
Self::mark_as_modified(handle, rb, &mut self.modified_bodies);
|
||||
rb
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user