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

Make kinematic bodies properly wake up dynamic bodies.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-09-21 10:43:20 +02:00
parent e16b7722be
commit 7b8e322446
9 changed files with 168 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples3d/platform3.rs
View File

@@ -64,7 +64,13 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Setup a callback to control the platform.
*/
let mut count = 0;
testbed.add_callback(move |_, physics, _, _, time| {
count += 1;
if count % 100 > 50 {
return;
}
if let Some(mut platform) = physics.bodies.get_mut(platform_handle) {
let mut next_pos = platform.position;

4
src/dynamics/joint/joint_set.rs
View File

@@ -202,8 +202,8 @@ impl JointSet {
}
// Wake up the attached bodies.
bodies.wake_up(h1);
bodies.wake_up(h2);
bodies.wake_up(h1, true);
bodies.wake_up(h2, true);
}
if let Some(other) = self.joint_graph.remove_node(deleted_id) {

16
src/dynamics/rigid_body.rs
View File

@@ -181,10 +181,13 @@ impl RigidBody {
}
/// Wakes up this rigid body if it is sleeping.
pub fn wake_up(&mut self) {
///
/// 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.energy == 0.0 && self.is_dynamic() {
if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
self.activation.energy = self.activation.threshold.abs() * 2.0;
}
}
@@ -198,9 +201,18 @@ impl RigidBody {
/// Is this rigid body sleeping?
pub fn is_sleeping(&self) -> bool {
// TODO: should we:
// - return false for static bodies.
// - return true for non-sleeping dynamic bodies.
// - return true only for kinematic bodies with non-zero velocity?
self.activation.sleeping
}
/// Is the velocity of this body not zero?
pub fn is_moving(&self) -> bool {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
let com = &self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);

74
src/dynamics/rigid_body_set.rs
View File

@@ -3,8 +3,9 @@ use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, RigidBody};
use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
use crossbeam::channel::{Receiver, Sender};
use num::Zero;
use std::ops::{Deref, DerefMut, Index, IndexMut};
/// A mutable reference to a rigid-body.
@@ -197,11 +198,14 @@ impl RigidBodySet {
}
/// Forces the specified rigid-body to wake up if it is dynamic.
pub fn wake_up(&mut self, handle: RigidBodyHandle) {
///
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
if let Some(rb) = self.bodies.get_mut(handle) {
// TODO: what about kinematic bodies?
if rb.is_dynamic() {
rb.wake_up();
rb.wake_up(strong);
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
@@ -447,6 +451,45 @@ impl RigidBodySet {
}
}
// Read all the contacts and push objects touching touching this rigid-body.
#[inline(always)]
fn push_contacting_colliders(
rb: &RigidBody,
colliders: &ColliderSet,
contact_graph: &InteractionGraph<ContactPair>,
stack: &mut Vec<ColliderHandle>,
) {
for collider_handle in &rb.colliders {
let collider = &colliders[*collider_handle];
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other =
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
let other_body = colliders[other].parent;
stack.push(other_body);
break;
}
}
}
}
}
// Now iterate on all active kinematic bodies and push all the bodies
// touching them to the stack so they can be woken up.
for h in self.active_kinematic_set.iter() {
let rb = &self.bodies[*h];
if !rb.is_moving() {
// If the kinematic body does not move, it does not have
// to wake up any dynamic body.
continue;
}
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
}
// println!("Selection: {}", instant::now() - t);
// let t = instant::now();
@@ -465,7 +508,9 @@ impl RigidBodySet {
// We already visited this body and its neighbors.
// Also, we don't propagate awake state through static bodies.
continue;
} else if self.stack.len() < island_marker {
}
if self.stack.len() < island_marker {
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
@@ -476,29 +521,16 @@ impl RigidBodySet {
island_marker = self.stack.len();
}
rb.wake_up();
rb.wake_up(false);
rb.active_island_id = self.active_islands.len() - 1;
rb.active_set_id = self.active_dynamic_set.len();
rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
rb.active_set_timestamp = self.active_set_timestamp;
self.active_dynamic_set.push(handle);
// Read all the contacts and push objects touching this one.
for collider_handle in &rb.colliders {
let collider = &colliders[*collider_handle];
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other =
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
let other_body = colliders[other].parent;
self.stack.push(other_body);
break;
}
}
}
}
// Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider.
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::other_handle((inter.0, inter.1), handle);

