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

Properly track some user-initiatied rigid-body modifications.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-11-25 16:00:02 +01:00
parent 72eb66425d
commit 2d4e14b869
11 changed files with 159 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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")]

54
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 = 0b001;
const POSITION = 0b010;
const SLEEP = 0b100;
const COLLIDERS = 0b1000;
}
}
#[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,14 @@ 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) {
if !self.changes.contains(RigidBodyChanges::MODIFIED) {
self.changes.set(
RigidBodyChanges::MODIFIED | RigidBodyChanges::COLLIDERS,
true,
);
}
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
@@ -160,6 +182,14 @@ 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) {
@@ -189,7 +219,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 +334,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 +649,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])
});
}
}