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

Merge pull request #69 from dimforge/rigid_body_modifications

Browse Source 
Track some user-initiated rigid-body modifications
This commit is contained in:
Sébastien Crozet
2020-11-26 13:26:07 +01:00
committed by GitHub
parent bdf2e15fdc 340f614d32
commit 51b7bf9a52
17 changed files with 447 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
examples3d/all_examples3.rs
View File

@@ -13,9 +13,12 @@ use std::cmp::Ordering;
mod collision_groups3;
mod compound3;
mod damping3;
mod debug_add_remove_collider3;
mod debug_boxes3;
mod debug_cylinder3;
mod debug_dynamic_collider_add3;
mod debug_infinite_fall3;
mod debug_rollback3;
mod debug_triangle3;
mod debug_trimesh3;
mod domino3;
@@ -81,11 +84,20 @@ pub fn main() {
("Sensor", sensor3::init_world),
("Trimesh", trimesh3::init_world),
("Keva tower", keva3::init_world),
(
"(Debug) add/rm collider",
debug_add_remove_collider3::init_world,
),
("(Debug) boxes", debug_boxes3::init_world),
(
"(Debug) dyn. coll. add",
debug_dynamic_collider_add3::init_world,
),
("(Debug) triangle", debug_triangle3::init_world),
("(Debug) trimesh", debug_trimesh3::init_world),
("(Debug) cylinder", debug_cylinder3::init_world),
("(Debug) infinite fall", debug_infinite_fall3::init_world),
("(Debug) rollback", debug_rollback3::init_world),
];
// Lexicographic sort, with stress tests moved at the end of the list.

59
examples3d/debug_add_remove_collider3.rs Normal file
View File

