nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: persistent islands + manifold reduction (#895)

Browse Source 
* feat: initial implementation of contact manifold reduction

* feat: try bepu-like manifold reduction

* feat: simplification of the constraints counting and indexing logic

* feat: add concept of incremental islands with a single awake island

More islands manager fixes

* feat: start adding support for multiple awake islands

* feat: add more timings

* feat: implement incremental island split & merge

* chore: refactor islands manager into multiple files

* chore: refactor manifold reduction to its own file + add naive reduction method

* feat: add islands manager validation checks

* fix various bugs in the new islands system

* chore: remove redundant active_set_offset field
This commit is contained in:
Sébastien Crozet
2026-01-09 17:04:02 +01:00
committed by GitHub
parent 134132900a
commit 48de83817e
40 changed files with 2099 additions and 1114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
crates/rapier2d-f64/Cargo.toml
View File

@@ -43,6 +43,7 @@ serde-serialize = [
"dep:serde",
"bit-vec/serde",
"arrayvec/serde",
"vec_map/serde"
]
enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"]
debug-render = []

1
crates/rapier2d/Cargo.toml
View File

@@ -44,6 +44,7 @@ serde-serialize = [
"dep:serde",
"bit-vec/serde",
"arrayvec/serde",
"vec_map/serde"
]
enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"]
debug-render = []

1
crates/rapier3d-f64/Cargo.toml
View File

@@ -46,6 +46,7 @@ serde-serialize = [
"parry3d-f64/serde-serialize",
"dep:serde",
"bit-vec/serde",
"vec_map/serde"
]
enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"]
debug-render = []

1
crates/rapier3d/Cargo.toml
View File

@@ -48,6 +48,7 @@ serde-serialize = [
"parry3d/serde-serialize",
"dep:serde",
"bit-vec/serde",
"vec_map/serde"
]
enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"]
debug-render = []

2
examples2d/one_way_platforms2.rs
View File

@@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) {
}
for handle in physics.islands.active_bodies() {
let body = &mut physics.bodies[*handle];
let body = &mut physics.bodies[handle];
if body.position().translation.y > 1.0 {
body.set_gravity_scale(1.0, false);
} else if body.position().translation.y < -1.0 {

2
examples3d/one_way_platforms3.rs
View File

@@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) {
}
for handle in physics.islands.active_bodies() {
let body = physics.bodies.get_mut(*handle).unwrap();
let body = physics.bodies.get_mut(handle).unwrap();
if body.position().translation.y > 1.0 {
body.set_gravity_scale(1.0, false);
} else if body.position().translation.y < -1.0 {

6
src/counters/collision_detection_counters.rs
View File

@@ -8,6 +8,9 @@ pub struct CollisionDetectionCounters {
pub ncontact_pairs: usize,
/// Time spent for the broad-phase of the collision detection.
pub broad_phase_time: Timer,
/// Time spent by the final broad-phase AABB update after body movement to keep
/// user scene queries valid.
pub final_broad_phase_time: Timer,
/// Time spent for the narrow-phase of the collision detection.
pub narrow_phase_time: Timer,
}
@@ -18,6 +21,7 @@ impl CollisionDetectionCounters {
CollisionDetectionCounters {
ncontact_pairs: 0,
broad_phase_time: Timer::new(),
final_broad_phase_time: Timer::new(),
narrow_phase_time: Timer::new(),
}
}
@@ -26,6 +30,7 @@ impl CollisionDetectionCounters {
pub fn reset(&mut self) {
self.ncontact_pairs = 0;
self.broad_phase_time.reset();
self.final_broad_phase_time.reset();
self.narrow_phase_time.reset();
}
}
@@ -34,6 +39,7 @@ impl Display for CollisionDetectionCounters {
fn fmt(&self, f: &mut Formatter) -> Result {
writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?;
writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
writeln!(f, "Final broad-phase time: {}", self.final_broad_phase_time)?;
writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
}
}

8
src/counters/solver_counters.rs
View File

@@ -12,6 +12,10 @@ pub struct SolverCounters {
pub velocity_resolution_time: Timer,
/// Time spent for the assembly of all the velocity constraints.
pub velocity_assembly_time: Timer,
/// Time spent by the velocity assembly for initializing solver bodies.
pub velocity_assembly_time_solver_bodies: Timer,
/// Time spent by the velocity assemble for initializing the constraints.
pub velocity_assembly_time_constraints_init: Timer,
/// Time spent for the update of the velocity of the bodies.
pub velocity_update_time: Timer,
/// Time spent to write force back to user-accessible data.
@@ -25,6 +29,8 @@ impl SolverCounters {
nconstraints: 0,
ncontacts: 0,
velocity_assembly_time: Timer::new(),
velocity_assembly_time_solver_bodies: Timer::new(),
velocity_assembly_time_constraints_init: Timer::new(),
velocity_resolution_time: Timer::new(),
velocity_update_time: Timer::new(),
velocity_writeback_time: Timer::new(),
@@ -37,6 +43,8 @@ impl SolverCounters {
self.ncontacts = 0;
self.velocity_resolution_time.reset();
self.velocity_assembly_time.reset();
self.velocity_assembly_time_solver_bodies.reset();
self.velocity_assembly_time_constraints_init.reset();
self.velocity_update_time.reset();
self.velocity_writeback_time.reset();
}

9
src/counters/stages_counters.rs
View File

@@ -10,6 +10,8 @@ pub struct StagesCounters {
pub collision_detection_time: Timer,
/// Time spent for the computation of collision island and body activation/deactivation (sleeping).
pub island_construction_time: Timer,
/// Time spent for collecting awake constraints from islands.
pub island_constraints_collection_time: Timer,
/// Total time spent for the constraints resolution and position update.t
pub solver_time: Timer,
/// Total time spent for CCD and CCD resolution.
@@ -25,6 +27,7 @@ impl StagesCounters {
update_time: Timer::new(),
collision_detection_time: Timer::new(),
island_construction_time: Timer::new(),
island_constraints_collection_time: Timer::new(),
solver_time: Timer::new(),
ccd_time: Timer::new(),
user_changes: Timer::new(),
@@ -36,6 +39,7 @@ impl StagesCounters {
self.update_time.reset();
self.collision_detection_time.reset();
self.island_construction_time.reset();
self.island_constraints_collection_time.reset();
self.solver_time.reset();
self.ccd_time.reset();
self.user_changes.reset();
@@ -55,6 +59,11 @@ impl Display for StagesCounters {
"Island construction time: {}",
self.island_construction_time
)?;
writeln!(
f,
"Island construction time: {}",
self.island_constraints_collection_time
)?;
writeln!(f, "Solver time: {}", self.solver_time)?;
writeln!(f, "CCD time: {}", self.ccd_time)?;
writeln!(f, "User changes time: {}", self.user_changes)

6
src/dynamics/ccd/ccd_solver.rs
View File

@@ -88,7 +88,7 @@ impl CCDSolver {
// println!("Checking CCD activation");
for handle in islands.active_bodies() {
let rb = bodies.index_mut_internal(*handle);
let rb = bodies.index_mut_internal(handle);
if rb.ccd.ccd_enabled {
let forces = if include_forces {
@@ -143,7 +143,7 @@ impl CCDSolver {
let mut min_toi = dt;
for handle in islands.active_bodies() {
let rb1 = &bodies[*handle];
let rb1 = &bodies[handle];
if rb1.ccd.ccd_active {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
@@ -276,7 +276,7 @@ impl CCDSolver {
*/
// TODO: don't iterate through all the colliders.
for handle in islands.active_bodies() {
let rb1 = &bodies[*handle];
let rb1 = &bodies[handle];
if rb1.ccd.ccd_active {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(

357
src/dynamics/island_manager.rs
View File

@@ -1,357 +0,0 @@
use crate::dynamics::{
ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders,
RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::{ColliderSet, NarrowPhase};
use crate::math::Real;
use crate::utils::SimdDot;
/// System that manages which bodies are active (awake) vs sleeping to optimize performance.
///
/// ## Sleeping Optimization
///
/// Bodies at rest automatically "sleep" - they're excluded from simulation until something
/// disturbs them (collision, joint connection to moving body, manual wake-up). This can
/// dramatically improve performance in scenes with many static/resting objects.
///
/// ## Islands
///
/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together.
/// This allows parallel solving and better organization.
///
/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline).
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Default)]
pub struct IslandManager {
pub(crate) active_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
active_set_timestamp: u32,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>, // Workspace.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
stack: Vec<RigidBodyHandle>, // Workspace.
}
impl IslandManager {
/// Creates a new empty island manager.
pub fn new() -> Self {
Self {
active_set: vec![],
active_islands: vec![],
active_islands_additional_solver_iterations: vec![],
active_set_timestamp: 0,
can_sleep: vec![],
stack: vec![],
}
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len().saturating_sub(1)
}
/// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`.
pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) {
let mut i = 0;
while i < self.active_set.len() {
let handle = self.active_set[i];
if bodies.get(handle).is_none() {
// This rigid-body no longer exists, so we need to remove it from the active set.
self.active_set.swap_remove(i);
if i < self.active_set.len() {
// Update the self.active_set_id for the body that has been swapped.
if let Some(swapped_rb) = bodies.get_mut_internal(self.active_set[i]) {
swapped_rb.ids.active_set_id = i;
}
}
} else {
i += 1;
}
}
}
pub(crate) fn rigid_body_removed(
&mut self,
removed_handle: RigidBodyHandle,
removed_ids: &RigidBodyIds,
bodies: &mut RigidBodySet,
) {
if self.active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
self.active_set.swap_remove(removed_ids.active_set_id);
if let Some(replacement) = self
.active_set
.get(removed_ids.active_set_id)
.and_then(|h| bodies.get_mut_internal(*h))
{
replacement.ids.active_set_id = removed_ids.active_set_id;
}
}
}
/// Wakes up a sleeping body, forcing it back into the active simulation.
///
/// Use this when you want to ensure a body is active (useful after manually moving
/// a sleeping body, or to prevent it from sleeping in the next few frames).
///
/// # Parameters
/// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames.
/// If `false`, it might sleep again immediately if conditions are met.
///
/// # Example
/// ```
/// # use rapier3d::prelude::*;
/// # let mut bodies = RigidBodySet::new();
/// # let mut islands = IslandManager::new();
/// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic());
/// islands.wake_up(&mut bodies, body_handle, true);
/// let body = bodies.get_mut(body_handle).unwrap();
/// // Wake up a body before applying force to it
/// body.add_force(vector![100.0, 0.0, 0.0], false);
/// ```
///
/// Only affects dynamic bodies (kinematic and fixed bodies don't sleep).
pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) {
// NOTE: the use an Option here because there are many legitimate cases (like when
// deleting a joint attached to an already-removed body) where we could be
// attempting to wake-up a rigid-body that has already been deleted.
if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
let rb = bodies.index_mut_internal(handle);
// Check that the user didn’t change the sleeping state explicitly, in which
// case we don’t overwrite it.
if !rb.changes.contains(RigidBodyChanges::SLEEP) {
rb.activation.wake_up(strong);
if rb.is_enabled() && self.active_set.get(rb.ids.active_set_id) != Some(&handle) {
rb.ids.active_set_id = self.active_set.len();
self.active_set.push(handle);
}
}
}
}
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
&self.active_set[island_range]
}
pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize {
self.active_islands_additional_solver_iterations[island_id]
}
/// Handls of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping).
#[inline]
pub fn active_bodies(&self) -> &[RigidBodyHandle] {
&self.active_set
}
#[cfg(feature = "parallel")]
#[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism.
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 update_active_set_with_contacts(
&mut self,
dt: Real,
length_unit: Real,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
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_set` vec. This reversal
// does not seem to affect performances nor stability. However it makes
// debugging slightly nicer.
for h in self.active_set.drain(..).rev() {
let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack;
let rb = bodies.index_mut_internal(h);
let sq_linvel = rb.vels.linvel.norm_squared();
let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
update_energy(
&mut rb.activation,
rb.body_type,
length_unit,
sq_linvel,
sq_angvel,
dt,
);
if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep {
// 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;
can_sleep.push(h);
} else {
stack.push(h);
}
}
// Read all the contacts and push objects touching touching this rigid-body.
#[inline]
fn push_contacting_bodies(
rb_colliders: &RigidBodyColliders,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
stack: &mut Vec<RigidBodyHandle>,
) {
for collider_handle in &rb_colliders.0 {
for inter in narrow_phase.contact_pairs_with(*collider_handle) {
for manifold in &inter.manifolds {
if !manifold.data.solver_contacts.is_empty() {
let other = crate::utils::select_other(
(inter.collider1, inter.collider2),
*collider_handle,
);
if let Some(other_body) = colliders[other].parent {
stack.push(other_body.handle);
}
break;
}
}
}
}
}
// 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_additional_solver_iterations.clear();
self.active_islands.clear();
self.active_islands.push(0);
// saturating_sub(1) prevents underflow when the stack is empty.
let mut island_marker = self.stack.len().saturating_sub(1);
// NOTE: islands containing a body with non-standard number of iterations won’t
// be merged with another island, unless another island with standard
// iterations number already started before and got continued due to the
// `min_island_size`. That could be avoided by pushing bodies with non-standard
// iterations on top of the stack (and other bodies on the back). Not sure it’s
// worth it though.
let mut additional_solver_iterations = 0;
while let Some(handle) = self.stack.pop() {
let rb = bodies.index_mut_internal(handle);
if rb.ids.active_set_timestamp == self.active_set_timestamp
|| !rb.is_dynamic_or_kinematic()
{
// We already visited this body and its neighbors.
// Also, we don't propagate awake state through fixed bodies.
continue;
}
if self.stack.len() < island_marker {
if additional_solver_iterations != rb.additional_solver_iterations
|| self.active_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
// We are starting a new island.
self.active_islands_additional_solver_iterations
.push(additional_solver_iterations);
self.active_islands.push(self.active_set.len());
additional_solver_iterations = 0;
}
island_marker = self.stack.len();
}
additional_solver_iterations =
additional_solver_iterations.max(rb.additional_solver_iterations);
// Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider.
push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
for inter in impulse_joints.attached_enabled_joints(handle) {
let other = crate::utils::select_other((inter.0, inter.1), handle);
self.stack.push(other);
}
for other in multibody_joints.bodies_attached_with_enabled_joint(handle) {
self.stack.push(other);
}
rb.activation.wake_up(false);
rb.ids.active_island_id = self.active_islands.len() - 1;
rb.ids.active_set_id = self.active_set.len();
rb.ids.active_set_offset =
(rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]) as u32;
rb.ids.active_set_timestamp = self.active_set_timestamp;
self.active_set.push(handle);
}
self.active_islands_additional_solver_iterations
.push(additional_solver_iterations);
self.active_islands.push(self.active_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.
for handle in &self.can_sleep {
let rb = bodies.index_mut_internal(*handle);
if rb.activation.sleeping {
rb.vels = RigidBodyVelocity::zero();
rb.activation.sleep();
}
}
}
}
fn update_energy(
activation: &mut RigidBodyActivation,
body_type: RigidBodyType,
length_unit: Real,
sq_linvel: Real,
sq_angvel: Real,
dt: Real,
) {
let can_sleep = match body_type {
RigidBodyType::Dynamic => {
let linear_threshold = activation.normalized_linear_threshold * length_unit;
sq_linvel < linear_threshold * linear_threshold.abs()
&& sq_angvel < activation.angular_threshold * activation.angular_threshold.abs()
}
RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => {
// Platforms only sleep if both velocities are exactly zero. If it’s not exactly
// zero, then the user really wants them to move.
sq_linvel == 0.0 && sq_angvel == 0.0
}
RigidBodyType::Fixed => true,
};
if can_sleep {
activation.time_since_can_sleep += dt;
} else {
activation.time_since_can_sleep = 0.0;
}
}

163
src/dynamics/island_manager/island.rs Normal file
View File

@@ -0,0 +1,163 @@
use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
use super::IslandManager;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Default)]
pub(crate) struct Island {
/// The rigid-bodies part of this island.
pub(super) bodies: Vec<RigidBodyHandle>,
/// The additional solver iterations needed by this island.
pub(super) additional_solver_iterations: usize,
/// Index of this island in `IslandManager::awake_islands`.
///
/// If `None`, the island is sleeping.
pub(super) id_in_awake_list: Option<usize>,
}
impl Island {
pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self {
Self {
bodies: vec![handle],
additional_solver_iterations: rb.additional_solver_iterations,
id_in_awake_list: None,
}
}
pub fn bodies(&self) -> &[RigidBodyHandle] {
&self.bodies
}
pub fn additional_solver_iterations(&self) -> usize {
self.additional_solver_iterations
}
pub fn is_sleeping(&self) -> bool {
self.id_in_awake_list.is_none()
}
pub fn len(&self) -> usize {
self.bodies.len()
}
pub(crate) fn id_in_awake_list(&self) -> Option<usize> {
self.id_in_awake_list
}
}
impl IslandManager {
/// Remove from the island at `source_id` all the rigid-body that are in `new_island`, and
/// insert `new_island` into the islands set.
///
/// **All** rigid-bodies from `new_island` must currently be part of the island at `source_id`.
pub(super) fn extract_sub_island(
&mut self,
bodies: &mut RigidBodySet,
source_id: usize,
mut new_island: Island,
sleep: bool,
) {
let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len());
let source_island = &mut self.islands[source_id];
for (id, handle) in new_island.bodies.iter().enumerate() {
let rb = bodies.index_mut_internal(*handle);
// If the new island is sleeping, ensure all its bodies are sleeping.
if sleep {
rb.sleep();
}
let id_to_remove = rb.ids.active_set_id;
assert_eq!(
rb.ids.active_island_id, source_id,
"note, new id: {}",
new_island_id
);
rb.ids.active_island_id = new_island_id;
rb.ids.active_set_id = id;
new_island.additional_solver_iterations = new_island
.additional_solver_iterations
.max(rb.additional_solver_iterations);
source_island.bodies.swap_remove(id_to_remove);
if let Some(moved_handle) = source_island.bodies.get(id_to_remove).copied() {
let moved_rb = bodies.index_mut_internal(moved_handle);
moved_rb.ids.active_set_id = id_to_remove;
}
}
// If the new island is awake, add it to the awake list.
if !sleep {
new_island.id_in_awake_list = Some(self.awake_islands.len());
self.awake_islands.push(new_island_id);
} else {
new_island.id_in_awake_list = None;
}
self.islands.insert(new_island_id, new_island);
}
pub(super) fn merge_islands(
&mut self,
bodies: &mut RigidBodySet,
island_id1: usize,
island_id2: usize,
) {
if island_id1 == island_id2 {
return;
}
let island1 = &self.islands[island_id1];
let island2 = &self.islands[island_id2];
assert_eq!(
island1.id_in_awake_list.is_some(),
island2.id_in_awake_list.is_some(),
"Internal error: cannot merge two island with different sleeping statuses."
);
// Prefer removing the smallest island to reduce the amount of memory to move.
let (to_keep, to_remove) = if island1.bodies.len() < island2.bodies.len() {
(island_id2, island_id1)
} else {
(island_id1, island_id2)
};
// println!("Merging: {} <- {}", to_keep, to_remove);
let Some(removed_island) = self.islands.remove(to_remove) else {
// TODO: the island doesn’t exist is that an internal error?
return;
};
self.free_islands.push(to_remove);
// TODO: if we switched to linked list, we could avoid moving around all this memory.
let target_island = &mut self.islands[to_keep];
for handle in &removed_island.bodies {
let Some(rb) = bodies.get_mut_internal(*handle) else {
// This body no longer exists.
continue;
};
rb.wake_up(false);
rb.ids.active_island_id = to_keep;
rb.ids.active_set_id = target_island.bodies.len();
target_island.bodies.push(*handle);
target_island.additional_solver_iterations = target_island
.additional_solver_iterations
.max(rb.additional_solver_iterations);
}
// Update the awake_islands list.
if let Some(awake_id_to_remove) = removed_island.id_in_awake_list {
self.awake_islands.swap_remove(awake_id_to_remove);
// Update the awake list index of the awake island id we moved.
if let Some(moved_id) = self.awake_islands.get(awake_id_to_remove) {
self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove);
}
}
}
}

290
src/dynamics/island_manager/manager.rs Normal file
View File

@@ -0,0 +1,290 @@
use super::{Island, IslandsOptimizer};
use crate::dynamics::{
ImpulseJointSet, MultibodyJointSet, RigidBodyChanges, RigidBodyHandle, RigidBodyIds,
RigidBodySet,
};
use crate::geometry::{ColliderSet, NarrowPhase};
use crate::math::Real;
use crate::prelude::SleepRootState;
use crate::utils::SimdDot;
use std::collections::VecDeque;
use vec_map::VecMap;
/// An island starting at this rigid-body might be eligible for sleeping.
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(super) struct SleepCandidate(RigidBodyHandle);
/// System that manages which bodies are active (awake) vs sleeping to optimize performance.
///
/// ## Sleeping Optimization
///
/// Bodies at rest automatically "sleep" - they're excluded from simulation until something
/// disturbs them (collision, joint connection to moving body, manual wake-up). This can
/// dramatically improve performance in scenes with many static/resting objects.
///
/// ## Islands
///
/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together.
/// This allows parallel solving and better organization.
///
/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline).
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Default)]
pub struct IslandManager {
pub(crate) islands: VecMap<Island>,
pub(crate) awake_islands: Vec<usize>,
// TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation?
pub(crate) free_islands: Vec<usize>,
/// Potential candidate roots for graph traversal to identify a sleeping
/// connected component or to split an island in two.
pub(super) traversal_candidates: VecDeque<SleepCandidate>,
pub(super) traversal_timestamp: u32,
pub(super) optimizer: IslandsOptimizer,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub(super) stack: Vec<RigidBodyHandle>, // Workspace.
}
impl IslandManager {
/// Creates a new empty island manager.
pub fn new() -> Self {
Self::default()
}
pub(crate) fn active_islands(&self) -> &[usize] {
&self.awake_islands
}
pub(crate) fn rigid_body_removed(
&mut self,
removed_handle: RigidBodyHandle,
removed_ids: &RigidBodyIds,
bodies: &mut RigidBodySet,
) {
let Some(island) = self.islands.get_mut(removed_ids.active_island_id) else {
// The island already doesn’t exist.
return;
};
let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle);
island.bodies.swap_remove(removed_ids.active_set_id);
// Remap the active_set_id of the body we moved with the `swap_remove`.
if swapped_handle != removed_handle {
let swapped_body = bodies
.get_mut(swapped_handle)
.expect("Internal error: bodies must be removed from islands on at a times");
swapped_body.ids.active_set_id = removed_ids.active_set_id;
}
// If we deleted the last body from this island, delete the island.
if island.bodies.is_empty() {
if let Some(awake_id) = island.id_in_awake_list {
// Remove it from the free island list.
self.awake_islands.swap_remove(awake_id);
// Update the awake list index of the awake island id we moved.
if let Some(moved_id) = self.awake_islands.get(awake_id) {
self.islands[*moved_id].id_in_awake_list = Some(awake_id);
}
}
self.islands.remove(removed_ids.active_island_id);
self.free_islands.push(removed_ids.active_island_id);
}
}
pub(crate) fn interaction_started_or_stopped(
&mut self,
bodies: &mut RigidBodySet,
handle1: Option<RigidBodyHandle>,
handle2: Option<RigidBodyHandle>,
started: bool,
wake_up: bool,
) {
match (handle1, handle2) {
(Some(handle1), Some(handle2)) => {
if wake_up {
self.wake_up(bodies, handle1, false);
self.wake_up(bodies, handle2, false);
}
if started {
if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) {
assert!(rb1.is_fixed() || rb1.ids.active_island_id != usize::MAX);
assert!(rb2.is_fixed() || rb2.ids.active_island_id != usize::MAX);
// If both bodies are not part of the same island, merge the islands.
if !rb1.is_fixed()
&& !rb2.is_fixed()
&& rb1.ids.active_island_id != rb2.ids.active_island_id
{
self.merge_islands(
bodies,
rb1.ids.active_island_id,
rb2.ids.active_island_id,
);
}
}
}
}
(Some(handle1), None) => {
if wake_up {
// NOTE: see NOTE of the Some(_), Some(_) case.
self.wake_up(bodies, handle1, false);
}
}
(None, Some(handle2)) => {
if wake_up {
// NOTE: see NOTE of the Some(_), Some(_) case.
self.wake_up(bodies, handle2, false);
}
}
(None, None) => { /* Nothing to do. */ }
}
}
pub(crate) fn island(&self, island_id: usize) -> &Island {
&self.islands[island_id]
}
/// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping).
#[inline]
pub fn active_bodies(&self) -> impl Iterator<Item = RigidBodyHandle> + '_ {
self.awake_islands
.iter()
.flat_map(|i| self.islands[*i].bodies.iter().copied())
}
pub(crate) fn rigid_body_updated(
&mut self,
handle: RigidBodyHandle,
bodies: &mut RigidBodySet,
) {
let Some(rb) = bodies.get_mut(handle) else {
return;
};
if rb.is_fixed() {
return;
}
// Check if this is the first time we see this rigid-body.
if rb.ids.active_island_id == usize::MAX {
// Check if there is room in the last awake island to add this body.
// NOTE: only checking the last is suboptimal. Perhaps we should keep vec of
// small islands ids?
let insert_in_last_island = self.awake_islands.last().map(|id| {
self.islands[*id].bodies.len() < self.optimizer.min_island_size
&& self.islands[*id].is_sleeping() == rb.is_sleeping()
});
// let insert_in_last_island = insert_in_last_island.is_some().then_some(true);
if !rb.is_sleeping() && insert_in_last_island == Some(true) {
let id = *self.awake_islands.last().unwrap_or_else(|| unreachable!());
let target_island = &mut self.islands[id];
rb.ids.active_island_id = id;
rb.ids.active_set_id = target_island.bodies.len();
target_island.bodies.push(handle);
} else {
let mut new_island = Island::singleton(handle, rb);
let id = self.free_islands.pop().unwrap_or(self.islands.len());
if !rb.is_sleeping() {
new_island.id_in_awake_list = Some(self.awake_islands.len());
self.awake_islands.push(id);
}
self.islands.insert(id, new_island);
rb.ids.active_island_id = id;
rb.ids.active_set_id = 0;
}
}
// Push the body to the active set if it is not inside the active set yet, and
// is not longer sleeping or became dynamic.
if (rb.changes.contains(RigidBodyChanges::SLEEP)
|| rb.changes.contains(RigidBodyChanges::TYPE))
&& rb.is_enabled()
// Don’t wake up if the user put it to sleep manually.
&& !rb.activation.sleeping
{
self.wake_up(bodies, handle, false);
}
}
pub(crate) fn update_islands(
&mut self,
dt: Real,
length_unit: Real,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
) {
// 1. Update active rigid-bodies energy.
// TODO PERF: should this done by the velocity solver after solving the constraints?
// let t0 = std::time::Instant::now();
for handle in self
.awake_islands
.iter()
.flat_map(|i| self.islands[*i].bodies.iter().copied())
{
let Some(rb) = bodies.get_mut_internal(handle) else {
// This branch happens if the rigid-body no longer exists.
continue;
};
let sq_linvel = rb.vels.linvel.norm_squared();
let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
rb.activation
.update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt);
let can_sleep_now = rb.activation.is_eligible_for_sleep();
// 2. Identify active rigid-bodies that transition from "awake" to "can_sleep"
// and push the sleep root candidate if applicable.
if can_sleep_now && rb.activation.sleep_root_state == SleepRootState::Unknown {
// This is a new candidate for island extraction.
self.traversal_candidates.push_back(SleepCandidate(handle));
rb.activation.sleep_root_state = SleepRootState::TraversalPending;
} else if !can_sleep_now {
rb.activation.sleep_root_state = SleepRootState::Unknown;
}
}
// println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0);
let mut cost = 0;
// 3. Perform one, or multiple, sleeping islands extraction (graph traversal).
// Limit the traversal cost by not traversing all the known sleeping roots if
// there are too many.
const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value.
while let Some(sleep_root) = self.traversal_candidates.pop_front() {
cost += self.extract_sleeping_island(
bodies,
colliders,
impulse_joints,
multibody_joints,
narrow_phase,
sleep_root.0,
);
if cost > MAX_PER_FRAME_COST {
// Early-break if we consider we have done enough island extraction work.
break;
}
}
self.update_optimizer(
bodies,
colliders,
impulse_joints,
multibody_joints,
narrow_phase,
);
// println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0);
// NOTE: uncomment for debugging.
// self.assert_state_is_valid(bodies, colliders, narrow_phase);
}
}

11
src/dynamics/island_manager/mod.rs Normal file
View File

@@ -0,0 +1,11 @@
pub use manager::IslandManager;
pub(crate) use island::Island;
use optimizer::IslandsOptimizer;
mod island;
mod manager;
mod optimizer;
mod sleep;
mod utils;
mod validation;

225
src/dynamics/island_manager/optimizer.rs Normal file
View File

@@ -0,0 +1,225 @@
use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ColliderSet, NarrowPhase};
use std::ops::IndexMut;
use super::{Island, IslandManager};
#[derive(Copy, Clone, Default)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(super) struct IslandsOptimizerMergeState {
curr_awake_id: usize,
}
#[derive(Copy, Clone, Default)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(super) struct IslandsOptimizerSplitState {
curr_awake_id: usize,
curr_body_id: usize,
}
/// Configuration of the awake islands optimization strategy.
///
/// Note that this currently only affects active islands. Sleeping islands are always kept minimal.
#[derive(Copy, Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(super) struct IslandsOptimizer {
/// The optimizer will try to merge small islands so their size exceed this minimum.
///
/// Note that it will never merge incompatible islands (currently define as islands with
/// different additional solver iteration counts).
pub(super) min_island_size: usize,
/// The optimizer will try to split large islands so their size do not exceed this maximum.
///
/// IMPORTANT: Must be greater than `2 * min_island_size` to avoid conflict between the splits
/// and merges.
pub(super) max_island_size: usize,
/// Indicates if the optimizer is in split or merge mode. Swaps between modes every step.
pub(super) mode: usize,
pub(super) merge_state: IslandsOptimizerMergeState,
pub(super) split_state: IslandsOptimizerSplitState,
}
impl Default for IslandsOptimizer {
fn default() -> Self {
// TODO: figure out the best values.
Self {
min_island_size: 1024,
max_island_size: 4096,
mode: 0,
merge_state: Default::default(),
split_state: Default::default(),
}
}
}
impl IslandManager {
pub(super) fn update_optimizer(
&mut self,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
narrow_phase: &NarrowPhase,
) {
if self.optimizer.mode % 2 == 0 {
self.incremental_merge(bodies);
} else {
self.incremental_split(
bodies,
colliders,
impulse_joints,
multibody_joints,
narrow_phase,
);
}
self.optimizer.mode = self.optimizer.mode.wrapping_add(1);
}
/// Attempts to merge awake islands that are too small.
fn incremental_merge(&mut self, bodies: &mut RigidBodySet) {
struct IslandData {
id: usize,
awake_id: usize,
solver_iters: usize,
}
// Ensure the awake id is still in bounds.
if self.optimizer.merge_state.curr_awake_id >= self.awake_islands.len() {
self.optimizer.merge_state.curr_awake_id = 0;
}
// Find a first candidate for a merge.
let mut island1 = None;
for awake_id in self.optimizer.merge_state.curr_awake_id..self.awake_islands.len() {
let id = self.awake_islands[awake_id];
let island = &self.islands[id];
if island.len() < self.optimizer.min_island_size {
island1 = Some(IslandData {
awake_id,
id,
solver_iters: island.additional_solver_iterations,
});
break;
}
}
if let Some(island1) = island1 {
// Indicates if we found a merge candidate for the next incremental update.
let mut found_next = false;
self.optimizer.merge_state.curr_awake_id = island1.awake_id + 1;
// Find a second candidate for a merge.
for awake_id2 in island1.awake_id + 1..self.awake_islands.len() {
let id2 = self.awake_islands[awake_id2];
let island2 = &self.islands[id2];
if island1.solver_iters == island2.additional_solver_iterations
&& island2.len() < self.optimizer.min_island_size
{
// Found a second candidate! Merge them.
self.merge_islands(bodies, island1.id, id2);
// TODO: support doing more than a single merge per frame.
return;
} else if island2.len() < self.optimizer.min_island_size && !found_next {
// We found a good candidate for the next incremental merge (we can’t just
// merge it now because it’s not compatible with the current island).
self.optimizer.merge_state.curr_awake_id = awake_id2;
found_next = true;
}
}
} else {
self.optimizer.merge_state.curr_awake_id = 0;
}
}
/// Attempts to split awake islands that are too big.
fn incremental_split(
&mut self,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
narrow_phase: &NarrowPhase,
) {
if self.optimizer.split_state.curr_awake_id >= self.awake_islands.len() {
self.optimizer.split_state.curr_awake_id = 0;
}
for awake_id in self.optimizer.split_state.curr_awake_id..self.awake_islands.len() {
let id = self.awake_islands[awake_id];
if self.islands[id].len() > self.optimizer.max_island_size {
// Try to split this island.
// Note that the traversal logic is similar to the sleeping island
// extraction, except that we have different stopping criteria.
self.stack.clear();
// TODO: implement islands recycling to avoid reallocating every time.
let mut new_island = Island::default();
self.traversal_timestamp += 1;
for root in &self.islands[id].bodies {
self.stack.push(*root);
let len_before_traversal = new_island.len();
while let Some(handle) = self.stack.pop() {
let rb = bodies.index_mut(handle);
if rb.is_fixed() {
// Don’t propagate islands through rigid-bodies.
continue;
}
if rb.ids.active_set_timestamp == self.traversal_timestamp {
// We already visited this body.
continue;
}
rb.ids.active_set_timestamp = self.traversal_timestamp;
assert!(!rb.activation.sleeping);
// Traverse bodies that are interacting with the current one either through
// contacts or a joint.
super::utils::push_contacting_bodies(
&rb.colliders,
colliders,
narrow_phase,
&mut self.stack,
);
super::utils::push_linked_bodies(
impulse_joints,
multibody_joints,
handle,
&mut self.stack,
);
new_island.bodies.push(handle);
// Our new island cannot grow any further.
if new_island.bodies.len() > self.optimizer.max_island_size {
new_island.bodies.truncate(len_before_traversal);
self.stack.clear();
break;
}
}
// Extract this island.
if new_island.len() == 0 {
// println!("Failed to split island.");
return;
} else if new_island.bodies.len() >= self.optimizer.min_island_size {
// println!(
// "Split an island: {}/{} ({} islands)",
// new_island.len(),
// self.islands[id].len(),
// self.awake_islands.len(),
// );
self.extract_sub_island(bodies, id, new_island, false);
return; // TODO: support extracting more than one island per frame.
}
}
} else {
self.optimizer.split_state.curr_awake_id = awake_id + 1;
}
}
}
}

203
src/dynamics/island_manager/sleep.rs Normal file
View File

@@ -0,0 +1,203 @@
use crate::dynamics::{
ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SleepRootState,
};
use crate::geometry::{ColliderSet, NarrowPhase};
use super::{Island, IslandManager};
impl IslandManager {
/// Wakes up a sleeping body, forcing it back into the active simulation.
///
/// Use this when you want to ensure a body is active (useful after manually moving
/// a sleeping body, or to prevent it from sleeping in the next few frames).
///
/// # Parameters
/// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames.
/// If `false`, it might sleep again immediately if conditions are met.
///
/// # Example
/// ```
/// # use rapier3d::prelude::*;
/// # let mut bodies = RigidBodySet::new();
/// # let mut islands = IslandManager::new();
/// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic());
/// islands.wake_up(&mut bodies, body_handle, true);
/// let body = bodies.get_mut(body_handle).unwrap();
/// // Wake up a body before applying force to it
/// body.add_force(vector![100.0, 0.0, 0.0], false);
/// ```
///
/// Only affects dynamic bodies (kinematic and fixed bodies don't sleep).
pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) {
// NOTE: the use an Option here because there are many legitimate cases (like when
// deleting a joint attached to an already-removed body) where we could be
// attempting to wake-up a rigid-body that has already been deleted.
if bodies.get(handle).map(|rb| !rb.is_fixed()) == Some(true) {
let rb = bodies.index_mut_internal(handle);
// TODO: not sure if this is still relevant:
// // Check that the user didn’t change the sleeping state explicitly, in which
// // case we don’t overwrite it.
// if rb.changes.contains(RigidBodyChanges::SLEEP) {
// return;
// }
rb.activation.wake_up(strong);
let island_to_wake_up = rb.ids.active_island_id;
self.wake_up_island(bodies, island_to_wake_up);
}
}
/// Returns the number of iterations run by the graph traversal so we can balance load across
/// frames.
pub(super) fn extract_sleeping_island(
&mut self,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
narrow_phase: &NarrowPhase,
sleep_root: RigidBodyHandle,
) -> usize {
let Some(rb) = bodies.get_mut_internal(sleep_root) else {
// This branch happens if the rigid-body no longer exists.
return 0;
};
if rb.activation.sleep_root_state != SleepRootState::TraversalPending {
// We already traversed this sleep root.
return 0;
}
rb.activation.sleep_root_state = SleepRootState::Traversed;
let active_island_id = rb.ids.active_island_id;
let active_island = &mut self.islands[active_island_id];
if active_island.is_sleeping() {
// This rigid-body is already part of a sleeping island.
return 0;
}
// TODO: implement recycling islands to avoid repeated allocations?
let mut new_island = Island::default();
self.stack.clear();
self.stack.push(sleep_root);
let mut niter = 0;
self.traversal_timestamp += 1;
while let Some(handle) = self.stack.pop() {
let rb = bodies.index_mut_internal(handle);
if rb.is_fixed() {
// Don’t propagate islands through fixed bodies.
continue;
}
if rb.ids.active_set_timestamp == self.traversal_timestamp {
// We already visited this body and its neighbors.
continue;
}
// if rb.ids.active_set_timestamp >= frame_base_timestamp {
// // We already visited this body and its neighbors during this frame.
// // So we already know this islands cannot sleep (otherwise the bodies
// // currently being traversed would already have been marked as sleeping).
// return niter;
// }
niter += 1;
rb.ids.active_set_timestamp = self.traversal_timestamp;
if rb.activation.is_eligible_for_sleep() {
rb.activation.sleep_root_state = SleepRootState::Traversed;
}
assert_eq!(
rb.ids.active_island_id,
active_island_id,
"handle: {:?}, note niter: {}, isl size: {}",
handle,
niter,
active_island.len()
);
assert!(
!rb.activation.sleeping,
"is sleeping: {:?} note niter: {}, isl size: {}",
handle,
niter,
active_island.len()
);
if !rb.activation.is_eligible_for_sleep() {
// If this body cannot sleep, abort the traversal, we are not traversing
// yet an island that can sleep.
self.stack.clear();
return niter;
}
// Traverse bodies that are interacting with the current one either through
// contacts or a joint.
super::utils::push_contacting_bodies(
&rb.colliders,
colliders,
narrow_phase,
&mut self.stack,
);
super::utils::push_linked_bodies(
impulse_joints,
multibody_joints,
handle,
&mut self.stack,
);
new_island.bodies.push(handle);
}
// If we reached this line, we completed a sleeping island traversal.
// - Put its bodies to sleep.
// - Remove them from the active set.
// - Push the sleeping island.
if active_island.len() == new_island.len() {
// The whole island is asleep. No need to insert a new one.
// Put all its bodies to sleep.
for handle in &active_island.bodies {
let rb = bodies.index_mut_internal(*handle);
rb.sleep();
}
// Mark the existing island as sleeping (by clearing its `id_in_awake_list`)
// and remove it from the awake list.
let island_awake_id = active_island
.id_in_awake_list
.take()
.unwrap_or_else(|| unreachable!());
self.awake_islands.swap_remove(island_awake_id);
if let Some(moved_id) = self.awake_islands.get(island_awake_id) {
self.islands[*moved_id].id_in_awake_list = Some(island_awake_id);
}
} else {
niter += new_island.len(); // Include this part into the cost estimate for this function.
self.extract_sub_island(bodies, active_island_id, new_island, true);
}
niter
}
fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) {
let Some(island) = self.islands.get_mut(island_id) else {
return;
};
if island.is_sleeping() {
island.id_in_awake_list = Some(self.awake_islands.len());
self.awake_islands.push(island_id);
// Wake up all the bodies from this island.
for handle in &island.bodies {
if let Some(rb) = bodies.get_mut(*handle) {
rb.wake_up(false);
}
}
}
}
}

41
src/dynamics/island_manager/utils.rs Normal file
View File

@@ -0,0 +1,41 @@
use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyColliders, RigidBodyHandle};
use crate::geometry::{ColliderSet, NarrowPhase};
// Read all the contacts and push objects touching this rigid-body.
#[inline]
pub(super) fn push_contacting_bodies(
rb_colliders: &RigidBodyColliders,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
stack: &mut Vec<RigidBodyHandle>,
) {
for collider_handle in &rb_colliders.0 {
for inter in narrow_phase.contact_pairs_with(*collider_handle) {
if inter.has_any_active_contact() {
let other = crate::utils::select_other(
(inter.collider1, inter.collider2),
*collider_handle,
);
if let Some(other_body) = colliders[other].parent {
stack.push(other_body.handle);
}
}
}
}
}
pub(super) fn push_linked_bodies(
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
handle: RigidBodyHandle,
stack: &mut Vec<RigidBodyHandle>,
) {
for inter in impulse_joints.attached_enabled_joints(handle) {
let other = crate::utils::select_other((inter.0, inter.1), handle);
stack.push(other);
}
for other in multibody_joints.bodies_attached_with_enabled_joint(handle) {
stack.push(other);
}
}

71
src/dynamics/island_manager/validation.rs Normal file
View File

@@ -0,0 +1,71 @@
use crate::dynamics::{IslandManager, RigidBodySet};
use crate::geometry::NarrowPhase;
use crate::prelude::ColliderSet;
impl IslandManager {
#[allow(dead_code)]
pub(super) fn assert_state_is_valid(
&self,
bodies: &RigidBodySet,
colliders: &ColliderSet,
nf: &NarrowPhase,
) {
for (island_id, island) in self.islands.iter() {
// Sleeping island must not be in the awake list.
if island.is_sleeping() {
assert!(!self.awake_islands.contains(&island_id));
} else {
// If the island is awake, the awake id must match.
let awake_id = island.id_in_awake_list.unwrap();
assert_eq!(self.awake_islands[awake_id], island_id);
}
for (body_id, handle) in island.bodies.iter().enumerate() {
if let Some(rb) = bodies.get(*handle) {
// The body’s sleeping status must match the island’s status.
assert_eq!(rb.is_sleeping(), island.is_sleeping());
// The body’s island id must match the island id.
assert_eq!(rb.ids.active_island_id, island_id);
// The body’s active set id must match its handle’s position in island.bodies.
assert_eq!(body_id, rb.ids.active_set_id);
}
}
}
// Free island ids must actually be free.
for id in self.free_islands.iter() {
assert!(self.islands.get(*id).is_none());
}
// The awake islands list must not have duplicates.
let mut awake_islands_dedup = self.awake_islands.clone();
awake_islands_dedup.sort();
awake_islands_dedup.dedup();
assert_eq!(self.awake_islands.len(), awake_islands_dedup.len());
// If two bodies have solver contacts, they must be in the same island.
for pair in nf.contact_pairs() {
let Some(body_handle1) = colliders[pair.collider1].parent.map(|p| p.handle) else {
continue;
};
let Some(body_handle2) = colliders[pair.collider2].parent.map(|p| p.handle) else {
continue;
};
let body1 = &bodies[body_handle1];
let body2 = &bodies[body_handle2];
if body1.is_fixed() || body2.is_fixed() {
continue;
}
if pair.has_any_active_contact() {
assert_eq!(body1.ids.active_island_id, body2.ids.active_island_id);
}
}
println!(
"`IslandManager::assert_state_is_valid` validation checks passed. This is slow. Only enable for debugging."
);
}
}

19
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -73,6 +73,8 @@ pub struct ImpulseJointSet {
joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
/// A set of rigid-body handles to wake-up during the next timestep.
pub(crate) to_wake_up: HashSet<RigidBodyHandle>,
/// A set of rigid-body pairs to join in the island manager during the next timestep.
pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>,
}
impl ImpulseJointSet {
@@ -83,6 +85,7 @@ impl ImpulseJointSet {
joint_ids: Arena::new(),
joint_graph: InteractionGraph::new(),
to_wake_up: HashSet::default(),
to_join: HashSet::default(),
}
}
@@ -366,6 +369,8 @@ impl ImpulseJointSet {
self.to_wake_up.insert(body2);
}
self.to_join.insert((body1, body2));
ImpulseJointHandle(handle)
}
@@ -377,7 +382,7 @@ impl ImpulseJointSet {
bodies: &RigidBodySet,
out: &mut [Vec<JointIndex>],
) {
for out_island in &mut out[..islands.num_islands()] {
for out_island in &mut out[..islands.active_islands().len()] {
out_island.clear();
}
@@ -392,13 +397,17 @@ impl ImpulseJointSet {
&& (!rb1.is_dynamic_or_kinematic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic_or_kinematic() || !rb2.is_sleeping())
{
let island_index = if !rb1.is_dynamic_or_kinematic() {
rb2.ids.active_island_id
let island_awake_index = if !rb1.is_dynamic_or_kinematic() {
islands.islands[rb2.ids.active_island_id]
.id_in_awake_list()
.expect("Internal error: island should be awake.")
} else {
rb1.ids.active_island_id
islands.islands[rb1.ids.active_island_id]
.id_in_awake_list()
.expect("Internal error: island should be awake.")
};
out[island_index].push(i);
out[island_awake_index].push(i);
}
}
}

5
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -95,6 +95,8 @@ pub struct MultibodyJointSet {
// that any more in the future when we improve our island builder.
pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
pub(crate) to_wake_up: HashSet<RigidBodyHandle>,
/// A set of rigid-body pairs to join in the island manager during the next timestep.
pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>,
}
impl MultibodyJointSet {
@@ -105,6 +107,7 @@ impl MultibodyJointSet {
rb2mb: Coarena::new(),
connectivity_graph: InteractionGraph::new(),
to_wake_up: HashSet::default(),
to_join: HashSet::default(),
}
}
@@ -205,6 +208,8 @@ impl MultibodyJointSet {
self.to_wake_up.insert(body2);
}
self.to_join.insert((body1, body2));
// Because each rigid-body can only have one parent link,
// we can use the second rigid-body’s handle as the multibody_joint’s
// handle.

2
src/dynamics/rigid_body.rs
View File

@@ -665,7 +665,7 @@ impl RigidBody {
// to all the fixed bodies active set offsets?
pub fn effective_active_set_offset(&self) -> u32 {
if self.is_dynamic_or_kinematic() {
self.ids.active_set_offset
self.ids.active_set_id as u32
} else {
u32::MAX
}

60
src/dynamics/rigid_body_components.rs
View File

@@ -1029,7 +1029,6 @@ impl RigidBodyCcd {
pub struct RigidBodyIds {
pub(crate) active_island_id: usize,
pub(crate) active_set_id: usize,
pub(crate) active_set_offset: u32,
pub(crate) active_set_timestamp: u32,
}
@@ -1038,7 +1037,6 @@ impl Default for RigidBodyIds {
Self {
active_island_id: usize::MAX,
active_set_id: usize::MAX,
active_set_offset: u32::MAX,
active_set_timestamp: 0,
}
}
@@ -1138,6 +1136,19 @@ impl RigidBodyDominance {
}
}
#[derive(Copy, Clone, PartialEq, Eq, Debug, Default)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(crate) enum SleepRootState {
/// This sleep root has already been traversed. No need to traverse
/// again until the rigid-body either gets awaken by an event.
Traversed,
/// This sleep root has not been traversed yet.
TraversalPending,
/// This body can become a sleep root once it falls asleep.
#[default]
Unknown,
}
/// Controls when a body goes to sleep (becomes inactive to save CPU).
///
/// ## Sleeping System
@@ -1183,6 +1194,8 @@ pub struct RigidBodyActivation {
/// Is this body currently sleeping?
pub sleeping: bool,
pub(crate) sleep_root_state: SleepRootState,
}
impl Default for RigidBodyActivation {
@@ -1216,6 +1229,7 @@ impl RigidBodyActivation {
time_until_sleep: Self::default_time_until_sleep(),
time_since_can_sleep: 0.0,
sleeping: false,
sleep_root_state: SleepRootState::Unknown,
}
}
@@ -1227,6 +1241,7 @@ impl RigidBodyActivation {
time_until_sleep: Self::default_time_until_sleep(),
time_since_can_sleep: Self::default_time_until_sleep(),
sleeping: true,
sleep_root_state: SleepRootState::Unknown,
}
}
@@ -1249,6 +1264,12 @@ impl RigidBodyActivation {
#[inline]
pub fn wake_up(&mut self, strong: bool) {
self.sleeping = false;
// Make this body eligible as a sleep root again.
if self.sleep_root_state != SleepRootState::TraversalPending {
self.sleep_root_state = SleepRootState::Unknown;
}
if strong {
self.time_since_can_sleep = 0.0;
}
@@ -1260,6 +1281,41 @@ impl RigidBodyActivation {
self.sleeping = true;
self.time_since_can_sleep = self.time_until_sleep;
}
/// Does this body have a sufficiently low kinetic energy for a long enough
/// duration to be eligible for sleeping?
pub fn is_eligible_for_sleep(&self) -> bool {
self.time_since_can_sleep >= self.time_until_sleep
}
pub(crate) fn update_energy(
&mut self,
body_type: RigidBodyType,
length_unit: Real,
sq_linvel: Real,
sq_angvel: Real,
dt: Real,
) {
let can_sleep = match body_type {
RigidBodyType::Dynamic => {
let linear_threshold = self.normalized_linear_threshold * length_unit;
sq_linvel < linear_threshold * linear_threshold.abs()
&& sq_angvel < self.angular_threshold * self.angular_threshold.abs()
}
RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => {
// Platforms only sleep if both velocities are exactly zero. If it’s not exactly
// zero, then the user really wants them to move.
sq_linvel == 0.0 && sq_angvel == 0.0
}
RigidBodyType::Fixed => true,
};
if can_sleep {
self.time_since_can_sleep += dt;
} else {
self.time_since_can_sleep = 0.0;
}
}
}
#[cfg(test)]

101
src/dynamics/solver/contact_constraint/contact_constraints_set.rs
View File

@@ -11,10 +11,9 @@ use crate::dynamics::{
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Real;
use crate::math::SIMD_WIDTH;
use crate::math::{MAX_MANIFOLD_POINTS, Real};
use na::DVector;
use parry::math::DIM;
use crate::dynamics::solver::contact_constraint::any_contact_constraint::AnyContactConstraintMut;
#[cfg(feature = "dim3")]
@@ -31,14 +30,8 @@ pub struct ConstraintsCounts {
}
impl ConstraintsCounts {
pub fn from_contacts(manifold: &ContactManifold) -> Self {
let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
Self {
num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS
+ rest as usize,
num_jacobian_lines: manifold.data.solver_contacts.len() * DIM,
}
}
// NOTE: constraints count from contacts is always 1 since the max number of solver contacts
// matches the max number of contact per constraint.
pub fn from_joint(joint: &ImpulseJoint) -> Self {
let joint = &joint.data;
@@ -334,18 +327,8 @@ impl ContactConstraintsSet {
solver_bodies: &SolverBodies,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints)
.sum::<usize>()
+ self
.interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum::<usize>();
let total_num_constraints = (self.interaction_groups.simd_interactions.len() / SIMD_WIDTH)
+ self.interaction_groups.nongrouped_interactions.len();
unsafe {
reset_buffer(
@@ -358,15 +341,14 @@ impl ContactConstraintsSet {
);
}
let mut curr_start = 0;
// TODO PERF: could avoid this index using zip.
let mut curr_id = 0;
for manifolds_i in self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
{
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = array![|ii| manifolds_i[ii]];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]];
@@ -375,16 +357,14 @@ impl ContactConstraintsSet {
manifolds,
bodies,
solver_bodies,
&mut self.simd_velocity_twist_constraints_builder[curr_start..],
&mut self.simd_velocity_twist_constraints[curr_start..],
&mut self.simd_velocity_twist_constraints_builder[curr_id],
&mut self.simd_velocity_twist_constraints[curr_id],
);
curr_start += num_to_add;
curr_id += 1;
}
for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() {
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints;
let mut manifold_id = [usize::MAX; SIMD_WIDTH];
manifold_id[0] = *manifolds_i;
let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH];
@@ -394,14 +374,14 @@ impl ContactConstraintsSet {
manifolds,
bodies,
solver_bodies,
&mut self.simd_velocity_twist_constraints_builder[curr_start..],
&mut self.simd_velocity_twist_constraints[curr_start..],
&mut self.simd_velocity_twist_constraints_builder[curr_id],
&mut self.simd_velocity_twist_constraints[curr_id],
);
curr_start += num_to_add;
curr_id += 1;
}
assert_eq!(curr_start, total_num_constraints);
assert_eq!(curr_id, total_num_constraints);
}
fn simd_compute_coulomb_constraints(
@@ -410,18 +390,8 @@ impl ContactConstraintsSet {
solver_bodies: &SolverBodies,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints)
.sum::<usize>()
+ self
.interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum::<usize>();
let total_num_constraints = self.interaction_groups.simd_interactions.len() / SIMD_WIDTH
+ self.interaction_groups.nongrouped_interactions.len();
unsafe {
reset_buffer(
@@ -434,15 +404,14 @@ impl ContactConstraintsSet {
);
}
let mut curr_start = 0;
// TODO PERF: could avoid this index using zip.
let mut curr_id = 0;
for manifolds_i in self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
{
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = array![|ii| manifolds_i[ii]];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]];
@@ -451,16 +420,14 @@ impl ContactConstraintsSet {
manifolds,
bodies,
solver_bodies,
&mut self.simd_velocity_coulomb_constraints_builder[curr_start..],
&mut self.simd_velocity_coulomb_constraints[curr_start..],
&mut self.simd_velocity_coulomb_constraints_builder[curr_id],
&mut self.simd_velocity_coulomb_constraints[curr_id],
);
curr_start += num_to_add;
curr_id += 1;
}
for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() {
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints;
let mut manifold_id = [usize::MAX; SIMD_WIDTH];
manifold_id[0] = *manifolds_i;
let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH];
@@ -470,14 +437,14 @@ impl ContactConstraintsSet {
manifolds,
bodies,
solver_bodies,
&mut self.simd_velocity_coulomb_constraints_builder[curr_start..],
&mut self.simd_velocity_coulomb_constraints[curr_start..],
&mut self.simd_velocity_coulomb_constraints_builder[curr_id],
&mut self.simd_velocity_coulomb_constraints[curr_id],
);
curr_start += num_to_add;
curr_id += 1;
}
assert_eq!(curr_start, total_num_constraints);
assert_eq!(curr_id, total_num_constraints);
}
fn compute_generic_constraints(
@@ -487,11 +454,7 @@ impl ContactConstraintsSet {
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) {
let total_num_constraints = self
.generic_two_body_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum::<usize>();
let total_num_constraints = self.generic_two_body_interactions.len();
self.generic_velocity_constraints_builder.resize(
total_num_constraints,
@@ -500,27 +463,27 @@ impl ContactConstraintsSet {
self.generic_velocity_constraints
.resize(total_num_constraints, GenericContactConstraint::invalid());
let mut curr_start = 0;
// TODO PERF: could avoid this index using zip.
let mut curr_id = 0;
for manifold_i in &self.generic_two_body_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
GenericContactConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_constraints_builder[curr_start..],
&mut self.generic_velocity_constraints[curr_start..],
&mut self.generic_velocity_constraints_builder[curr_id],
&mut self.generic_velocity_constraints[curr_id],
&mut self.generic_jacobians,
jacobian_id,
);
curr_start += num_to_add;
curr_id += 1;
}
assert_eq!(curr_start, total_num_constraints);
assert_eq!(curr_id, total_num_constraints);
}
pub fn warmstart(

102
src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs
View File

@@ -43,8 +43,8 @@ impl ContactWithCoulombFrictionBuilder {
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
out_builders: &mut [ContactWithCoulombFrictionBuilder],
out_constraints: &mut [ContactWithCoulombFriction],
out_builder: &mut ContactWithCoulombFrictionBuilder,
out_constraint: &mut ContactWithCoulombFriction,
) {
// TODO: could we avoid having to fetch the ids here? It’s the only thing we
// read from the original rigid-bodies.
@@ -52,7 +52,7 @@ impl ContactWithCoulombFrictionBuilder {
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
bodies[handle].ids.active_set_id as u32
} else {
u32::MAX
}];
@@ -60,7 +60,7 @@ impl ContactWithCoulombFrictionBuilder {
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
bodies[handle].ids.active_set_id as u32
} else {
u32::MAX
}];
@@ -83,24 +83,20 @@ impl ContactWithCoulombFrictionBuilder {
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points =
array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
array![|ii| &manifolds[ii].data.solver_contacts[..num_active_contacts]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
constraint.dir1 = force_dir1;
constraint.im1 = poses1.im;
constraint.im2 = poses2.im;
constraint.solver_vel1 = ids1;
constraint.solver_vel2 = ids2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
out_constraint.dir1 = force_dir1;
out_constraint.im1 = poses1.im;
out_constraint.im2 = poses2.im;
out_constraint.solver_vel1 = ids1;
out_constraint.solver_vel2 = ids2;
out_constraint.manifold_id = manifold_id;
out_constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
out_constraint.tangent1 = tangents1[0];
}
for k in 0..num_points {
@@ -117,8 +113,8 @@ impl ContactWithCoulombFrictionBuilder {
let vel1 = vels1.linear + vels1.angular.gcross(dp1);
let vel2 = vels2.linear + vels2.angular.gcross(dp2);
constraint.limit = solver_contact.friction;
constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
out_constraint.limit = solver_contact.friction;
out_constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
// Normal part.
let normal_rhs_wo_bias;
@@ -136,19 +132,18 @@ impl ContactWithCoulombFrictionBuilder {
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
normal_rhs_wo_bias =
is_bouncy * solver_contact.restitution * projected_velocity;
normal_rhs_wo_bias = is_bouncy * solver_contact.restitution * projected_velocity;
constraint.normal_part[k].torque_dir1 = torque_dir1;
constraint.normal_part[k].torque_dir2 = torque_dir2;
constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
constraint.normal_part[k].r = projected_mass;
out_constraint.normal_part[k].torque_dir1 = torque_dir1;
out_constraint.normal_part[k].torque_dir2 = torque_dir2;
out_constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
out_constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
out_constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
out_constraint.normal_part[k].r = projected_mass;
}
// tangent parts.
constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse;
out_constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -163,13 +158,13 @@ impl ContactWithCoulombFrictionBuilder {
+ ii_torque_dir2.gdot(torque_dir2);
let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]);
constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") {
out_constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
out_constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
out_constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
out_constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
out_constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
out_constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
out_constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
r
@@ -180,21 +175,21 @@ impl ContactWithCoulombFrictionBuilder {
{
// TODO PERF: we already applied the inverse inertia to the torque
// dire before. Could we reuse the value instead of retransforming?
constraint.tangent_part[k].r[2] = SimdReal::splat(2.0)
* (constraint.tangent_part[k].ii_torque_dir1[0]
.gdot(constraint.tangent_part[k].torque_dir1[1])
+ constraint.tangent_part[k].ii_torque_dir2[0]
.gdot(constraint.tangent_part[k].torque_dir2[1]));
out_constraint.tangent_part[k].r[2] = SimdReal::splat(2.0)
* (out_constraint.tangent_part[k].ii_torque_dir1[0]
.gdot(out_constraint.tangent_part[k].torque_dir1[1])
+ out_constraint.tangent_part[k].ii_torque_dir2[0]
.gdot(out_constraint.tangent_part[k].torque_dir2[1]));
}
// Builder.
builder.infos[k].local_p1 =
out_builder.infos[k].local_p1 =
poses1.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].local_p2 =
out_builder.infos[k].local_p2 =
poses2.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].tangent_vel = solver_contact.tangent_velocity;
builder.infos[k].dist = solver_contact.dist;
builder.infos[k].normal_vel = normal_rhs_wo_bias;
out_builder.infos[k].tangent_vel = solver_contact.tangent_velocity;
out_builder.infos[k].dist = solver_contact.dist;
out_builder.infos[k].normal_vel = normal_rhs_wo_bias;
}
if BLOCK_SOLVER_ENABLED {
@@ -204,20 +199,20 @@ impl ContactWithCoulombFrictionBuilder {
let k1 = k * 2 + 1;
let imsum = poses1.im + poses2.im;
let r0 = constraint.normal_part[k0].r;
let r1 = constraint.normal_part[k1].r;
let r0 = out_constraint.normal_part[k0].r;
let r1 = out_constraint.normal_part[k1].r;
let mut r_mat = SdpMatrix2::zero();
// TODO PERF: we already applied the inverse inertia to the torque
// dire before. Could we reuse the value instead of retransforming?
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+ constraint.normal_part[k0]
+ out_constraint.normal_part[k0]
.ii_torque_dir1
.gdot(constraint.normal_part[k1].torque_dir1)
+ constraint.normal_part[k0]
.gdot(out_constraint.normal_part[k1].torque_dir1)
+ out_constraint.normal_part[k0]
.ii_torque_dir2
.gdot(constraint.normal_part[k1].torque_dir2);
.gdot(out_constraint.normal_part[k1].torque_dir2);
r_mat.m11 = utils::simd_inv(r0);
r_mat.m22 = utils::simd_inv(r1);
@@ -233,18 +228,17 @@ impl ContactWithCoulombFrictionBuilder {
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.normal_part[k0].r_mat_elts = [
out_constraint.normal_part[k0].r_mat_elts = [
inv.m11.select(is_invertible, r0),
inv.m22.select(is_invertible, SimdReal::zero()),
];
constraint.normal_part[k1].r_mat_elts = [
out_constraint.normal_part[k1].r_mat_elts = [
inv.m12.select(is_invertible, SimdReal::zero()),
r_mat.m12.select(is_invertible, SimdReal::zero()),
];
}
}
}
}
pub fn update(
&self,

103
src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs
View File

@@ -50,8 +50,8 @@ impl ContactWithTwistFrictionBuilder {
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
out_builders: &mut [ContactWithTwistFrictionBuilder],
out_constraints: &mut [ContactWithTwistFriction],
out_builder: &mut ContactWithTwistFrictionBuilder,
out_constraint: &mut ContactWithTwistFriction,
) {
// TODO: could we avoid having to fetch the ids here? It’s the only thing we
// read from the original rigid-bodies.
@@ -59,7 +59,7 @@ impl ContactWithTwistFrictionBuilder {
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
bodies[handle].ids.active_set_id as u32
} else {
u32::MAX
}];
@@ -67,7 +67,7 @@ impl ContactWithTwistFrictionBuilder {
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
bodies[handle].ids.active_set_id as u32
} else {
u32::MAX
}];
@@ -90,26 +90,22 @@ impl ContactWithTwistFrictionBuilder {
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points =
array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
array![|ii| &manifolds[ii].data.solver_contacts[..num_active_contacts]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let inv_num_points = SimdReal::splat(1.0 / num_points as Real);
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
constraint.dir1 = force_dir1;
constraint.im1 = poses1.im;
constraint.im2 = poses2.im;
constraint.solver_vel1 = ids1;
constraint.solver_vel2 = ids2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
out_constraint.dir1 = force_dir1;
out_constraint.im1 = poses1.im;
out_constraint.im2 = poses2.im;
out_constraint.solver_vel1 = ids1;
out_constraint.solver_vel2 = ids2;
out_constraint.manifold_id = manifold_id;
out_constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
out_constraint.tangent1 = tangents1[0];
}
let mut friction_center = Point::origin();
@@ -137,8 +133,8 @@ impl ContactWithTwistFrictionBuilder {
tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points;
tangent_vel += solver_contact.tangent_velocity * inv_num_points;
constraint.limit = solver_contact.friction;
constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
out_constraint.limit = solver_contact.friction;
out_constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
// Normal part.
let normal_rhs_wo_bias;
@@ -156,34 +152,33 @@ impl ContactWithTwistFrictionBuilder {
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
normal_rhs_wo_bias =
is_bouncy * solver_contact.restitution * projected_velocity;
normal_rhs_wo_bias = is_bouncy * solver_contact.restitution * projected_velocity;
constraint.normal_part[k].torque_dir1 = torque_dir1;
constraint.normal_part[k].torque_dir2 = torque_dir2;
constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
constraint.normal_part[k].r = projected_mass;
out_constraint.normal_part[k].torque_dir1 = torque_dir1;
out_constraint.normal_part[k].torque_dir2 = torque_dir2;
out_constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
out_constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
out_constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
out_constraint.normal_part[k].r = projected_mass;
}
// Builder.
builder.infos[k].local_p1 =
out_builder.infos[k].local_p1 =
poses1.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].local_p2 =
out_builder.infos[k].local_p2 =
poses2.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].dist = solver_contact.dist;
builder.infos[k].normal_vel = normal_rhs_wo_bias;
out_builder.infos[k].dist = solver_contact.dist;
out_builder.infos[k].normal_vel = normal_rhs_wo_bias;
}
/*
* Tangent/twist part
*/
constraint.tangent_part.impulse = tangent_warmstart;
constraint.twist_part.impulse = twist_warmstart;
out_constraint.tangent_part.impulse = tangent_warmstart;
out_constraint.twist_part.impulse = twist_warmstart;
builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center);
builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center);
out_builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center);
out_builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center);
let dp1 = friction_center - world_com1;
let dp2 = friction_center - world_com2;
@@ -200,13 +195,12 @@ impl ContactWithTwistFrictionBuilder {
let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1);
let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1);
constraint.twist_part.rhs = SimdReal::zero();
constraint.twist_part.ii_twist_dir1 = ii_twist_dir1;
constraint.twist_part.ii_twist_dir2 = ii_twist_dir2;
constraint.twist_part.r = utils::simd_inv(
ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1),
);
constraint.twist_dists = twist_dists;
out_constraint.twist_part.rhs = SimdReal::zero();
out_constraint.twist_part.ii_twist_dir1 = ii_twist_dir1;
out_constraint.twist_part.ii_twist_dir2 = ii_twist_dir2;
out_constraint.twist_part.r =
utils::simd_inv(ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1));
out_constraint.twist_dists = twist_dists;
}
// Tangent part.
@@ -227,13 +221,13 @@ impl ContactWithTwistFrictionBuilder {
// have the same tangent vel?
let rhs_wo_bias = tangent_vel.dot(&tangents1[j]);
constraint.tangent_part.torque_dir1[j] = torque_dir1;
constraint.tangent_part.torque_dir2[j] = torque_dir2;
constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1;
constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2;
constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.tangent_part.rhs[j] = rhs_wo_bias;
constraint.tangent_part.r[j] = if cfg!(feature = "dim2") {
out_constraint.tangent_part.torque_dir1[j] = torque_dir1;
out_constraint.tangent_part.torque_dir2[j] = torque_dir2;
out_constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1;
out_constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2;
out_constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
out_constraint.tangent_part.rhs[j] = rhs_wo_bias;
out_constraint.tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
r
@@ -244,12 +238,11 @@ impl ContactWithTwistFrictionBuilder {
{
// TODO PERF: we already applied the inverse inertia to the torque
// dire before. Could we reuse the value instead of retransforming?
constraint.tangent_part.r[2] = SimdReal::splat(2.0)
* (constraint.tangent_part.ii_torque_dir1[0]
.gdot(constraint.tangent_part.torque_dir1[1])
+ constraint.tangent_part.ii_torque_dir2[0]
.gdot(constraint.tangent_part.torque_dir2[1]));
}
out_constraint.tangent_part.r[2] = SimdReal::splat(2.0)
* (out_constraint.tangent_part.ii_torque_dir1[0]
.gdot(out_constraint.tangent_part.torque_dir1[1])
+ out_constraint.tangent_part.ii_torque_dir2[0]
.gdot(out_constraint.tangent_part.torque_dir2[1]));
}
}

74
src/dynamics/solver/contact_constraint/generic_contact_constraint.rs
View File

@@ -36,8 +36,8 @@ impl GenericContactConstraintBuilder {
manifold: &ContactManifold,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builders: &mut [GenericContactConstraintBuilder],
out_constraints: &mut [GenericContactConstraint],
out_builder: &mut GenericContactConstraintBuilder,
out_constraint: &mut GenericContactConstraint,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
) {
@@ -68,7 +68,7 @@ impl GenericContactConstraintBuilder {
multibody1
.map(|mb| mb.0.solver_id)
.unwrap_or(if type1.is_dynamic_or_kinematic() {
rb1.ids.active_set_offset
rb1.ids.active_set_id as u32
} else {
u32::MAX
});
@@ -76,7 +76,7 @@ impl GenericContactConstraintBuilder {
multibody2
.map(|mb| mb.0.solver_id)
.unwrap_or(if type2.is_dynamic_or_kinematic() {
rb2.ids.active_set_offset
rb2.ids.active_set_id as u32
} else {
u32::MAX
});
@@ -99,34 +99,27 @@ impl GenericContactConstraintBuilder {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let chunk_j_id = *jacobian_id;
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
constraint.dir1 = force_dir1;
constraint.im1 = if type1.is_dynamic_or_kinematic() {
let manifold_points = &manifold.data.solver_contacts;
out_constraint.dir1 = force_dir1;
out_constraint.im1 = if type1.is_dynamic_or_kinematic() {
mprops1.effective_inv_mass
} else {
na::zero()
};
constraint.im2 = if type2.is_dynamic_or_kinematic() {
out_constraint.im2 = if type2.is_dynamic_or_kinematic() {
mprops2.effective_inv_mass
} else {
na::zero()
};
constraint.solver_vel1 = solver_vel1;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = manifold_points.len() as u8;
out_constraint.solver_vel1 = solver_vel1;
out_constraint.solver_vel2 = solver_vel2;
out_constraint.manifold_id = manifold_id;
out_constraint.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
out_constraint.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
@@ -138,8 +131,8 @@ impl GenericContactConstraintBuilder {
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8;
out_constraint.limit = manifold_point.friction;
out_constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8;
// Normal part.
let normal_rhs_wo_bias;
@@ -207,7 +200,7 @@ impl GenericContactConstraintBuilder {
normal_rhs_wo_bias =
(is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
constraint.normal_part[k] = ContactConstraintNormalPart {
out_constraint.normal_part[k] = ContactConstraintNormalPart {
torque_dir1,
torque_dir2,
ii_torque_dir1,
@@ -223,7 +216,7 @@ impl GenericContactConstraintBuilder {
// Tangent parts.
{
constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse;
out_constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -234,8 +227,8 @@ impl GenericContactConstraintBuilder {
} else {
na::zero()
};
constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
out_constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
out_constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
let torque_dir2 = dp2.gcross(-tangents1[j]);
let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() {
@@ -245,8 +238,8 @@ impl GenericContactConstraintBuilder {
} else {
na::zero()
};
constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
out_constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
out_constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
mb1.fill_jacobians(
@@ -289,13 +282,13 @@ impl GenericContactConstraintBuilder {
let r = crate::utils::inv(inv_r1 + inv_r2);
let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
out_constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
out_constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
// TODO: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.tangent_part[k].r[j] = r;
out_constraint.tangent_part[k].r[j] = r;
}
}
@@ -314,11 +307,11 @@ impl GenericContactConstraintBuilder {
normal_vel: normal_rhs_wo_bias,
};
builder.handle1 = handle1;
builder.handle2 = handle2;
builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
builder.infos[k] = infos;
constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8;
out_builder.handle1 = handle1;
out_builder.handle2 = handle2;
out_builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
out_builder.infos[k] = infos;
out_constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8;
}
let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0);
@@ -331,11 +324,10 @@ impl GenericContactConstraintBuilder {
| (!type1.is_dynamic_or_kinematic() as u8)
| ((!type2.is_dynamic_or_kinematic() as u8) << 1);
constraint.j_id = chunk_j_id;
constraint.ndofs1 = ndofs1;
constraint.ndofs2 = ndofs2;
constraint.generic_constraint_mask = generic_constraint_mask;
}
out_constraint.j_id = chunk_j_id;
out_constraint.ndofs1 = ndofs1;
out_constraint.ndofs2 = ndofs2;
out_constraint.generic_constraint_mask = generic_constraint_mask;
}
pub fn update(

52
src/dynamics/solver/interaction_groups.rs
View File

@@ -72,7 +72,7 @@ impl ParallelInteractionGroups {
interactions: &[Interaction],
interaction_indices: &[usize],
) {
let num_island_bodies = islands.active_island(island_id).len();
let num_island_bodies = islands.island(island_id).len();
self.bodies_color.clear();
self.interaction_indices.clear();
self.groups.clear();
@@ -117,26 +117,26 @@ impl ParallelInteractionGroups {
(false, false) => {
let rb1 = &bodies[body_pair.0.unwrap()];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb1.ids.active_set_offset as usize]
| bcolors[rb2.ids.active_set_offset as usize];
let color_mask =
bcolors[rb1.ids.active_set_id] | bcolors[rb2.ids.active_set_id];
*color = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1;
bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color;
bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color;
bcolors[rb1.ids.active_set_id] |= 1 << *color;
bcolors[rb2.ids.active_set_id] |= 1 << *color;
}
(true, false) => {
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb2.ids.active_set_offset as usize];
let color_mask = bcolors[rb2.ids.active_set_id];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color;
bcolors[rb2.ids.active_set_id] |= 1 << *color;
}
(false, true) => {
let rb1 = &bodies[body_pair.0.unwrap()];
let color_mask = bcolors[rb1.ids.active_set_offset as usize];
let color_mask = bcolors[rb1.ids.active_set_id];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color;
bcolors[rb1.ids.active_set_id] |= 1 << *color;
}
(true, true) => unreachable!(),
}
@@ -244,7 +244,7 @@ impl InteractionGroups {
// is full, we don't clear the corresponding body mask bit. This may result
// in less grouped constraints.
self.body_masks
.resize(islands.active_island(island_id).len(), 0u128);
.resize(islands.island(island_id).len(), 0u128);
// NOTE: each bit of the occupied mask indicates what bucket already
// contains at least one constraint.
@@ -270,18 +270,10 @@ impl InteractionGroups {
}
let ijoint = interaction.data.locked_axes.bits() as usize;
let i1 = rb1.ids.active_set_offset;
let i2 = rb2.ids.active_set_offset;
let conflicts = self
.body_masks
.get(i1 as usize)
.copied()
.unwrap_or_default()
| self
.body_masks
.get(i2 as usize)
.copied()
.unwrap_or_default()
let i1 = rb1.ids.active_set_id;
let i2 = rb2.ids.active_set_id;
let conflicts = self.body_masks.get(i1).copied().unwrap_or_default()
| self.body_masks.get(i2).copied().unwrap_or_default()
| joint_type_conflicts[ijoint];
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
@@ -396,7 +388,7 @@ impl InteractionGroups {
// in less grouped contacts.
// NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
self.body_masks
.resize(islands.active_island(island_id).len(), 0u128);
.resize(islands.island(island_id).len(), 0u128);
// NOTE: each bit of the occupied mask indicates what bucket already
// contains at least one constraint.
@@ -420,17 +412,15 @@ impl InteractionGroups {
continue;
}
let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
{
let (status1, active_set_id1) = if let Some(rb1) = interaction.data.rigid_body1 {
let rb1 = &bodies[rb1];
(rb1.body_type, rb1.ids.active_set_offset)
(rb1.body_type, rb1.ids.active_set_id as u32)
} else {
(RigidBodyType::Fixed, u32::MAX)
};
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{
let (status2, active_set_id2) = if let Some(rb2) = interaction.data.rigid_body2 {
let rb2 = &bodies[rb2];
(rb2.body_type, rb2.ids.active_set_offset)
(rb2.body_type, rb2.ids.active_set_id as u32)
} else {
(RigidBodyType::Fixed, u32::MAX)
};
@@ -443,8 +433,8 @@ impl InteractionGroups {
continue;
}
let i1 = active_set_offset1;
let i2 = active_set_offset2;
let i1 = active_set_id1;
let i2 = active_set_id2;
let mask1 = if !is_fixed1 {
self.body_masks[i1 as usize]
} else {

24
src/dynamics/solver/island_solver.rs
View File

@@ -43,8 +43,12 @@ impl IslandSolver {
multibodies: &mut MultibodyJointSet,
) {
counters.solver.velocity_assembly_time.resume();
counters
.solver
.velocity_assembly_time_solver_bodies
.resume();
let num_solver_iterations = base_params.num_solver_iterations
+ islands.active_island_additional_solver_iterations(island_id);
+ islands.island(island_id).additional_solver_iterations();
let mut params = *base_params;
params.dt /= num_solver_iterations as Real;
@@ -55,7 +59,6 @@ impl IslandSolver {
*
*/
// INIT
// let t0 = std::time::Instant::now();
self.velocity_solver
.init_solver_velocities_and_solver_bodies(
base_params.dt,
@@ -65,8 +68,11 @@ impl IslandSolver {
bodies,
multibodies,
);
// let t_solver_body_init = t0.elapsed().as_secs_f32();
// let t0 = std::time::Instant::now();
counters.solver.velocity_assembly_time_solver_bodies.pause();
counters
.solver
.velocity_assembly_time_constraints_init
.resume();
self.velocity_solver.init_constraints(
island_id,
islands,
@@ -81,13 +87,11 @@ impl IslandSolver {
#[cfg(feature = "dim3")]
params.friction_model,
);
// let t_init_constraints = t0.elapsed().as_secs_f32();
counters
.solver
.velocity_assembly_time_constraints_init
.pause();
counters.solver.velocity_assembly_time.pause();
// println!(
// "Solver body init: {}, init constraints: {}",
// t_solver_body_init * 1000.0,
// t_init_constraints * 1000.0
// );
// SOLVE
counters.solver.velocity_resolution_time.resume();

4
src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

@@ -123,12 +123,12 @@ impl JointConstraintBuilderSimd {
let rb2 = array![|ii| &bodies[joint[ii].body2]];
let body1 = array![|ii| if rb1[ii].is_dynamic_or_kinematic() {
rb1[ii].ids.active_set_offset
rb1[ii].ids.active_set_id as u32
} else {
u32::MAX
}];
let body2 = array![|ii| if rb2[ii].is_dynamic_or_kinematic() {
rb2[ii].ids.active_set_offset
rb2[ii].ids.active_set_id as u32
} else {
u32::MAX
}];

5
src/dynamics/solver/joint_constraint/joint_constraints_set.rs
View File

@@ -183,6 +183,7 @@ impl JointConstraintsSet {
fn compute_generic_joint_constraints(
&mut self,
// TODO: pass around the &Island directly.
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
@@ -193,7 +194,7 @@ impl JointConstraintsSet {
// Count the internal and external constraints builder.
let num_external_constraint_builders = self.generic_two_body_interactions.len();
let mut num_internal_constraint_builders = 0;
for handle in islands.active_island(island_id) {
for handle in islands.island(island_id).bodies() {
if let Some(link_id) = multibodies.rigid_body_link(*handle) {
if JointGenericInternalConstraintBuilder::num_constraints(multibodies, link_id) > 0
{
@@ -230,7 +231,7 @@ impl JointConstraintsSet {
// Generate internal constraints builder. They are indexed after the
let mut curr_builder = self.generic_two_body_interactions.len();
for handle in islands.active_island(island_id) {
for handle in islands.island(island_id).bodies() {
if curr_builder >= self.generic_velocity_constraints_builder.len() {
break; // No more builder need to be generated.
}

19
src/dynamics/solver/velocity_solver.rs
View File

@@ -82,7 +82,7 @@ impl VelocitySolver {
self.solver_bodies.clear();
let aligned_solver_bodies_len =
islands.active_island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH;
islands.island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH;
self.solver_bodies.resize(aligned_solver_bodies_len);
self.solver_vels_increment.clear();
@@ -97,7 +97,8 @@ impl VelocitySolver {
// Assign solver ids to multibodies, and collect the relevant roots.
// And init solver_vels for rigid-bodies.
let mut multibody_solver_id = 0;
for handle in islands.active_island(island_id) {
for (offset, handle) in islands.island(island_id).bodies().iter().enumerate() {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
@@ -110,10 +111,10 @@ impl VelocitySolver {
}
} else {
let rb = &bodies[*handle];
let solver_vel_incr =
&mut self.solver_vels_increment[rb.ids.active_set_offset as usize];
assert_eq!(offset, rb.ids.active_set_id);
let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_id];
self.solver_bodies
.copy_from(total_step_dt, rb.ids.active_set_offset as usize, rb);
.copy_from(total_step_dt, rb.ids.active_set_id, rb);
solver_vel_incr.angular =
rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt;
@@ -181,7 +182,7 @@ impl VelocitySolver {
if params.warmstart_coefficient != 0.0 {
// TODO PERF: we could probably figure out a way to avoid this warmstart when
// step_id > 0? Maybe for that to happen `solver_vels` needs to
// represent velocity changes instead of total rigid-boody velocities.
// represent velocity changes instead of total rigid-body velocities.
// Need to be careful wrt. multibody and joints too.
contact_constraints
.warmstart(&mut self.solver_bodies, &mut self.generic_solver_vels);
@@ -290,7 +291,7 @@ impl VelocitySolver {
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
) {
for handle in islands.active_island(island_id) {
for handle in islands.island(island_id).bodies() {
let link = if self.multibody_roots.is_empty() {
None
} else {
@@ -310,8 +311,8 @@ impl VelocitySolver {
}
} else {
let rb = bodies.index_mut_internal(*handle);
let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_offset as usize];
let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_offset as usize];
let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_id];
let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_id];
let dangvel = solver_vels.angular;

11
src/geometry/contact_pair.rs
View File

@@ -155,8 +155,6 @@ pub struct ContactPair {
/// [`Collider::contact_skin`] which only affects the constraint solver and the
/// [`SolverContact`].
pub manifolds: Vec<ContactManifold>,
/// Is there any active contact in this contact pair?
pub has_any_active_contact: bool,
/// Was a `CollisionEvent::Started` emitted for this collider?
pub(crate) start_event_emitted: bool,
pub(crate) workspace: Option<ContactManifoldsWorkspace>,
@@ -173,17 +171,22 @@ impl ContactPair {
Self {
collider1,
collider2,
has_any_active_contact: false,
manifolds: Vec::new(),
start_event_emitted: false,
workspace: None,
}
}
/// Is there any active contact in this contact pair?
pub fn has_any_active_contact(&self) -> bool {
self.manifolds
.iter()
.any(|m| !m.data.solver_contacts.is_empty())
}
/// Clears all the contacts of this contact pair.
pub fn clear(&mut self) {
self.manifolds.clear();
self.has_any_active_contact = false;
self.workspace = None;
}

218
src/geometry/manifold_reduction.rs Normal file
View File

@@ -0,0 +1,218 @@
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::utils::SimdBasis;
pub(crate) fn reduce_manifold_naive(
manifold: &ContactManifold,
selected: &mut [usize; 4],
num_selected: &mut usize,
prediction_distance: Real,
) {
if manifold.points.len() <= 4 {
return;
}
// 1. Find the deepest contact.
*selected = [usize::MAX; 4];
let mut deepest_dist = Real::MAX;
for (i, pt) in manifold.points.iter().enumerate() {
if pt.dist < deepest_dist {
deepest_dist = pt.dist;
selected[0] = i;
}
}
if selected[0] == usize::MAX {
*num_selected = 0;
return;
}
// 2. Find the point that is the furthest from the deepest point.
let selected_a = &manifold.points[selected[0]].local_p1;
let mut furthest_dist = -Real::MAX;
for (i, pt) in manifold.points.iter().enumerate() {
let dist = na::distance_squared(&pt.local_p1, selected_a);
if i != selected[0] && pt.dist <= prediction_distance && dist > furthest_dist {
furthest_dist = dist;
selected[1] = i;
}
}
if selected[1] == usize::MAX {
*num_selected = 1;
return;
}
// 3. Now find the two points furthest from the segment we built so far.
let selected_b = &manifold.points[selected[1]].local_p1;
if selected_a == selected_b {
*num_selected = 1;
return;
}
let selected_ab = selected_b - selected_a;
let tangent = selected_ab.cross(&manifold.local_n1);
// Find the points that minimize and maximize the dot product with the tangent.
let mut min_dot = Real::MAX;
let mut max_dot = -Real::MAX;
for (i, pt) in manifold.points.iter().enumerate() {
if i == selected[0] || i == selected[1] || pt.dist > prediction_distance {
continue;
}
let dot = (pt.local_p1 - selected_a).dot(&tangent);
if dot < min_dot {
min_dot = dot;
selected[2] = i;
}
if dot > max_dot {
max_dot = dot;
selected[3] = i;
}
}
if selected[2] == usize::MAX {
*num_selected = 2;
} else if selected[2] == selected[3] {
*num_selected = 3;
} else {
*num_selected = 4;
}
}
// Run contact reduction using Bepu's InternalReduce algorithm.
// The general idea is quite similar to our naive approach except that they add some
// additional heuristics. This is implemented mainly for comparison purpose to see
// if there is a strong advantage to having the extra checks.
#[allow(dead_code)]
pub(crate) fn reduce_manifold_bepu_like(
manifold: &ContactManifold,
selected: &mut [usize; 4],
num_selected: &mut usize,
) {
if manifold.points.len() <= 4 {
return;
}
// Step 1: Find the deepest contact, biased by extremity for frame stability.
// The extremity heuristic helps maintain consistent contact selection across frames
// when multiple contacts have similar depths.
let mut best_score = -Real::MAX;
const EXTREMITY_SCALE: Real = 1e-2;
// Use an arbitrary direction (roughly 38 degrees from X axis) to break ties
const EXTREMITY_DIR_X: Real = 0.7946898;
const EXTREMITY_DIR_Y: Real = 0.6070158;
let tangents = manifold.local_n1.orthonormal_basis();
for (i, pt) in manifold.points.iter().enumerate() {
// Extremity measures how far the contact is from the origin in the tangent plane
let tx1 = pt.local_p1.coords.dot(&tangents[0]);
let ty1 = pt.local_p1.coords.dot(&tangents[1]);
let extremity = (tx1 * EXTREMITY_DIR_X + ty1 * EXTREMITY_DIR_Y).abs();
// Score = depth + small extremity bias (only for non-speculative contacts)
// Negative dist = deeper penetration = higher score
let score = if pt.dist >= 0.0 {
-pt.dist // Speculative contact, no extremity bias
} else {
-pt.dist + extremity * EXTREMITY_SCALE
};
if score > best_score {
best_score = score;
selected[0] = i;
}
}
// Step 2: Find the point most distant from the first contact.
// This establishes a baseline "edge" for the manifold.
let contact0_pos = manifold.points[selected[0]].local_p1;
let mut max_distance_squared = 0.0;
for (i, pt) in manifold.points.iter().enumerate() {
let offset = pt.local_p1 - contact0_pos;
let offset_x = offset.dot(&tangents[0]);
let offset_y = offset.dot(&tangents[1]);
let distance_squared = offset_x * offset_x + offset_y * offset_y;
if distance_squared > max_distance_squared {
max_distance_squared = distance_squared;
selected[1] = i;
}
}
// Early out if the contacts are too close together
let epsilon = 1e-6;
if max_distance_squared <= epsilon {
// Only one meaningful contact
*num_selected = 1;
} else {
// Step 3: Find two more contacts that maximize positive and negative signed area.
// Using the first two contacts as an edge, we look for contacts that form triangles
// with the largest magnitude negative and positive areas. This maximizes the
// spatial extent of the contact manifold.
*num_selected = 2;
selected[2] = usize::MAX;
selected[3] = usize::MAX;
let contact1_pos = manifold.points[selected[1]].local_p1;
let edge_offset = contact1_pos - contact0_pos;
let edge_offset_x = edge_offset.dot(&tangents[0]);
let edge_offset_y = edge_offset.dot(&tangents[1]);
let mut min_signed_area = 0.0;
let mut max_signed_area = 0.0;
for (i, pt) in manifold.points.iter().enumerate() {
let candidate_offset = pt.local_p1 - contact0_pos;
let candidate_offset_x = candidate_offset.dot(&tangents[0]);
let candidate_offset_y = candidate_offset.dot(&tangents[1]);
// Signed area of the triangle formed by (contact0, contact1, candidate)
// This is a 2D cross product: (candidate - contact0) × (contact1 - contact0)
let mut signed_area =
candidate_offset_x * edge_offset_y - candidate_offset_y * edge_offset_x;
// Penalize speculative contacts (they're less important)
if pt.dist >= 0.0 {
signed_area *= 0.25;
}
if signed_area < min_signed_area {
min_signed_area = signed_area;
selected[2] = i;
}
if signed_area > max_signed_area {
max_signed_area = signed_area;
selected[3] = i;
}
}
// Check if the signed areas are significant enough
// Epsilon based on the edge length squared
let area_epsilon = max_distance_squared * max_distance_squared * 1e-6;
// If the areas are too small, don't add those contacts
if min_signed_area * min_signed_area <= area_epsilon {
selected[2] = usize::MAX;
}
if max_signed_area * max_signed_area <= area_epsilon {
selected[3] = usize::MAX;
}
let keep2 = selected[2] != usize::MAX;
let keep3 = selected[3] != usize::MAX;
*num_selected += keep2 as usize + keep3 as usize;
if !keep2 {
selected[2] = selected[3];
}
}
}

3
src/geometry/mod.rs
View File

@@ -231,3 +231,6 @@ mod broad_phase_pair_event;
mod collider;
mod collider_set;
mod mesh_converter;
#[cfg(feature = "dim3")]
mod manifold_reduction;

150
src/geometry/narrow_phase.rs
View File

@@ -13,7 +13,7 @@ use crate::geometry::{
ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
TemporaryInteractionIndex,
};
use crate::math::{Real, Vector};
use crate::math::{MAX_MANIFOLD_POINTS, Real, Vector};
use crate::pipeline::{
ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
PhysicsHooks,
@@ -582,7 +582,7 @@ impl NarrowPhase {
// Emit a contact stopped event if we had a contact before removing the edge.
// Also wake up the dynamic bodies that were in contact.
if let Some(mut ctct) = contact_pair {
if ctct.has_any_active_contact {
if ctct.has_any_active_contact() {
if let Some(islands) = islands {
if let Some(co_parent1) = &co1.parent {
islands.wake_up(bodies, co_parent1.handle, true);
@@ -808,7 +808,8 @@ impl NarrowPhase {
&mut self,
prediction_distance: Real,
dt: Real,
bodies: &RigidBodySet,
islands: &mut IslandManager,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
@@ -817,10 +818,10 @@ impl NarrowPhase {
) {
let query_dispatcher = &*self.query_dispatcher;
// TODO: don't iterate on all the edges.
// TODO PERF: don't iterate on all the edges.
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
let pair = &mut edge.weight;
let had_any_active_contact = pair.has_any_active_contact;
let had_any_active_contact = pair.has_any_active_contact();
let co1 = &colliders[pair.collider1];
let co2 = &colliders[pair.collider2];
@@ -831,7 +832,9 @@ impl NarrowPhase {
// No update needed for these colliders.
return;
}
if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) && co1.parent.is_some()
if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle)
&& co1.parent.is_some()
{
// Same parents. Ignore collisions.
pair.clear();
@@ -858,7 +861,7 @@ impl NarrowPhase {
let link1 = multibody_joints.rigid_body_link(co_parent1.handle);
let link2 = multibody_joints.rigid_body_link(co_parent2.handle);
if let (Some(link1),Some(link2)) = (link1, link2) {
if let (Some(link1), Some(link2)) = (link1, link2) {
// If both bodies belong to the same multibody, apply some additional built-in
// contact filtering rules.
if link1.multibody == link2.multibody {
@@ -936,24 +939,27 @@ impl NarrowPhase {
let contact_skin_sum = co1.contact_skin() + co2.contact_skin();
let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 {
let effective_prediction_distance =
if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 {
let aabb1 = co1.compute_collision_aabb(0.0);
let aabb2 = co2.compute_collision_aabb(0.0);
let inv_dt = crate::utils::inv(dt);
let linvel1 = rb1.map(|rb| rb.linvel()
.cap_magnitude(soft_ccd_prediction1 * inv_dt)).unwrap_or_default();
let linvel2 = rb2.map(|rb| rb.linvel()
.cap_magnitude(soft_ccd_prediction2 * inv_dt)).unwrap_or_default();
let linvel1 = rb1
.map(|rb| rb.linvel().cap_magnitude(soft_ccd_prediction1 * inv_dt))
.unwrap_or_default();
let linvel2 = rb2
.map(|rb| rb.linvel().cap_magnitude(soft_ccd_prediction2 * inv_dt))
.unwrap_or_default();
if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) {
if !aabb1.intersects(&aabb2)
&& !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1)
{
pair.clear();
break 'emit_events;
}
prediction_distance.max(
dt * (linvel1 - linvel2).norm()) + contact_skin_sum
prediction_distance.max(dt * (linvel1 - linvel2).norm()) + contact_skin_sum
} else {
prediction_distance + contact_skin_sum
};
@@ -984,8 +990,6 @@ impl NarrowPhase {
let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero);
let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero);
pair.has_any_active_contact = false;
for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos);
let world_pos2 = manifold.subshape_pos2.prepend_to(&co2.pos);
@@ -998,20 +1002,42 @@ impl NarrowPhase {
manifold.data.normal = world_pos1 * manifold.local_n1;
// Generate solver contacts.
for (contact_id, contact) in manifold.points.iter().enumerate() {
if contact_id > u8::MAX as usize {
log::warn!("A contact manifold cannot contain more than 255 contacts currently, dropping contact in excess.");
break;
}
#[allow(unused_mut)] // Mut not needed in 2D.
let mut selected = [0, 1, 2, 3];
#[allow(unused_mut)] // Mut not needed in 2D.
let mut num_selected = MAX_MANIFOLD_POINTS.min(manifold.points.len());
let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin();
#[cfg(feature = "dim3")]
// super::manifold_reduction::reduce_manifold_bepu_like(
// manifold,
// &mut selected,
// &mut num_selected,
// );
#[cfg(feature = "dim3")]
super::manifold_reduction::reduce_manifold_naive(
manifold,
&mut selected,
&mut num_selected,
prediction_distance,
);
for contact_id in &selected[..num_selected] {
// // manifold.points.iter().enumerate() {
let contact = &manifold.points[*contact_id];
let effective_contact_dist =
contact.dist - co1.contact_skin() - co2.contact_skin();
let keep_solver_contact = effective_contact_dist < prediction_distance || {
let world_pt1 = world_pos1 * contact.local_p1;
let world_pt2 = world_pos2 * contact.local_p2;
let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default();
let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default();
effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance
let vel1 = rb1
.map(|rb| rb.velocity_at_point(&world_pt1))
.unwrap_or_default();
let vel2 = rb2
.map(|rb| rb.velocity_at_point(&world_pt2))
.unwrap_or_default();
effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt
< prediction_distance
};
if keep_solver_contact {
@@ -1021,7 +1047,7 @@ impl NarrowPhase {
let effective_point = na::center(&world_pt1, &world_pt2);
let solver_contact = SolverContact {
contact_id: [contact_id as u32],
contact_id: [*contact_id as u32],
point: effective_point,
dist: effective_contact_dist,
friction,
@@ -1039,7 +1065,6 @@ impl NarrowPhase {
};
manifold.data.solver_contacts.push(solver_contact);
pair.has_any_active_contact = true;
}
}
@@ -1069,49 +1094,34 @@ impl NarrowPhase {
manifold.data.normal = modifiable_normal;
manifold.data.user_data = modifiable_user_data;
}
}
}
/*
* TODO: When using the block solver in 3D, I’d expect this sort to help, but
* it makes the domino demo worse. Needs more investigation.
fn sort_solver_contacts(mut contacts: &mut [SolverContact]) {
while contacts.len() > 2 {
let first = contacts[0];
let mut furthest_id = 1;
let mut furthest_dist = na::distance(&first.point, &contacts[1].point);
for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) {
let candidate_dist = na::distance(&first.point, &candidate.point);
if candidate_dist > furthest_dist {
furthest_dist = candidate_dist;
furthest_id = candidate_id;
}
}
if furthest_id > 1 {
contacts.swap(1, furthest_id);
}
contacts = &mut contacts[2..];
}
}
sort_solver_contacts(&mut manifold.data.solver_contacts);
* Handle actions on contact start/stop:
* - Emit event (if applicable).
* - Notify the island manager to potentially wake up the bodies.
*/
}
}
let has_any_active_contact = pair.has_any_active_contact();
if has_any_active_contact != had_any_active_contact {
let active_events = co1.flags.active_events | co2.flags.active_events;
if pair.has_any_active_contact != had_any_active_contact
&& active_events.contains(ActiveEvents::COLLISION_EVENTS)
{
if pair.has_any_active_contact {
if active_events.contains(ActiveEvents::COLLISION_EVENTS) {
if has_any_active_contact {
pair.emit_start_event(bodies, colliders, events);
} else {
pair.emit_stop_event(bodies, colliders, events);
}
}
// FIXME: this won’t work with the parallel feature enabled.
islands.interaction_started_or_stopped(
bodies,
co1.parent.map(|p| p.handle),
co2.parent.map(|p| p.handle),
has_any_active_contact,
true,
);
}
});
}
@@ -1125,7 +1135,7 @@ impl NarrowPhase {
out_manifolds: &mut Vec<&'a mut ContactManifold>,
out: &mut [Vec<ContactManifoldIndex>],
) {
for out_island in &mut out[..islands.num_islands()] {
for out_island in &mut out[..islands.active_islands().len()] {
out_island.clear();
}
@@ -1168,13 +1178,17 @@ impl NarrowPhase {
&& (!rb_type1.is_dynamic() || !sleeping1)
&& (!rb_type2.is_dynamic() || !sleeping2)
{
let island_index = if !rb_type1.is_dynamic() {
active_island_id2
let island_awake_index = if !rb_type1.is_dynamic() {
islands.islands[active_island_id2]
.id_in_awake_list()
.expect("Internal error: island should be awake.")
} else {
active_island_id1
islands.islands[active_island_id1]
.id_in_awake_list()
.expect("Internal error: island should be awake.")
};
out[island_index].push(out_manifolds.len());
out[island_awake_index].push(out_manifolds.len());
out_manifolds.push(manifold);
push_pair = true;
}

6
src/pipeline/collision_pipeline.rs
View File

@@ -1,6 +1,6 @@
//! Physics pipeline structures.
use crate::dynamics::{ImpulseJointSet, IntegrationParameters, MultibodyJointSet};
use crate::dynamics::{ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet};
use crate::geometry::{
BroadPhaseBvh, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair,
ModifiedColliders, NarrowPhase,
@@ -53,6 +53,7 @@ impl CollisionPipeline {
fn detect_collisions(
&mut self,
prediction_distance: Real,
islands: &mut IslandManager,
broad_phase: &mut BroadPhaseBvh,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
@@ -98,6 +99,7 @@ impl CollisionPipeline {
narrow_phase.compute_contacts(
prediction_distance,
0.0,
islands,
bodies,
colliders,
&ImpulseJointSet::new(),
@@ -126,6 +128,7 @@ impl CollisionPipeline {
pub fn step(
&mut self,
prediction_distance: Real,
islands: &mut IslandManager,
broad_phase: &mut BroadPhaseBvh,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
@@ -162,6 +165,7 @@ impl CollisionPipeline {
self.detect_collisions(
prediction_distance,
islands,
broad_phase,
narrow_phase,
bodies,

90
src/pipeline/physics_pipeline.rs
View File

@@ -169,6 +169,7 @@ impl PhysicsPipeline {
narrow_phase.compute_contacts(
integration_parameters.prediction_distance(),
integration_parameters.dt,
islands,
bodies,
colliders,
impulse_joints,
@@ -195,7 +196,8 @@ impl PhysicsPipeline {
events: &dyn EventHandler,
) {
self.counters.stages.island_construction_time.resume();
islands.update_active_set_with_contacts(
// NOTE: islands update must be done after the narrow-phase.
islands.update_islands(
integration_parameters.dt,
integration_parameters.length_unit,
bodies,
@@ -203,19 +205,23 @@ impl PhysicsPipeline {
narrow_phase,
impulse_joints,
multibody_joints,
integration_parameters.min_island_size,
);
if self.manifold_indices.len() < islands.num_islands() {
self.manifold_indices
.resize(islands.num_islands(), Vec::new());
let num_active_islands = islands.active_islands().len();
if self.manifold_indices.len() < num_active_islands {
self.manifold_indices.resize(num_active_islands, Vec::new());
}
if self.joint_constraint_indices.len() < islands.num_islands() {
if self.joint_constraint_indices.len() < num_active_islands {
self.joint_constraint_indices
.resize(islands.num_islands(), Vec::new());
.resize(num_active_islands, Vec::new());
}
self.counters.stages.island_construction_time.pause();
self.counters
.stages
.island_constraints_collection_time
.resume();
let mut manifolds = Vec::new();
narrow_phase.select_active_contacts(
islands,
@@ -229,13 +235,16 @@ impl PhysicsPipeline {
bodies,
&mut self.joint_constraint_indices,
);
self.counters.stages.island_construction_time.pause();
self.counters
.stages
.island_constraints_collection_time
.pause();
self.counters.stages.update_time.resume();
for handle in islands.active_bodies() {
// TODO: should that be moved to the solver (just like we moved
// the multibody dynamics update) since it depends on dt?
let rb = bodies.index_mut_internal(*handle);
let rb = bodies.index_mut_internal(handle);
rb.mprops
.update_world_mass_properties(rb.body_type, &rb.pos.position);
let effective_mass = rb.mprops.effective_mass();
@@ -245,26 +254,26 @@ impl PhysicsPipeline {
self.counters.stages.update_time.pause();
self.counters.stages.solver_time.resume();
if self.solvers.len() < islands.num_islands() {
if self.solvers.len() < num_active_islands {
self.solvers
.resize_with(islands.num_islands(), IslandSolver::new);
.resize_with(num_active_islands, IslandSolver::new);
}
#[cfg(not(feature = "parallel"))]
{
enable_flush_to_zero!();
for island_id in 0..islands.num_islands() {
self.solvers[island_id].init_and_solve(
island_id,
for (island_awake_id, island_id) in islands.active_islands().iter().enumerate() {
self.solvers[island_awake_id].init_and_solve(
*island_id,
&mut self.counters,
integration_parameters,
islands,
bodies,
&mut manifolds[..],
&self.manifold_indices[island_id],
&self.manifold_indices[island_awake_id],
impulse_joints.joints_mut(),
&self.joint_constraint_indices[island_id],
&self.joint_constraint_indices[island_awake_id],
multibody_joints,
)
}
@@ -276,8 +285,7 @@ impl PhysicsPipeline {
use rayon::prelude::*;
use std::sync::atomic::Ordering;
let num_islands = islands.num_islands();
let solvers = &mut self.solvers[..num_islands];
let solvers = &mut self.solvers[..num_active_islands];
let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _);
let impulse_joints =
@@ -296,7 +304,8 @@ impl PhysicsPipeline {
solvers
.par_iter_mut()
.enumerate()
.for_each(|(island_id, solver)| {
.for_each(|(island_awake_id, solver)| {
let island_id = islands.active_islands()[island_awake_id];
let bodies: &mut RigidBodySet =
unsafe { &mut *bodies.load(Ordering::Relaxed) };
let manifolds: &mut Vec<&mut ContactManifold> =
@@ -314,9 +323,9 @@ impl PhysicsPipeline {
islands,
bodies,
&mut manifolds[..],
&manifold_indices[island_id],
&manifold_indices[island_awake_id],
impulse_joints,
&joint_constraint_indices[island_id],
&joint_constraint_indices[island_awake_id],
multibody_joints,
)
});
@@ -389,7 +398,7 @@ impl PhysicsPipeline {
) {
// Set the rigid-bodies and kinematic bodies to their final position.
for handle in islands.active_bodies() {
let rb = bodies.index_mut_internal(*handle);
let rb = bodies.index_mut_internal(handle);
rb.pos.position = rb.pos.next_position;
rb.colliders
.update_positions(colliders, modified_colliders, &rb.pos.position);
@@ -409,7 +418,7 @@ impl PhysicsPipeline {
// bodies it is touching.
for handle in islands.active_bodies() {
// TODO PERF: only iterate on kinematic position-based bodies
let rb = bodies.index_mut_internal(*handle);
let rb = bodies.index_mut_internal(handle);
match rb.body_type {
RigidBodyType::KinematicPositionBased => {
@@ -495,16 +504,16 @@ impl PhysicsPipeline {
// Apply some of delayed wake-ups.
self.counters.stages.user_changes.start();
#[cfg(feature = "enhanced-determinism")]
let impulse_joints_iterator = impulse_joints
let to_wake_up_iterator = impulse_joints
.to_wake_up
.drain(..)
.chain(multibody_joints.to_wake_up.drain(..));
#[cfg(not(feature = "enhanced-determinism"))]
let impulse_joints_iterator = impulse_joints
let to_wake_up_iterator = impulse_joints
.to_wake_up
.drain()
.chain(multibody_joints.to_wake_up.drain());
for handle in impulse_joints_iterator {
for handle in to_wake_up_iterator {
islands.wake_up(bodies, handle, true);
}
@@ -538,6 +547,27 @@ impl PhysicsPipeline {
.copied()
.filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)),
);
// Join islands based on new joints.
#[cfg(feature = "enhanced-determinism")]
let to_join_iterator = impulse_joints
.to_join
.drain(..)
.chain(multibody_joints.to_join.drain(..));
#[cfg(not(feature = "enhanced-determinism"))]
let to_join_iterator = impulse_joints
.to_join
.drain()
.chain(multibody_joints.to_join.drain());
for (handle1, handle2) in to_join_iterator {
islands.interaction_started_or_stopped(
bodies,
Some(handle1),
Some(handle2),
true,
false,
);
}
self.counters.stages.user_changes.pause();
// TODO: do this only on user-change.
@@ -643,7 +673,9 @@ impl PhysicsPipeline {
self.counters.ccd.num_substeps += 1;
self.counters.custom.resume();
self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies);
self.counters.custom.pause();
self.build_islands_and_solve_velocity_constraints(
gravity,
&integration_parameters,
@@ -705,6 +737,8 @@ impl PhysicsPipeline {
} else {
// If we ran the last substep, just update the broad-phase bvh instead
// of a full collision-detection step.
self.counters.stages.collision_detection_time.resume();
self.counters.cd.final_broad_phase_time.resume();
for handle in modified_colliders.iter() {
let co = colliders.index_mut_internal(*handle);
// NOTE: `advance_to_final_positions` might have added disabled colliders to
@@ -732,6 +766,8 @@ impl PhysicsPipeline {
// Empty the modified colliders set. See comment for `co.change.remove(..)` above.
modified_colliders.clear();
self.counters.cd.final_broad_phase_time.pause();
self.counters.stages.collision_detection_time.pause();
}
}
@@ -743,7 +779,7 @@ impl PhysicsPipeline {
// not modified by the user in the mean time.
self.counters.stages.update_time.resume();
for handle in islands.active_bodies() {
let rb = bodies.index_mut_internal(*handle);
let rb = bodies.index_mut_internal(handle);
rb.mprops
.update_world_mass_properties(rb.body_type, &rb.pos.position);
}

19
src/pipeline/user_changes.rs
View File

@@ -63,30 +63,21 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
continue;
}
{
let rb = bodies.index_mut_internal(*handle);
let mut ids = rb.ids;
let changes = rb.changes;
let activation = rb.activation;
{
if rb.is_enabled() {
// The body's status changed. We need to make sure
// it is on the correct active set.
if let Some(islands) = islands.as_deref_mut() {
// Push the body to the active set if it is not inside the active set yet, and
// is not longer sleeping or became dynamic.
if (changes.contains(RigidBodyChanges::SLEEP) || changes.contains(RigidBodyChanges::TYPE))
&& rb.is_enabled()
&& !rb.activation.sleeping // May happen if the body was put to sleep manually.
&& rb.is_dynamic_or_kinematic() // Only dynamic bodies are in the active dynamic set.
&& islands.active_set.get(ids.active_set_id) != Some(handle)
{
ids.active_set_id = islands.active_set.len(); // This will handle the case where the activation_channel contains duplicates.
islands.active_set.push(*handle);
}
islands.rigid_body_updated(*handle, bodies);
}
}
let rb = bodies.index_mut_internal(*handle);
// Update the colliders' positions.
if changes.contains(RigidBodyChanges::POSITION)
|| changes.contains(RigidBodyChanges::COLLIDERS)
@@ -157,7 +148,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
);
}
rb.ids = ids;
rb.activation = activation;
}
@@ -166,6 +156,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
if let Some(action) = final_action {
match action {
FinalAction::RemoveFromIsland => {
let rb = bodies.index_mut_internal(*handle);
let ids = rb.ids;
islands.rigid_body_removed(*handle, &ids, bodies);
}

34
src_testbed/physx_backend.rs
View File

@@ -1,5 +1,6 @@
#![allow(dead_code)]
use crate::ui::egui::emath::OrderedFloat;
use na::{Isometry3, Matrix4, Point3, Quaternion, Translation3, Unit, UnitQuaternion, Vector3};
use physx::cooking::{
ConvexMeshCookingResult, PxConvexMeshDesc, PxCookingParams, PxHeightFieldDesc,
@@ -133,7 +134,7 @@ pub static FOUNDATION: std::cell::RefCell<PxPhysicsFoundation> = std::cell::RefC
pub struct PhysxWorld {
// physics: Physics,
materials: Vec<Owner<PxMaterial>>,
// materials: Vec<Owner<PxMaterial>>,
shapes: Vec<Owner<PxShape>>,
scene: Option<Owner<PxScene>>,
}
@@ -141,7 +142,7 @@ pub struct PhysxWorld {
impl Drop for PhysxWorld {
fn drop(&mut self) {
let scene = self.scene.take();
// FIXME: we get a segfault if we don't forget the scene.
// FIXME: we get a segfault if we don't leak the scene.
std::mem::forget(scene);
}
}
@@ -161,7 +162,6 @@ impl PhysxWorld {
FOUNDATION.with(|physics| {
let mut physics = physics.borrow_mut();
let mut shapes = Vec::new();
let mut materials = Vec::new();
let friction_type = if use_two_friction_directions {
FrictionType::TwoDirectional
@@ -300,10 +300,13 @@ impl PhysxWorld {
* Colliders
*
*/
let mut materials_cache = HashMap::new();
for (_, collider) in colliders.iter() {
if let Some((mut px_shape, px_material, collider_pos)) =
physx_collider_from_rapier_collider(&mut *physics, &collider)
{
if let Some((mut px_shape, collider_pos)) = physx_collider_from_rapier_collider(
&mut *physics,
&collider,
&mut materials_cache,
) {
if let Some(parent_handle) = collider.parent() {
let parent_body = &bodies[parent_handle];
@@ -331,7 +334,6 @@ impl PhysxWorld {
}
shapes.push(px_shape);
materials.push(px_material);
}
}
}
@@ -396,7 +398,6 @@ impl PhysxWorld {
Self {
scene: Some(scene),
shapes,
materials,
}
})
}
@@ -557,7 +558,8 @@ impl PhysxWorld {
fn physx_collider_from_rapier_collider(
physics: &mut PxPhysicsFoundation,
collider: &Collider,
) -> Option<(Owner<PxShape>, Owner<PxMaterial>, Isometry3<f32>)> {
materials_cache: &mut HashMap<[OrderedFloat<f32>; 2], Owner<PxMaterial>>,
) -> Option<(Owner<PxShape>, Isometry3<f32>)> {
let mut local_pose = collider.position_wrt_parent().copied().unwrap_or(na::one());
let cooking_params = PxCookingParams::new(physics).unwrap();
let shape = collider.shape();
@@ -566,14 +568,22 @@ fn physx_collider_from_rapier_collider(
} else {
ShapeFlags::SimulationShape
};
let mut material = physics
let material = materials_cache
.entry([
OrderedFloat(collider.material().friction),
OrderedFloat(collider.material().restitution),
])
.or_insert_with(|| {
physics
.create_material(
collider.material().friction,
collider.material().friction,
collider.material().restitution,
(),
)
.unwrap();
.unwrap()
});
let materials = &mut [material.as_mut()][..];
let shape = if let Some(cuboid) = shape.as_cuboid() {
@@ -705,7 +715,7 @@ fn physx_collider_from_rapier_collider(
return None;
};
shape.map(|s| (s, material, local_pose))
shape.map(|s| (s, local_pose))
}
type PxPhysicsFoundation = PhysicsFoundation<DefaultAllocator, PxShape>;

24
src_testbed/ui.rs
View File

@@ -320,6 +320,10 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) {
"Broad-phase: {:.2}ms",
counters.broad_phase_time_ms()
));
ui.label(format!(
"Final broad-phase: {:.2}ms",
counters.cd.final_broad_phase_time.time_ms()
));
ui.label(format!(
"Narrow-phase: {:.2}ms",
counters.narrow_phase_time_ms()
@@ -332,6 +336,20 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) {
"Velocity assembly: {:.2}ms",
counters.solver.velocity_assembly_time.time_ms()
));
ui.label(format!(
"> Velocity assembly - solver bodies: {:.2}ms",
counters
.solver
.velocity_assembly_time_solver_bodies
.time_ms()
));
ui.label(format!(
"> Velocity assembly - constraints init: {:.2}ms",
counters
.solver
.velocity_assembly_time_constraints_init
.time_ms()
));
ui.label(format!(
"Velocity resolution: {:.2}ms",
counters.velocity_resolution_time_ms()
@@ -370,10 +388,16 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) {
"Island computation: {:.2}ms",
counters.island_construction_time_ms()
));
ui.label(format!(
"Active constraints collection: {:.2}ms",
counters.stages.island_constraints_collection_time.time_ms()
));
ui.label(format!("Mprops update: {:.2}ms", counters.update_time_ms()));
ui.label(format!(
"User changes: {:.2}ms",
counters.stages.user_changes.time_ms()
));
ui.label(format!("Debug timer: {:.2}ms", counters.custom.time_ms()));
});
}