diff options
| -rw-r--r-- | examples3d/platform3.rs | 6 | ||||
| -rw-r--r-- | src/dynamics/joint/joint_set.rs | 4 | ||||
| -rw-r--r-- | src/dynamics/rigid_body.rs | 16 | ||||
| -rw-r--r-- | src/dynamics/rigid_body_set.rs | 74 | ||||
| -rw-r--r-- | src/geometry/narrow_phase.rs | 17 | ||||
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 2 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 49 | ||||
| -rw-r--r-- | src_testbed/engine.rs | 16 | ||||
| -rw-r--r-- | src_testbed/testbed.rs | 38 |
9 files changed, 168 insertions, 54 deletions
diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index bb71b76..6c3483e 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -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; diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 51d7432..90f5190 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -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) { diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index d32ea46..af1fb4a 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -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); diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 14a6471..3970d54 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -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); diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 1a36511..016da33 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -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; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 0184295..f30dae7 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -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); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 192ca9a..3fcd1af 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -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) @@ -304,6 +308,37 @@ mod test { 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(); let mut joints = JointSet::new(); diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 62ab37e..27a8510 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -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); } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index ba28a4b..6c03ef4 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -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 ); } |