@@ -0,0 +1,59 @@
use na::{Isometry3, Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground.
*/
let ground_size = 3.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
.build();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4).build();
let mut ground_collider_handle = colliders.insert(collider, ground_handle, &mut bodies);
/*
* Rolling ball
*/
let ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic()
.translation(0.0, 0.2, 0.0)
.build();
let ball_handle = bodies.insert(rb);
let collider = ColliderBuilder::ball(ball_rad).density(100.0).build();
colliders.insert(collider, ball_handle, &mut bodies);
testbed.add_callback(move |window, physics, _, graphics, _| {
// Remove then re-add the ground collider.
let coll = physics
.colliders
.remove(ground_collider_handle, &mut physics.bodies, true)
.unwrap();
ground_collider_handle = physics
.colliders
.insert(coll, ground_handle, &mut physics.bodies);
graphics.add_collider(window, ground_collider_handle, &physics.colliders);
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

112
examples3d/debug_dynamic_collider_add3.rs Normal file
View File

@@ -0,0 +1,112 @@
use na::{Isometry3, Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground.
*/
let ground_size = 20.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
.build();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4)
.friction(0.15)
// .restitution(0.5)
.build();
let mut ground_collider_handle = colliders.insert(collider, ground_handle, &mut bodies);
/*
* Rolling ball
*/
let ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic()
.translation(0.0, 0.2, 0.0)
.linvel(10.0, 0.0, 0.0)
.build();
let ball_handle = bodies.insert(rb);
let collider = ColliderBuilder::ball(ball_rad).density(100.0).build();
colliders.insert(collider, ball_handle, &mut bodies);
let mut linvel = Vector3::zeros();
let mut angvel = Vector3::zeros();
let mut pos = Isometry3::identity();
let mut step = 0;
let mut extra_colliders = Vec::new();
let snapped_frame = 51;
testbed.add_callback(move |window, physics, _, graphics, _| {
step += 1;
// Add a bigger ball collider
let collider = ColliderBuilder::ball(ball_rad + 0.01 * (step as f32))
.density(100.0)
.build();
let new_ball_collider_handle =
physics
.colliders
.insert(collider, ball_handle, &mut physics.bodies);
graphics.add_collider(window, new_ball_collider_handle, &physics.colliders);
extra_colliders.push(new_ball_collider_handle);
// Snap the ball velocity or restore it.
let mut ball = physics.bodies.get_mut(ball_handle).unwrap();
if step == snapped_frame {
linvel = *ball.linvel();
angvel = *ball.angvel();
pos = *ball.position();
}
if step == 100 {
ball.set_linvel(linvel, true);
ball.set_angvel(angvel, true);
ball.set_position(pos, true);
step = snapped_frame;
for handle in &extra_colliders {
physics.colliders.remove(*handle, &mut physics.bodies, true);
}
extra_colliders.clear();
}
// Remove then re-add the ground collider.
// let ground = physics.bodies.get_mut(ground_handle).unwrap();
// ground.set_position(Isometry3::translation(0.0, step as f32 * 0.001, 0.0), false);
// let coll = physics
// .colliders
// .remove(ground_collider_handle, &mut physics.bodies, true)
// .unwrap();
let coll = ColliderBuilder::cuboid(ground_size, ground_height + step as f32 * 0.01, 0.4)
.friction(0.15)
.build();
let new_ground_collider_handle =
physics
.colliders
.insert(coll, ground_handle, &mut physics.bodies);
graphics.add_collider(window, new_ground_collider_handle, &physics.colliders);
extra_colliders.push(new_ground_collider_handle);
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

77
examples3d/debug_rollback3.rs Normal file
View File

@@ -0,0 +1,77 @@
use na::{Isometry3, Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground.
*/
let ground_size = 20.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
.build();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4)
.friction(0.15)
// .restitution(0.5)
.build();
let mut ground_collider_handle = colliders.insert(collider, ground_handle, &mut bodies);
/*
* Rolling ball
*/
let ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic()
.translation(0.0, 0.2, 0.0)
.linvel(10.0, 0.0, 0.0)
.build();
let ball_handle = bodies.insert(rb);
let collider = ColliderBuilder::ball(ball_rad).density(100.0).build();
colliders.insert(collider, ball_handle, &mut bodies);
let mut linvel = Vector3::zeros();
let mut angvel = Vector3::zeros();
let mut pos = Isometry3::identity();
let mut step = 0;
let snapped_frame = 51;
testbed.add_callback(move |window, physics, _, graphics, _| {
step += 1;
// Snap the ball velocity or restore it.
let mut ball = physics.bodies.get_mut(ball_handle).unwrap();
if step == snapped_frame {
linvel = *ball.linvel();
angvel = *ball.angvel();
pos = *ball.position();
}
if step == 100 {
ball.set_linvel(linvel, true);
ball.set_angvel(angvel, true);
ball.set_position(pos, true);
step = snapped_frame;
}
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

2
examples3d/platform3.rs
View File

@@ -71,7 +71,7 @@ pub fn init_world(testbed: &mut Testbed) {
return;
}
if let Some(mut platform) = physics.bodies.get_mut(platform_handle) {
if let Some(platform) = physics.bodies.get_mut(platform_handle) {
let mut next_pos = *platform.position();
let dt = 0.016;

3
src/dynamics/mod.rs
View File

@@ -9,9 +9,10 @@ pub use self::joint::{
};
pub use self::mass_properties::MassProperties;
pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodyMut, RigidBodySet};
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
// #[cfg(not(feature = "parallel"))]
pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::rigid_body::RigidBodyChanges;
#[cfg(not(feature = "parallel"))]
pub(crate) use self::solver::IslandSolver;
#[cfg(feature = "parallel")]

53
src/dynamics/rigid_body.rs
View File

@@ -1,5 +1,7 @@
use crate::dynamics::MassProperties;
use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
use crate::utils::{WCross, WDot};
use num::Zero;
@@ -23,6 +25,17 @@ pub enum BodyStatus {
// Disabled,
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub(crate) struct RigidBodyChanges: u32 {
const MODIFIED = 1 << 0;
const POSITION = 1 << 1;
const SLEEP = 1 << 2;
const COLLIDERS = 1 << 3;
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A rigid body.
///
@@ -56,6 +69,7 @@ pub struct RigidBody {
pub(crate) active_set_id: usize,
pub(crate) active_set_offset: usize,
pub(crate) active_set_timestamp: u32,
pub(crate) changes: RigidBodyChanges,
/// The status of the body, governing how it is affected by external forces.
pub body_status: BodyStatus,
/// User-defined data associated to this rigid-body.
@@ -83,6 +97,7 @@ impl RigidBody {
active_set_id: 0,
active_set_offset: 0,
active_set_timestamp: 0,
changes: RigidBodyChanges::all(),
body_status: BodyStatus::Dynamic,
user_data: 0,
}
@@ -151,7 +166,12 @@ impl RigidBody {
}
/// Adds a collider to this rigid-body.
pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) {
self.changes.set(
RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
true,
);
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
@@ -160,9 +180,18 @@ impl RigidBody {
self.update_world_mass_properties();
}
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
let collider = &mut colliders[*handle];
collider.position = self.position * collider.delta;
collider.predicted_position = self.predicted_position * collider.delta;
}
}
/// Removes a collider from this rigid-body.
pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
self.changes.set(RigidBodyChanges::COLLIDERS, true);
self.colliders.swap_remove(i);
let mass_properties = coll
.mass_properties()
@@ -189,7 +218,10 @@ impl RigidBody {
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, strong: bool) {
self.activation.sleeping = false;
if self.activation.sleeping {
self.changes.insert(RigidBodyChanges::SLEEP);
self.activation.sleeping = false;
}
if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
self.activation.energy = self.activation.threshold.abs() * 2.0;
@@ -301,14 +333,21 @@ impl RigidBody {
/// If `wake_up` is `true` then the rigid-body will be woken up if it was
/// put to sleep because it did not move for a while.
pub fn set_position(&mut self, pos: Isometry<f32>, wake_up: bool) {
self.changes.insert(RigidBodyChanges::POSITION);
self.set_position_internal(pos);
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
self.wake_up(true)
}
}
pub(crate) fn set_position_internal(&mut self, pos: Isometry<f32>) {
self.position = pos;
// TODO: update the predicted position for dynamic bodies too?
if self.is_static() || self.is_kinematic() {
self.predicted_position = pos;
} else if wake_up {
// wake_up is true and the rigid-body is dynamic.
self.wake_up(true);
}
}
@@ -609,7 +648,7 @@ impl RigidBodyBuilder {
pub fn build(&self) -> RigidBody {
let mut rb = RigidBody::new();
rb.predicted_position = self.position; // FIXME: compute the correct value?
rb.set_position(self.position, false);
rb.set_position_internal(self.position);
rb.linvel = self.linvel;
rb.angvel = self.angvel;
rb.body_status = self.body_status;

203
src/dynamics/rigid_body_set.rs
View File

@@ -2,54 +2,9 @@
use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph, NarrowPhase};
use crossbeam::channel::{Receiver, Sender};
use std::ops::{Deref, DerefMut, Index, IndexMut};
/// A mutable reference to a rigid-body.
pub struct RigidBodyMut<'a> {
rb: &'a mut RigidBody,
was_sleeping: bool,
handle: RigidBodyHandle,
sender: &'a Sender<RigidBodyHandle>,
}
impl<'a> RigidBodyMut<'a> {
fn new(
handle: RigidBodyHandle,
rb: &'a mut RigidBody,
sender: &'a Sender<RigidBodyHandle>,
) -> Self {
Self {
was_sleeping: rb.is_sleeping(),
handle,
sender,
rb,
}
}
}
impl<'a> Deref for RigidBodyMut<'a> {
type Target = RigidBody;
fn deref(&self) -> &RigidBody {
&*self.rb
}
}
impl<'a> DerefMut for RigidBodyMut<'a> {
fn deref_mut(&mut self) -> &mut RigidBody {
self.rb
}
}
impl<'a> Drop for RigidBodyMut<'a> {
fn drop(&mut self) {
if self.was_sleeping && !self.rb.is_sleeping() {
self.sender.send(self.handle).unwrap();
}
}
}
use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
use crate::geometry::{ColliderHandle, ColliderSet, InteractionGraph, NarrowPhase};
use std::ops::{Index, IndexMut};
/// The unique handle of a rigid body added to a `RigidBodySet`.
pub type RigidBodyHandle = crate::data::arena::Index;
@@ -90,15 +45,12 @@ pub struct RigidBodySet {
pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
active_set_timestamp: u32,
pub(crate) modified_bodies: Vec<RigidBodyHandle>,
pub(crate) modified_all_bodies: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
can_sleep: Vec<RigidBodyHandle>, // Workspace.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
stack: Vec<RigidBodyHandle>, // Workspace.
#[cfg_attr(
feature = "serde-serialize",
serde(skip, default = "crossbeam::channel::unbounded")
)]
activation_channel: (Sender<RigidBodyHandle>, Receiver<RigidBodyHandle>),
}
impl RigidBodySet {
@@ -111,9 +63,10 @@ impl RigidBodySet {
modified_inactive_set: Vec::new(),
active_islands: Vec::new(),
active_set_timestamp: 0,
modified_bodies: Vec::new(),
modified_all_bodies: false,
can_sleep: Vec::new(),
stack: Vec::new(),
activation_channel: crossbeam::channel::unbounded(),
}
}
@@ -127,28 +80,6 @@ impl RigidBodySet {
self.bodies.len()
}
pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
let mut rb = &mut self.bodies[handle];
match rb.body_status {
// XXX: this case should only concern the dynamic bodies.
// For static bodies we should use the modified_inactive_set, or something
// similar. Right now we do this for static bodies as well so the broad-phase
// takes them into account the first time they are inserted.
BodyStatus::Dynamic | BodyStatus::Static => {
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
BodyStatus::Kinematic => {
if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
}
}
}
/// Is the given body handle valid?
pub fn contains(&self, handle: RigidBodyHandle) -> bool {
self.bodies.contains(handle)
@@ -159,24 +90,18 @@ impl RigidBodySet {
// Make sure the internal links are reset, they may not be
// if this rigid-body was obtained by cloning another one.
rb.reset_internal_references();
rb.changes.set(RigidBodyChanges::all(), true);
let handle = self.bodies.insert(rb);
let rb = &mut self.bodies[handle];
self.modified_bodies.push(handle);
if !rb.is_sleeping() && rb.is_dynamic() {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
let rb = &mut self.bodies[handle];
if rb.is_kinematic() {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
if !rb.is_dynamic() {
self.modified_inactive_set.push(handle);
}
handle
}
@@ -262,11 +187,13 @@ impl RigidBodySet {
///
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not
/// suffer form the ABA problem.
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> {
let sender = &self.activation_channel.0;
self.bodies
.get_unknown_gen_mut(i)
.map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
let result = self.bodies.get_unknown_gen_mut(i)?;
if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
result.0.changes = RigidBodyChanges::MODIFIED;
self.modified_bodies.push(result.1);
}
Some(result)
}
/// Gets the rigid-body with the given handle.
@@ -275,11 +202,13 @@ impl RigidBodySet {
}
/// Gets a mutable reference to the rigid-body with the given handle.
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<RigidBodyMut> {
let sender = &self.activation_channel.0;
self.bodies
.get_mut(handle)
.map(|rb| RigidBodyMut::new(handle, rb, sender))
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle)?;
if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
result.changes = RigidBodyChanges::MODIFIED;
self.modified_bodies.push(handle);
}
Some(result)
}
pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
@@ -300,11 +229,10 @@ impl RigidBodySet {
}
/// Iterates mutably through all the rigid-bodies on this set.
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyMut)> {
let sender = &self.activation_channel.0;
self.bodies
.iter_mut()
.map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
self.modified_bodies.clear();
self.modified_all_bodies = true;
self.bodies.iter_mut()
}
/// Iter through all the active kinematic rigid-bodies on this set.
@@ -433,17 +361,70 @@ impl RigidBodySet {
&self.active_dynamic_set[self.active_island_range(island_id)]
}
pub(crate) fn maintain_active_set(&mut self) {
for handle in self.activation_channel.1.try_iter() {
if let Some(rb) = self.bodies.get_mut(handle) {
// Push the body to the active set if it is not
// sleeping and if it is not already inside of the active set.
if !rb.is_sleeping() // May happen if the body was put to sleep manually.
&& rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
&& self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
self.active_dynamic_set.push(handle);
// Utility function to avoid some borrowing issue in the `maintain` method.
fn maintain_one(
colliders: &mut ColliderSet,
handle: RigidBodyHandle,
rb: &mut RigidBody,
modified_inactive_set: &mut Vec<RigidBodyHandle>,
active_kinematic_set: &mut Vec<RigidBodyHandle>,
active_dynamic_set: &mut Vec<RigidBodyHandle>,
) {
// Update the positions of the colliders.
if rb.changes.contains(RigidBodyChanges::POSITION)
|| rb.changes.contains(RigidBodyChanges::COLLIDERS)
{
rb.update_colliders_positions(colliders);
if rb.is_static() {
modified_inactive_set.push(handle);
}
if rb.is_kinematic() && active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = active_kinematic_set.len();
active_kinematic_set.push(handle);
}
}
// Push the body to the active set if it is not
// sleeping and if it is not already inside of the active set.
if rb.changes.contains(RigidBodyChanges::SLEEP)
&& !rb.is_sleeping() // May happen if the body was put to sleep manually.
&& rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
&& active_dynamic_set.get(rb.active_set_id) != Some(&handle)
{
rb.active_set_id = active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
active_dynamic_set.push(handle);
}
rb.changes = RigidBodyChanges::empty();
}
pub(crate) fn maintain(&mut self, colliders: &mut ColliderSet) {
if self.modified_all_bodies {
for (handle, rb) in self.bodies.iter_mut() {
Self::maintain_one(
colliders,
handle,
rb,
&mut self.modified_inactive_set,
&mut self.active_kinematic_set,
&mut self.active_dynamic_set,
)
}
self.modified_bodies.clear();
} else {
for handle in self.modified_bodies.drain(..) {
if let Some(rb) = self.bodies.get_mut(handle) {
Self::maintain_one(
colliders,
handle,
rb,
&mut self.modified_inactive_set,
&mut self.active_kinematic_set,
&mut self.active_dynamic_set,
)
}
}
}

2
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -250,7 +250,7 @@ impl ParallelIslandSolver {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies[*handle];
rb.set_position(positions[rb.active_set_offset], false);
rb.set_position_internal(positions[rb.active_set_offset]);
}
}
})

2
src/dynamics/solver/position_solver.rs
View File

@@ -120,7 +120,7 @@ impl PositionSolver {
}
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.set_position(self.positions[rb.active_set_offset], false)
rb.set_position_internal(self.positions[rb.active_set_offset])
});
}
}

9
src/geometry/broad_phase_multi_sap.rs
View File

@@ -586,9 +586,6 @@ impl BroadPhase {
let proxy = &mut self.proxies[proxy_index];
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
// Discretize the AABB to find the regions that need to be invalidated.
let start = point_key(proxy.aabb.mins);
let end = point_key(proxy.aabb.maxs);
@@ -615,6 +612,9 @@ impl BroadPhase {
}
}
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
self.proxies.remove(proxy_index);
}
@@ -631,8 +631,9 @@ impl BroadPhase {
self.complete_removals();
for body_handle in bodies
.active_dynamic_set
.modified_inactive_set
.iter()
.chain(bodies.active_dynamic_set.iter())
.chain(bodies.active_kinematic_set.iter())
{
for handle in &bodies[*body_handle].colliders {

4
src/geometry/collider.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh,
Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle,
Trimesh,
};
#[cfg(feature = "dim3")]
use crate::geometry::{Cone, Cylinder, RoundCylinder};

28
src/geometry/collider_set.rs
View File

@@ -1,7 +1,7 @@
use crate::data::arena::Arena;
use crate::data::pubsub::PubSub;
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::{Collider, ColliderGraphIndex};
use crate::geometry::Collider;
use std::ops::{Index, IndexMut};
/// The unique identifier of a collider added to a collider set.
@@ -63,15 +63,17 @@ impl ColliderSet {
coll.reset_internal_references();
coll.parent = parent_handle;
// NOTE: we use `get_mut` instead of `get_mut_internal` so that the
// modification flag is updated properly.
let parent = bodies
.get_mut_internal(parent_handle)
.get_mut(parent_handle)
.expect("Parent rigid body not found.");
coll.position = parent.position * coll.delta;
coll.predicted_position = parent.predicted_position * coll.delta;
let handle = self.colliders.insert(coll);
let coll = self.colliders.get(handle).unwrap();
parent.add_collider_internal(handle, &coll);
bodies.activate(parent_handle);
parent.add_collider(handle, &coll);
handle
}
@@ -90,7 +92,9 @@ impl ColliderSet {
/*
* Delete the collider from its parent body.
*/
if let Some(parent) = bodies.get_mut_internal(collider.parent) {
// NOTE: we use `get_mut` instead of `get_mut_internal` so that the
// modification flag is updated properly.
if let Some(parent) = bodies.get_mut(collider.parent) {
parent.remove_collider_internal(handle, &collider);
if wake_up {
@@ -147,13 +151,13 @@ impl ColliderSet {
self.colliders.get_mut(handle)
}
pub(crate) fn get2_mut_internal(
&mut self,
h1: ColliderHandle,
h2: ColliderHandle,
) -> (Option<&mut Collider>, Option<&mut Collider>) {
self.colliders.get2_mut(h1, h2)
}
// pub(crate) fn get2_mut_internal(
// &mut self,
// h1: ColliderHandle,
// h2: ColliderHandle,
// ) -> (Option<&mut Collider>, Option<&mut Collider>) {
// self.colliders.get2_mut(h1, h2)
// }
// pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, ColliderMut)> {
// // let sender = &self.activation_channel_sender;

2
src/pipeline/collision_pipeline.rs
View File

@@ -47,7 +47,7 @@ impl CollisionPipeline {
proximity_pair_filter: Option<&dyn ProximityPairFilter>,
events: &dyn EventHandler,
) {
bodies.maintain_active_set();
bodies.maintain(colliders);
self.broadphase_collider_pairs.clear();
broad_phase.update_aabbs(prediction_distance, bodies, colliders);

8
src/pipeline/physics_pipeline.rs
View File

@@ -74,9 +74,9 @@ impl PhysicsPipeline {
events: &dyn EventHandler,
) {
self.counters.step_started();
bodies.maintain(colliders);
broad_phase.maintain(colliders);
narrow_phase.maintain(colliders, bodies);
bodies.maintain_active_set();
// Update kinematic bodies velocities.
// TODO: what is the best place for this? It should at least be
@@ -242,11 +242,7 @@ impl PhysicsPipeline {
rb.update_predicted_position(integration_parameters.dt());
}
for handle in &rb.colliders {
let collider = &mut colliders[*handle];
collider.position = rb.position * collider.delta;
collider.predicted_position = rb.predicted_position * collider.delta;
}
rb.update_colliders_positions(colliders);
});
self.counters.stages.solver_time.pause();

19
src_testbed/engine.rs
View File

@@ -237,7 +237,7 @@ impl GraphicsManager {
for collider_handle in bodies[handle].colliders() {
let color = self.c2color.get(collider_handle).copied().unwrap_or(color);
let collider = &colliders[*collider_handle];
self.add_collider(window, *collider_handle, collider, color, &mut new_nodes);
self.do_add_collider(window, *collider_handle, collider, color, &mut new_nodes);
}
new_nodes.iter_mut().for_each(|n| n.update(colliders));
@@ -256,7 +256,22 @@ impl GraphicsManager {
nodes.append(&mut new_nodes);
}
fn add_collider(
pub fn add_collider(
&mut self,
window: &mut Window,
handle: ColliderHandle,
colliders: &ColliderSet,
) {
let collider = &colliders[handle];
let color = *self.b2color.get(&collider.parent()).unwrap();
let color = self.c2color.get(&handle).copied().unwrap_or(color);
let mut nodes =
std::mem::replace(self.b2sn.get_mut(&collider.parent()).unwrap(), Vec::new());
self.do_add_collider(window, handle, collider, color, &mut nodes);
self.b2sn.insert(collider.parent(), nodes);
}
fn do_add_collider(
&mut self,
window: &mut Window,
handle: ColliderHandle,

2
src_testbed/physx_backend.rs
View File

@@ -402,7 +402,7 @@ impl PhysxWorld {
pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
for (rapier_handle, physx_handle) in self.rapier2physx.iter() {
let mut rb = bodies.get_mut(*rapier_handle).unwrap();
let rb = bodies.get_mut(*rapier_handle).unwrap();
let ra = self.scene.get_rigid_actor(*physx_handle).unwrap();
let pos = ra.get_global_pose().into_na();
let iso = na::convert_unchecked(pos);