17
src/geometry/narrow_phase.rs
View File

@@ -87,11 +87,11 @@ impl NarrowPhase {
// Wake up every body in contact with the deleted collider.
for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
if let Some(parent) = colliders.get(a).map(|c| c.parent) {
bodies.wake_up(parent)
bodies.wake_up(parent, true)
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent)
bodies.wake_up(parent, true)
}
}
@@ -119,6 +119,7 @@ impl NarrowPhase {
pub(crate) fn register_pairs(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
broad_phase_events: &[BroadPhasePairEvent],
events: &dyn EventHandler,
) {
@@ -218,9 +219,13 @@ impl NarrowPhase {
.contact_graph
.remove_edge(co1.contact_graph_index, co2.contact_graph_index);
// Emit a contact stopped event if we had a proximity before removing the edge.
// 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(ctct) = contact_pair {
if ctct.has_any_active_contact() {
bodies.wake_up(co1.parent, true);
bodies.wake_up(co2.parent, true);
events.handle_contact_event(ContactEvent::Stopped(
pair.collider1,
pair.collider2,
@@ -250,8 +255,7 @@ impl NarrowPhase {
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
{
if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) {
// No need to update this contact because nothing moved.
return;
}
@@ -359,7 +363,8 @@ impl NarrowPhase {
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()))
|| (!rb1.is_dynamic() && !rb2.is_dynamic())
{
// No need to update this contact because nothing moved.
return;

2
src/pipeline/collision_pipeline.rs
View File

@@ -50,7 +50,7 @@ impl CollisionPipeline {
self.broad_phase_events.clear();
broad_phase.find_pairs(&mut self.broad_phase_events);
narrow_phase.register_pairs(colliders, &self.broad_phase_events, events);
narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events);
narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events);

49
src/pipeline/physics_pipeline.rs
View File

@@ -75,6 +75,15 @@ impl PhysicsPipeline {
self.counters.step_started();
bodies.maintain_active_set();
// Update kinematic bodies velocities.
// TODO: what is the best place for this? It should at least be
// located before the island computation because we test the velocity
// there to determine if this kinematic body should wake-up dynamic
// bodies it is touching.
bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
});
self.counters.stages.collision_detection_time.start();
self.counters.cd.broad_phase_time.start();
self.broadphase_collider_pairs.clear();
@@ -91,7 +100,7 @@ impl PhysicsPipeline {
broad_phase.find_pairs(&mut self.broad_phase_events);
// println!("Find pairs time: {}", instant::now() - t);
narrow_phase.register_pairs(colliders, &self.broad_phase_events, events);
narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
self.counters.cd.broad_phase_time.pause();
// println!("Num contact pairs: {}", pairs.len());
@@ -122,11 +131,6 @@ impl PhysicsPipeline {
);
self.counters.stages.island_construction_time.pause();
// Update kinematic bodies velocities.
bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
});
if self.manifold_indices.len() < bodies.num_islands() {
self.manifold_indices
.resize(bodies.num_islands(), Vec::new());
@@ -261,7 +265,7 @@ impl PhysicsPipeline {
if let Some(parent) = bodies.get_mut_internal(collider.parent) {
parent.remove_collider_internal(handle, &collider);
bodies.wake_up(collider.parent);
bodies.wake_up(collider.parent, true);
}
Some(collider)
@@ -303,6 +307,37 @@ mod test {
use crate::math::Vector;
use crate::pipeline::PhysicsPipeline;
#[test]
fn kinematic_and_static_contact_crash() {
let mut colliders = ColliderSet::new();
let mut joints = JointSet::new();
let mut pipeline = PhysicsPipeline::new();
let mut bf = BroadPhase::new();
let mut nf = NarrowPhase::new();
let mut bodies = RigidBodySet::new();
let rb = RigidBodyBuilder::new_static().build();
let h1 = bodies.insert(rb.clone());
let co = ColliderBuilder::ball(10.0).build();
colliders.insert(co.clone(), h1, &mut bodies);
// The same but with a kinematic body.
let rb = RigidBodyBuilder::new_kinematic().build();
let h2 = bodies.insert(rb.clone());
colliders.insert(co, h2, &mut bodies);
pipeline.step(
&Vector::zeros(),
&IntegrationParameters::default(),
&mut bf,
&mut nf,
&mut bodies,
&mut colliders,
&mut joints,
&(),
);
}
#[test]
fn rigid_body_removal_before_step() {
let mut colliders = ColliderSet::new();

16
src_testbed/engine.rs
View File

@@ -609,7 +609,7 @@ impl GraphicsManager {
}
}
pub fn draw(&mut self, colliders: &ColliderSet, window: &mut Window) {
pub fn draw(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, window: &mut Window) {
// use kiss3d::camera::Camera;
// println!(
// "camera eye {:?}, at: {:?}",
@@ -618,6 +618,20 @@ impl GraphicsManager {
// );
for (_, ns) in self.b2sn.iter_mut() {
for n in ns.iter_mut() {
/*
if let Some(co) = colliders.get(n.collider()) {
let bo = &bodies[co.parent()];
if bo.is_dynamic() {
if bo.is_sleeping() {
n.set_color(Point3::new(1.0, 0.0, 0.0));
} else {
n.set_color(Point3::new(0.0, 1.0, 0.0));
}
}
}
*/
n.update(colliders);
n.draw(window);
}

38
src_testbed/testbed.rs
View File

@@ -824,7 +824,7 @@ impl Testbed {
.physics
.bodies
.iter()
.filter(|e| e.1.is_dynamic())
.filter(|e| !e.1.is_static())
.map(|e| e.0)
.collect();
let num_to_delete = (dynamic_bodies.len() / 10).max(1);
@@ -1367,7 +1367,7 @@ impl State for Testbed {
}
} else {
for (_, mut body) in self.physics.bodies.iter_mut() {
body.wake_up();
body.wake_up(true);
body.activation.threshold = -1.0;
}
}
@@ -1571,7 +1571,8 @@ impl State for Testbed {
}
self.highlight_hovered_body(window);
self.graphics.draw(&self.physics.colliders, window);
self.graphics
.draw(&self.physics.bodies, &self.physics.colliders, window);
#[cfg(feature = "fluids")]
{
@@ -1648,26 +1649,35 @@ Fluids: {:.2}ms
}
if self.state.flags.contains(TestbedStateFlags::DEBUG) {
let hash_bf = md5::compute(&bincode::serialize(&self.physics.broad_phase).unwrap());
let hash_nf = md5::compute(&bincode::serialize(&self.physics.narrow_phase).unwrap());
let hash_bodies = md5::compute(&bincode::serialize(&self.physics.bodies).unwrap());
let hash_colliders =
md5::compute(&bincode::serialize(&self.physics.colliders).unwrap());
let hash_joints = md5::compute(&bincode::serialize(&self.physics.joints).unwrap());
let bf = bincode::serialize(&self.physics.broad_phase).unwrap();
let nf = bincode::serialize(&self.physics.narrow_phase).unwrap();
let bs = bincode::serialize(&self.physics.bodies).unwrap();
let cs = bincode::serialize(&self.physics.colliders).unwrap();
let js = bincode::serialize(&self.physics.joints).unwrap();
let hash_bf = md5::compute(&bf);
let hash_nf = md5::compute(&nf);
let hash_bodies = md5::compute(&bs);
let hash_colliders = md5::compute(&cs);
let hash_joints = md5::compute(&js);
profile = format!(
r#"{}
Hashes at frame: {}
|_ Broad phase: {:?}
|_ Narrow phase: {:?}
|_ Bodies: {:?}
|_ Colliders: {:?}
|_ Joints: {:?}"#,
|_ Broad phase [{:.1}KB]: {:?}
|_ Narrow phase [{:.1}KB]: {:?}
|_ Bodies [{:.1}KB]: {:?}
|_ Colliders [{:.1}KB]: {:?}
|_ Joints [{:.1}KB]: {:?}"#,
profile,
self.state.timestep_id,
bf.len() as f32 / 1000.0,
hash_bf,
nf.len() as f32 / 1000.0,
hash_nf,
bs.len() as f32 / 1000.0,
hash_bodies,
cs.len() as f32 / 1000.0,
hash_colliders,
js.len() as f32 / 1000.0,
hash_joints
);
}