From 3c85a6ac41397cf95199933c6a93909bc070a844 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 8 Sep 2020 21:18:17 +0200 Subject: Start implementing ray-casting. This adds a QueryPipeline structure responsible for scene queries. Currently this structure is able to perform a brute-force ray-cast. This commit also includes the beginning of implementation of a SIMD-based acceleration structure which will be used for these scene queries in the future. --- src/dynamics/rigid_body.rs | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index a2fcacc..d32ea46 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -137,6 +137,15 @@ impl RigidBody { crate::utils::inv(self.mass_properties.inv_mass) } + /// The predicted position of this rigid-body. + /// + /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position` + /// method and is used for estimating the kinematic body velocity at the next timestep. + /// For non-kinematic bodies, this value is currently unspecified. + pub fn predicted_position(&self) -> &Isometry { + &self.predicted_position + } + /// Adds a collider to this rigid-body. pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) { let mass_properties = coll @@ -201,12 +210,17 @@ impl RigidBody { self.position = self.integrate_velocity(dt) * self.position; } - /// Sets the position of this rigid body. + /// Sets the position and `next_kinematic_position` of this rigid body. + /// + /// This will teleport the rigid-body to the specified position/orientation, + /// completely ignoring any physics rule. If this body is kinematic, this will + /// also set the next kinematic position to the same value, effectively + /// resetting to zero the next interpolated velocity of the kinematic body. pub fn set_position(&mut self, pos: Isometry) { self.position = pos; // TODO: update the predicted position for dynamic bodies too? - if self.is_static() { + if self.is_static() || self.is_kinematic() { self.predicted_position = pos; } } -- cgit From e16b7722be23f7b6627bd54e174d7782d33c53fe Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Sep 2020 22:22:55 +0200 Subject: Fix crash caused by the removal of a kinematic body. --- src/dynamics/rigid_body_set.rs | 46 +++++++++++++++++++++++++++++++----------- 1 file changed, 34 insertions(+), 12 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 99d6f10..14a6471 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,7 +2,7 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::dynamics::{Joint, RigidBody}; +use crate::dynamics::{BodyStatus, Joint, RigidBody}; use crate::geometry::{ColliderSet, ContactPair, InteractionGraph}; use crossbeam::channel::{Receiver, Sender}; use std::ops::{Deref, DerefMut, Index, IndexMut}; @@ -128,9 +128,23 @@ impl RigidBodySet { pub(crate) fn activate(&mut self, handle: RigidBodyHandle) { let mut rb = &mut self.bodies[handle]; - 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); + 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); + } + } } } @@ -143,13 +157,14 @@ impl RigidBodySet { pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle { let handle = self.bodies.insert(rb); let rb = &mut self.bodies[handle]; - rb.active_set_id = self.active_dynamic_set.len(); if !rb.is_sleeping() && rb.is_dynamic() { + rb.active_set_id = self.active_dynamic_set.len(); self.active_dynamic_set.push(handle); } if rb.is_kinematic() { + rb.active_set_id = self.active_kinematic_set.len(); self.active_kinematic_set.push(handle); } @@ -166,12 +181,15 @@ impl RigidBodySet { pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option { let rb = self.bodies.remove(handle)?; - // Remove this body from the active dynamic set. - if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) { - self.active_dynamic_set.swap_remove(rb.active_set_id); + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + + for active_set in &mut active_sets { + if active_set.get(rb.active_set_id) == Some(&handle) { + active_set.swap_remove(rb.active_set_id); - if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) { - self.bodies[*replacement].active_set_id = rb.active_set_id; + if let Some(replacement) = active_set.get(rb.active_set_id) { + self.bodies[*replacement].active_set_id = rb.active_set_id; + } } } @@ -181,6 +199,7 @@ impl RigidBodySet { /// Forces the specified rigid-body to wake up if it is dynamic. pub fn wake_up(&mut self, handle: RigidBodyHandle) { if let Some(rb) = self.bodies.get_mut(handle) { + // TODO: what about kinematic bodies? if rb.is_dynamic() { rb.wake_up(); @@ -214,8 +233,11 @@ 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<(&mut RigidBody, RigidBodyHandle)> { - self.bodies.get_unknown_gen_mut(i) + 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)) } /// Gets the rigid-body with the given handle. -- cgit From 7b8e322446ffa36e3f47078e23eb61ef423175dc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 21 Sep 2020 10:43:20 +0200 Subject: Make kinematic bodies properly wake up dynamic bodies. --- src/dynamics/joint/joint_set.rs | 4 +-- src/dynamics/rigid_body.rs | 16 +++++++-- src/dynamics/rigid_body_set.rs | 74 +++++++++++++++++++++++++++++------------ 3 files changed, 69 insertions(+), 25 deletions(-) (limited to 'src/dynamics') 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 { 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, + stack: &mut Vec, + ) { + 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); -- cgit From a7d77a01447d2b77694b2a957d000790af60b383 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 22 Sep 2020 15:29:29 +0200 Subject: Add non-topological WQuadtree update. --- src/dynamics/rigid_body_set.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 3970d54..f54bc55 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -282,6 +282,16 @@ impl RigidBodySet { .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender))) } + /// Iter through all the active kinematic rigid-bodies on this set. + pub fn iter_active_kinematic<'a>( + &'a self, + ) -> impl Iterator { + let bodies: &'a _ = &self.bodies; + self.active_kinematic_set + .iter() + .filter_map(move |h| Some((*h, bodies.get(*h)?))) + } + /// Iter through all the active dynamic rigid-bodies on this set. pub fn iter_active_dynamic<'a>( &'a self, -- cgit From d8dfd864a453a9ead60f970549b2d289d7025364 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 16:51:16 +0200 Subject: Joint set iteration: also yield the joint handle. --- src/dynamics/joint/joint_set.rs | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 90f5190..2b1895f 100644 --- a/src/dynamics/joint/joint_set.rs +++ b/src/dynamics/joint/joint_set.rs @@ -66,17 +66,21 @@ impl JointSet { } /// Iterates through all the joint on this set. - pub fn iter(&self) -> impl Iterator { - self.joint_graph.graph.edges.iter().map(|e| &e.weight) + pub fn iter(&self) -> impl Iterator { + self.joint_graph + .graph + .edges + .iter() + .map(|e| (e.weight.handle, &e.weight)) } /// Iterates mutably through all the joint on this set. - pub fn iter_mut(&mut self) -> impl Iterator { + pub fn iter_mut(&mut self) -> impl Iterator { self.joint_graph .graph .edges .iter_mut() - .map(|e| &mut e.weight) + .map(|e| (e.weight.handle, &mut e.weight)) } // /// The set of joints as an array. -- cgit From 2d0a888484dd296cc785caf978252dd97b58e10a Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 16:51:50 +0200 Subject: Make the query pipeline serializable. --- src/dynamics/integration_parameters.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/dynamics') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index faf22e1..b31c3f6 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -2,7 +2,7 @@ #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct IntegrationParameters { - /// The timestep (default: `1.0 / 60.0`) + /// The timestep length (default: `1.0 / 60.0`) dt: f32, /// The inverse of `dt`. inv_dt: f32, -- cgit From 93aa7b6e1e8cbfd73542ed10ad5c26ae0a8b9848 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 5 Oct 2020 19:04:18 +0200 Subject: Use the publish-subscribe mechanism to handle collider removals across pipelines. --- src/dynamics/rigid_body_set.rs | 34 ++++++++++++++++++++++++++++------ 1 file changed, 28 insertions(+), 6 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index f54bc55..7906083 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,7 +2,8 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::dynamics::{BodyStatus, Joint, RigidBody}; +use crate::data::pubsub::PubSub; +use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody}; use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph}; use crossbeam::channel::{Receiver, Sender}; use num::Zero; @@ -176,12 +177,17 @@ impl RigidBodySet { handle } - pub(crate) fn num_islands(&self) -> usize { - self.active_islands.len() - 1 - } - - pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option { + /// Removes a rigid-body, and all its attached colliders and joints, from these sets. + pub fn remove( + &mut self, + handle: RigidBodyHandle, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ) -> Option { let rb = self.bodies.remove(handle)?; + /* + * Update active sets. + */ let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; for active_set in &mut active_sets { @@ -194,9 +200,25 @@ impl RigidBodySet { } } + /* + * Remove colliders attached to this rigid-body. + */ + for collider in &rb.colliders { + colliders.remove(*collider, self); + } + + /* + * Remove joints attached to this rigid-body. + */ + joints.remove_rigid_body(rb.joint_graph_index, self); + Some(rb) } + pub(crate) fn num_islands(&self) -> usize { + self.active_islands.len() - 1 + } + /// Forces the specified rigid-body to wake up if it is dynamic. /// /// If `strong` is `true` then it is assured that the rigid-body will -- cgit From 682ff61f94931ef205a9f81e7d00417ac88537c1 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 6 Oct 2020 15:23:48 +0200 Subject: Don't let the PubSub internal offsets overflow + fix some warnings. --- src/dynamics/rigid_body_set.rs | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 7906083..83f1c51 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,11 +2,9 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::data::pubsub::PubSub; use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody}; 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. -- cgit