diff --git a/.github/workflows/rapier-ci-bench.yml b/.github/workflows/rapier-ci-bench.yml index 89ed247..9cce1b2 100644 --- a/.github/workflows/rapier-ci-bench.yml +++ b/.github/workflows/rapier-ci-bench.yml @@ -9,20 +9,23 @@ on: jobs: send-bench-message: + env: + BENCHBOT_AMQP_USER: ${{ secrets.BENCHBOT_AMQP_USER }} + BENCHBOT_AMQP_PASS: ${{ secrets.BENCHBOT_AMQP_PASS }} + BENCHBOT_AMQP_VHOST: ${{ secrets.BENCHBOT_AMQP_VHOST }} + BENCHBOT_AMQP_HOST: ${{ secrets.BENCHBOT_AMQP_HOST }} + BENCHBOT_TARGET_REPO: ${{ github.repository }} + BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }} + BENCHBOT_SHA: ${{ github.sha }} + BENCHBOT_HEAD_REF: ${{github.head_ref}} runs-on: ubuntu-latest steps: - - name: Get the latest commit on PR - id: get-latest-commit - uses: ActionsRML/get-PR-latest-commit@v1 + - name: Find commit SHA + if: github.ref == 'refs/heads/master' + run: | + echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA" - name: Send 3D bench message shell: bash - env: - BENCHBOT_AMQP_USER: ${{ secrets.BENCHBOT_AMQP_USER }} - BENCHBOT_AMQP_PASS: ${{ secrets.BENCHBOT_AMQP_PASS }} - BENCHBOT_AMQP_VHOST: ${{ secrets.BENCHBOT_AMQP_VHOST }} - BENCHBOT_AMQP_HOST: ${{ secrets.BENCHBOT_AMQP_HOST }} - BENCHBOT_TARGET_REPO: ${{ github.repository }} - BENCHBOT_TARGET_COMMIT: ${{ steps.get-latest-commit.outputs.latest_commit_sha }} run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS -i -H "content-type:application/json" -X POST https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish diff --git a/CHANGELOG b/CHANGELOG index 9e4731e..70b20e5 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -1,10 +1,15 @@ ## v0.2.0 - WIP +The most significant change on this version is the addition of the `QueryPipeline` responsible for performing +scene-wide queries. So far only ray-casting has been implemented. -- Add `PhysicsPipeline::remove_collider(...)` to remove a collider from the `ColliderSet`. +- Add `ColliderSet::remove(...)` to remove a collider from the `ColliderSet`. +- Replace `PhysicsPipeline::remove_rigid_body` by `RigidBodySet::remove`. +- The `JointSet.iter()` now returns an iterator yielding `(JointHandle, &Joint)` instead of just `&Joint`. - Add `ColliderDesc::translation(...)` to set the translation of a collider relative to the rigid-body it is attached to. - Add `ColliderDesc::rotation(...)` to set the rotation of a collider relative to the rigid-body it is attached to. - Add `ColliderDesc::position(...)` to set the position of a collider relative to the rigid-body it is attached to. - Add `Collider::position_wrt_parent()` to get the position of a collider relative to the rigid-body it is attached to. +- Modify `RigidBody::set_position(...)` so it also resets the next kinematic position to the same value. - Deprecate `Collider::delta()` in favor of the new `Collider::position_wrt_parent()`. - Fix multiple issues occurring when having colliders resulting in a non-zero center-of-mass. - Fix a crash happening when removing a rigid-body with a collider, stepping the simulation, adding another rigid-body diff --git a/Cargo.toml b/Cargo.toml index b4ce57e..944fa40 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,6 +4,7 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark [patch.crates-io] #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" } +#simba = { path = "../simba" } [profile.release] #debug = true diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index ec3a026..ff5253b 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -1,7 +1,7 @@ # Name idea: bident for 2D and trident for 3D [package] name = "rapier2d" -version = "0.1.5" +version = "0.2.0" authors = [ "Sébastien Crozet " ] description = "2-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier2d" @@ -37,7 +37,7 @@ instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.22" ncollide2d = "0.24" -simba = "0.2" +simba = "^0.2.1" approx = "0.3" rayon = { version = "1", optional = true } crossbeam = "0.7" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index dcf5aca..130e5dd 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -1,7 +1,7 @@ # Name idea: bident for 2D and trident for 3D [package] name = "rapier3d" -version = "0.1.5" +version = "0.2.0" authors = [ "Sébastien Crozet " ] description = "3-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier3d" @@ -37,7 +37,7 @@ instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.22" ncollide3d = "0.24" -simba = "0.2" +simba = "^0.2.1" approx = "0.3" rayon = { version = "1", optional = true } crossbeam = "0.7" diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 8be84aa..eeecb2a 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed2d" -version = "0.1.0" +version = "0.2.0" authors = [ "Sébastien Crozet " ] description = "Testbed for the 2-dimensional physics engine in Rust." homepage = "http://rapier.org" @@ -42,5 +42,5 @@ md5 = "0.7" [dependencies.rapier2d] path = "../rapier2d" -version = "0.1" +version = "0.2" features = [ "serde-serialize" ] diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index d349de7..7675701 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed3d" -version = "0.1.0" +version = "0.2.0" authors = [ "Sébastien Crozet " ] description = "Testbed for the 3-dimensional physics engine in Rust." homepage = "http://rapier.org" @@ -44,5 +44,5 @@ serde = { version = "1", features = [ "derive" ] } [dependencies.rapier3d] path = "../rapier3d" -version = "0.1" +version = "0.2" features = [ "serde-serialize" ] diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs index 3a32a95..67075fe 100644 --- a/examples2d/add_remove2.rs +++ b/examples2d/add_remove2.rs @@ -28,14 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { .map(|e| e.0) .collect(); for handle in to_remove { - physics.pipeline.remove_rigid_body( - handle, - &mut physics.broad_phase, - &mut physics.narrow_phase, - &mut physics.bodies, - &mut physics.colliders, - &mut physics.joints, - ); + physics + .bodies + .remove(handle, &mut physics.colliders, &mut physics.joints); graphics.remove_body_nodes(window, handle); } }); diff --git a/examples3d/add_remove3.rs b/examples3d/add_remove3.rs index 17aad44..6b58adf 100644 --- a/examples3d/add_remove3.rs +++ b/examples3d/add_remove3.rs @@ -28,14 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { .map(|e| e.0) .collect(); for handle in to_remove { - physics.pipeline.remove_rigid_body( - handle, - &mut physics.broad_phase, - &mut physics.narrow_phase, - &mut physics.bodies, - &mut physics.colliders, - &mut physics.joints, - ); + physics + .bodies + .remove(handle, &mut physics.colliders, &mut physics.joints); graphics.remove_body_nodes(window, handle); } }); diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index 3836baf..6c3483e 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -64,22 +64,29 @@ pub fn init_world(testbed: &mut Testbed) { /* * Setup a callback to control the platform. */ + let mut count = 0; testbed.add_callback(move |_, physics, _, _, time| { - let mut platform = physics.bodies.get_mut(platform_handle).unwrap(); - let mut next_pos = platform.position; - - let dt = 0.016; - next_pos.translation.vector.y += (time * 5.0).sin() * dt; - next_pos.translation.vector.z += time.sin() * 5.0 * dt; - - if next_pos.translation.vector.z >= rad * 10.0 { - next_pos.translation.vector.z -= dt; - } - if next_pos.translation.vector.z <= -rad * 10.0 { - next_pos.translation.vector.z += dt; + count += 1; + if count % 100 > 50 { + return; } - platform.set_next_kinematic_position(next_pos); + if let Some(mut platform) = physics.bodies.get_mut(platform_handle) { + let mut next_pos = platform.position; + + let dt = 0.016; + next_pos.translation.vector.y += (time * 5.0).sin() * dt; + next_pos.translation.vector.z += time.sin() * 5.0 * dt; + + if next_pos.translation.vector.z >= rad * 10.0 { + next_pos.translation.vector.z -= dt; + } + if next_pos.translation.vector.z <= -rad * 10.0 { + next_pos.translation.vector.z += dt; + } + + platform.set_next_kinematic_position(next_pos); + } }); /* diff --git a/src/data/mod.rs b/src/data/mod.rs index 7c00442..5d3efa6 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -2,3 +2,4 @@ pub mod arena; pub(crate) mod graph; +pub mod pubsub; diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs new file mode 100644 index 0000000..b2c9e27 --- /dev/null +++ b/src/data/pubsub.rs @@ -0,0 +1,175 @@ +//! Publish-subscribe mechanism for internal events. + +use std::collections::VecDeque; +use std::marker::PhantomData; + +/// A permanent subscription to a pub-sub queue. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Subscription { + // Position on the cursor array. + id: u32, + _phantom: PhantomData, +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct PubSubCursor { + // Position on the offset array. + id: u32, + // Index of the next message to read. + // NOTE: Having this here is not actually necessary because + // this value is supposed to be equal to `offsets[self.id]`. + // However, we keep it because it lets us avoid one lookup + // on the `offsets` array inside of message-polling loops + // based on `read_ith`. + next: u32, +} + +impl PubSubCursor { + fn id(&self, num_deleted: u32) -> usize { + (self.id - num_deleted) as usize + } + + fn next(&self, num_deleted: u32) -> usize { + (self.next - num_deleted) as usize + } +} + +/// A pub-sub queue. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct PubSub { + deleted_messages: u32, + deleted_offsets: u32, + messages: VecDeque, + offsets: VecDeque, + cursors: Vec, +} + +impl PubSub { + /// Create a new empty pub-sub queue. + pub fn new() -> Self { + Self { + deleted_offsets: 0, + deleted_messages: 0, + messages: VecDeque::new(), + offsets: VecDeque::new(), + cursors: Vec::new(), + } + } + + fn reset_shifts(&mut self) { + for offset in &mut self.offsets { + *offset -= self.deleted_messages; + } + + for cursor in &mut self.cursors { + cursor.id -= self.deleted_offsets; + cursor.next -= self.deleted_messages; + } + + self.deleted_offsets = 0; + self.deleted_messages = 0; + } + + /// Publish a new message. + pub fn publish(&mut self, message: T) { + if self.offsets.is_empty() { + // No subscribers, drop the message. + return; + } + + self.messages.push_back(message); + } + + /// Subscribe to the queue. + /// + /// A subscription cannot be cancelled. + #[must_use] + pub fn subscribe(&mut self) -> Subscription { + let cursor = PubSubCursor { + next: self.messages.len() as u32 + self.deleted_messages, + id: self.offsets.len() as u32 + self.deleted_offsets, + }; + + let subscription = Subscription { + id: self.cursors.len() as u32, + _phantom: PhantomData, + }; + + self.offsets.push_back(cursor.next); + self.cursors.push(cursor); + subscription + } + + /// Read the i-th message not yet read by the given subsciber. + pub fn read_ith(&self, sub: &Subscription, i: usize) -> Option<&T> { + let cursor = &self.cursors[sub.id as usize]; + self.messages + .get(cursor.next(self.deleted_messages) as usize + i) + } + + /// Get the messages not yet read by the given subscriber. + pub fn read(&self, sub: &Subscription) -> impl Iterator { + let cursor = &self.cursors[sub.id as usize]; + let next = cursor.next(self.deleted_messages); + + // TODO: use self.queue.range(next..) once it is stabilised. + MessageRange { + queue: &self.messages, + next, + } + } + + /// Makes the given subscribe acknowledge all the messages in the queue. + /// + /// A subscriber cannot read acknowledged messages any more. + pub fn ack(&mut self, sub: &Subscription) { + // Update the cursor. + let cursor = &mut self.cursors[sub.id as usize]; + + self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX; + cursor.id = self.offsets.len() as u32 + self.deleted_offsets; + + cursor.next = self.messages.len() as u32 + self.deleted_messages; + self.offsets.push_back(cursor.next); + + // Now clear the messages we don't need to + // maintain in memory anymore. + while self.offsets.front() == Some(&u32::MAX) { + self.offsets.pop_front(); + self.deleted_offsets += 1; + } + + // There must be at least one offset otherwise + // that would mean we have no subscribers. + let next = self.offsets.front().unwrap(); + let num_to_delete = *next - self.deleted_messages; + + for _ in 0..num_to_delete { + self.messages.pop_front(); + } + + self.deleted_messages += num_to_delete; + + if self.deleted_messages > u32::MAX / 2 || self.deleted_offsets > u32::MAX / 2 { + // Don't let the deleted_* shifts grow indefinitely otherwise + // they will end up overflowing, breaking everything. + self.reset_shifts(); + } + } +} + +struct MessageRange<'a, T> { + queue: &'a VecDeque, + next: usize, +} + +impl<'a, T> Iterator for MessageRange<'a, T> { + type Item = &'a T; + + #[inline(always)] + fn next(&mut self) -> Option<&'a T> { + let result = self.queue.get(self.next); + self.next += 1; + result + } +} 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, diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs index 51d7432..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. @@ -202,8 +206,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 a2fcacc..af1fb4a 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 @@ -172,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; } } @@ -189,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); @@ -201,12 +222,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; } } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 99d6f10..83f1c51 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -2,8 +2,8 @@ use rayon::prelude::*; use crate::data::arena::Arena; -use crate::dynamics::{Joint, RigidBody}; -use crate::geometry::{ColliderSet, ContactPair, InteractionGraph}; +use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody}; +use crate::geometry::{ColliderHandle, 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); } @@ -160,29 +175,57 @@ 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)?; - // 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); + /* + * Update active sets. + */ + let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; - if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) { - self.bodies[*replacement].active_set_id = rb.active_set_id; + 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) = active_set.get(rb.active_set_id) { + self.bodies[*replacement].active_set_id = rb.active_set_id; + } } } + /* + * 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. - 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(); @@ -214,8 +257,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. @@ -256,6 +302,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, @@ -425,6 +481,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(); @@ -443,7 +538,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 { @@ -454,29 +551,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/broad_phase.rs b/src/geometry/broad_phase.rs deleted file mode 100644 index a43b7af..0000000 --- a/src/geometry/broad_phase.rs +++ /dev/null @@ -1,255 +0,0 @@ -use crate::geometry::ColliderHandle; -use ncollide::bounding_volume::AABB; -#[cfg(feature = "simd-is-enabled")] -use { - crate::geometry::WAABB, - crate::math::{Point, SIMD_WIDTH}, - crate::utils::WVec, - simba::simd::SimdBool as _, -}; - -#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct ColliderPair { - pub collider1: ColliderHandle, - pub collider2: ColliderHandle, -} - -impl ColliderPair { - pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { - ColliderPair { - collider1, - collider2, - } - } - - pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { - if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { - Self::new(collider1, collider2) - } else { - Self::new(collider2, collider1) - } - } - - pub fn swap(self) -> Self { - Self::new(self.collider2, self.collider1) - } - - pub fn zero() -> Self { - Self { - collider1: ColliderHandle::from_raw_parts(0, 0), - collider2: ColliderHandle::from_raw_parts(0, 0), - } - } -} - -pub struct WAABBHierarchyIntersections { - curr_level_interferences: Vec, - next_level_interferences: Vec, -} - -impl WAABBHierarchyIntersections { - pub fn new() -> Self { - Self { - curr_level_interferences: Vec::new(), - next_level_interferences: Vec::new(), - } - } - - pub fn computed_interferences(&self) -> &[usize] { - &self.curr_level_interferences[..] - } - - pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec { - &mut self.curr_level_interferences - } -} - -#[cfg(feature = "simd-is-enabled")] -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct WAABBHierarchy { - levels: Vec>, -} - -#[cfg(feature = "simd-is-enabled")] -impl WAABBHierarchy { - pub fn new(aabbs: &[AABB]) -> Self { - let mut waabbs: Vec<_> = aabbs - .chunks_exact(SIMD_WIDTH) - .map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH])) - .collect(); - - if aabbs.len() % SIMD_WIDTH != 0 { - let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH; - let last_i = aabbs.len() - 1; - let last_waabb = - WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]); - waabbs.push(last_waabb); - } - - let mut levels = vec![waabbs]; - - loop { - let last_level = levels.last().unwrap(); - let mut next_level = Vec::new(); - - for chunk in last_level.chunks_exact(SIMD_WIDTH) { - let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]); - let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]); - next_level.push(WAABB::new(mins, maxs)); - } - - // Deal with the last non-exact chunk. - if last_level.len() % SIMD_WIDTH != 0 { - let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH; - let last_id = last_level.len() - 1; - let mins = array![|ii| last_level[(first_id + ii).min(last_id)] - .mins - .horizontal_inf(); SIMD_WIDTH]; - let maxs = array![|ii| last_level[(first_id + ii).min(last_id)] - .maxs - .horizontal_sup(); SIMD_WIDTH]; - - let mins = Point::from(mins); - let maxs = Point::from(maxs); - next_level.push(WAABB::new(mins, maxs)); - } - - if next_level.len() == 1 { - levels.push(next_level); - break; - } - - levels.push(next_level); - } - - Self { levels } - } - - pub fn compute_interferences_with( - &self, - aabb: AABB, - workspace: &mut WAABBHierarchyIntersections, - ) { - let waabb1 = WAABB::splat(aabb); - workspace.next_level_interferences.clear(); - workspace.curr_level_interferences.clear(); - workspace.curr_level_interferences.push(0); - - for level in self.levels.iter().rev() { - for i in &workspace.curr_level_interferences { - // This `if let` handle the case when `*i` is out of bounds because - // the initial number of aabbs was not a power of SIMD_WIDTH. - if let Some(waabb2) = level.get(*i) { - // NOTE: using `intersect.bitmask()` and performing bit comparisons - // is much more efficient than testing if each intersect.extract(i) is true. - let intersect = waabb1.intersects_lanewise(waabb2); - let bitmask = intersect.bitmask(); - - for j in 0..SIMD_WIDTH { - if (bitmask & (1 << j)) != 0 { - workspace.next_level_interferences.push(i * SIMD_WIDTH + j) - } - } - } - } - - std::mem::swap( - &mut workspace.curr_level_interferences, - &mut workspace.next_level_interferences, - ); - workspace.next_level_interferences.clear(); - } - } -} - -#[cfg(not(feature = "simd-is-enabled"))] -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct WAABBHierarchy { - levels: Vec>>, -} - -#[cfg(not(feature = "simd-is-enabled"))] -impl WAABBHierarchy { - const GROUP_SIZE: usize = 4; - - pub fn new(aabbs: &[AABB]) -> Self { - use ncollide::bounding_volume::BoundingVolume; - - let mut levels = vec![aabbs.to_vec()]; - - loop { - let last_level = levels.last().unwrap(); - let mut next_level = Vec::new(); - - for chunk in last_level.chunks(Self::GROUP_SIZE) { - let mut merged = chunk[0]; - for aabb in &chunk[1..] { - merged.merge(aabb) - } - - next_level.push(merged); - } - - if next_level.len() == 1 { - levels.push(next_level); - break; - } - - levels.push(next_level); - } - - Self { levels } - } - - pub fn compute_interferences_with( - &self, - aabb1: AABB, - workspace: &mut WAABBHierarchyIntersections, - ) { - use ncollide::bounding_volume::BoundingVolume; - - workspace.next_level_interferences.clear(); - workspace.curr_level_interferences.clear(); - workspace.curr_level_interferences.push(0); - - for level in self.levels[1..].iter().rev() { - for i in &workspace.curr_level_interferences { - for j in 0..Self::GROUP_SIZE { - if let Some(aabb2) = level.get(*i + j) { - if aabb1.intersects(aabb2) { - workspace - .next_level_interferences - .push((i + j) * Self::GROUP_SIZE) - } - } - } - } - - std::mem::swap( - &mut workspace.curr_level_interferences, - &mut workspace.next_level_interferences, - ); - workspace.next_level_interferences.clear(); - } - - // Last level. - for i in &workspace.curr_level_interferences { - for j in 0..Self::GROUP_SIZE { - if let Some(aabb2) = self.levels[0].get(*i + j) { - if aabb1.intersects(aabb2) { - workspace.next_level_interferences.push(i + j) - } - } - } - } - - std::mem::swap( - &mut workspace.curr_level_interferences, - &mut workspace.next_level_interferences, - ); - workspace.next_level_interferences.clear(); - } -} diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 10c0c8b..db83fa3 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,5 +1,6 @@ +use crate::data::pubsub::Subscription; use crate::dynamics::RigidBodySet; -use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet}; +use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; #[cfg(feature = "enhanced-determinism")] use crate::utils::FxHashMap32 as HashMap; @@ -15,6 +16,41 @@ const NEXT_FREE_SENTINEL: u32 = u32::MAX; const SENTINEL_VALUE: f32 = f32::MAX; const CELL_WIDTH: f32 = 20.0; +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderPair { + pub collider1: ColliderHandle, + pub collider2: ColliderHandle, +} + +impl ColliderPair { + pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + ColliderPair { + collider1, + collider2, + } + } + + pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { + Self::new(collider1, collider2) + } else { + Self::new(collider2, collider1) + } + } + + pub fn swap(self) -> Self { + Self::new(self.collider2, self.collider1) + } + + pub fn zero() -> Self { + Self { + collider1: ColliderHandle::from_raw_parts(0, 0), + collider2: ColliderHandle::from_raw_parts(0, 0), + } + } +} + pub enum BroadPhasePairEvent { AddPair(ColliderPair), DeletePair(ColliderPair), @@ -392,6 +428,7 @@ impl SAPRegion { pub struct BroadPhase { proxies: Proxies, regions: HashMap, SAPRegion>, + removed_colliders: Option>, deleted_any: bool, // We could think serializing this workspace is useless. // It turns out is is important to serialize at least its capacity @@ -480,6 +517,7 @@ impl BroadPhase { /// Create a new empty broad-phase. pub fn new() -> Self { BroadPhase { + removed_colliders: None, proxies: Proxies::new(), regions: HashMap::default(), reporting: HashMap::default(), @@ -487,46 +525,60 @@ impl BroadPhase { } } - pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) { - for collider in handles.iter().filter_map(|h| colliders.get(*h)) { - if collider.proxy_index == crate::INVALID_USIZE { - // This collider has not been added to the broad-phase yet. - continue; + /// Maintain the broad-phase internal state by taking collider removal into account. + pub fn maintain(&mut self, colliders: &mut ColliderSet) { + // Ensure we already subscribed. + if self.removed_colliders.is_none() { + self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + } + + let mut cursor = self.removed_colliders.take().unwrap(); + for collider in colliders.removed_colliders.read(&cursor) { + self.remove_collider(collider.proxy_index); + } + + colliders.removed_colliders.ack(&mut cursor); + self.removed_colliders = Some(cursor); + } + + fn remove_collider<'a>(&mut self, proxy_index: usize) { + if proxy_index == crate::INVALID_USIZE { + // This collider has not been added to the broad-phase yet. + return; + } + + 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); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { + region.predelete_proxy(proxy_index); + self.deleted_any = true; + } } + } - let proxy = &mut self.proxies[collider.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); - - #[cfg(feature = "dim2")] - for i in start.x..=end.x { - for j in start.y..=end.y { - if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { - region.predelete_proxy(collider.proxy_index); + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { + region.predelete_proxy(proxy_index); self.deleted_any = true; } } } - - #[cfg(feature = "dim3")] - for i in start.x..=end.x { - for j in start.y..=end.y { - for k in start.z..=end.z { - if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { - region.predelete_proxy(collider.proxy_index); - self.deleted_any = true; - } - } - } - } - - self.proxies.remove(collider.proxy_index); } + + self.proxies.remove(proxy_index); } pub(crate) fn update_aabbs( @@ -664,16 +716,13 @@ impl BroadPhase { mod test { use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; - use crate::pipeline::PhysicsPipeline; #[test] fn test_add_update_remove() { let mut broad_phase = BroadPhase::new(); - let mut narrow_phase = NarrowPhase::new(); let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); let mut joints = JointSet::new(); - let mut pipeline = PhysicsPipeline::new(); let rb = RigidBodyBuilder::new_dynamic().build(); let co = ColliderBuilder::ball(0.5).build(); @@ -682,15 +731,8 @@ mod test { broad_phase.update_aabbs(0.0, &bodies, &mut colliders); - pipeline.remove_rigid_body( - hrb, - &mut broad_phase, - &mut narrow_phase, - &mut bodies, - &mut colliders, - &mut joints, - ); - + bodies.remove(hrb, &mut colliders, &mut joints); + broad_phase.maintain(&mut colliders); broad_phase.update_aabbs(0.0, &bodies, &mut colliders); // Create another body. diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 2d55857..7c293b6 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,11 +1,12 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::query::RayCast; use num::Zero; #[derive(Clone)] @@ -97,6 +98,49 @@ impl Shape { Shape::HeightField(heightfield) => heightfield.bounding_volume(position), } } + + /// Computes the first intersection point between a ray in this collider. + /// + /// Some shapes are not supported yet and will always return `None`. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + pub fn cast_ray( + &self, + position: &Isometry, + ray: &Ray, + max_toi: f32, + ) -> Option { + match self { + Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), + Shape::Polygon(_poly) => None, + Shape::Capsule(caps) => { + let pos = position * caps.transform_wrt_y(); + let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); + caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) + } + Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), + #[cfg(feature = "dim2")] + Shape::Triangle(_) | Shape::Trimesh(_) => { + // This is not implemented yet in 2D. + None + } + #[cfg(feature = "dim3")] + Shape::Triangle(triangle) => { + triangle.toi_and_normal_with_ray(position, ray, max_toi, true) + } + #[cfg(feature = "dim3")] + Shape::Trimesh(trimesh) => { + trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) + } + Shape::HeightField(heightfield) => { + heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) + } + } + } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -353,6 +397,12 @@ impl ColliderBuilder { self } + /// Sets the restitution coefficient of the collider this builder will build. + pub fn restitution(mut self, restitution: f32) -> Self { + self.restitution = restitution; + self + } + /// Sets the density of the collider this builder will build. pub fn density(mut self, density: f32) -> Self { self.density = Some(density); diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 22bba1b..5ac9658 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,14 +1,25 @@ use crate::data::arena::Arena; +use crate::data::pubsub::PubSub; use crate::dynamics::{RigidBodyHandle, RigidBodySet}; -use crate::geometry::Collider; +use crate::geometry::{Collider, ColliderGraphIndex}; use std::ops::{Index, IndexMut}; /// The unique identifier of a collider added to a collider set. pub type ColliderHandle = crate::data::arena::Index; +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct RemovedCollider { + pub handle: ColliderHandle, + pub(crate) contact_graph_index: ColliderGraphIndex, + pub(crate) proximity_graph_index: ColliderGraphIndex, + pub(crate) proxy_index: usize, +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A set of colliders that can be handled by a physics `World`. pub struct ColliderSet { + pub(crate) removed_colliders: PubSub, pub(crate) colliders: Arena, } @@ -16,6 +27,7 @@ impl ColliderSet { /// Create a new empty set of colliders. pub fn new() -> Self { ColliderSet { + removed_colliders: PubSub::new(), colliders: Arena::new(), } } @@ -26,7 +38,7 @@ impl ColliderSet { } /// Iterate through all the colliders on this set. - pub fn iter(&self) -> impl Iterator { + pub fn iter(&self) -> impl ExactSizeIterator { self.colliders.iter() } @@ -60,8 +72,35 @@ impl ColliderSet { handle } - pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option { - self.colliders.remove(handle) + /// Remove a collider from this set and update its parent accordingly. + pub fn remove( + &mut self, + handle: ColliderHandle, + bodies: &mut RigidBodySet, + ) -> Option { + let collider = self.colliders.remove(handle)?; + + /* + * Delete the collider from its parent body. + */ + if let Some(parent) = bodies.get_mut_internal(collider.parent) { + parent.remove_collider_internal(handle, &collider); + bodies.wake_up(collider.parent, true); + } + + /* + * Publish removal. + */ + let message = RemovedCollider { + handle, + contact_graph_index: collider.contact_graph_index, + proximity_graph_index: collider.proximity_graph_index, + proxy_index: collider.proxy_index, + }; + + self.removed_colliders.publish(message); + + Some(collider) } /// Gets the collider with the given handle without a known generation. diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index f59a94b..04afc65 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -71,10 +71,10 @@ fn do_generate_contacts( } else { manifold.subshape_index_pair.1 }; - println!( - "Restoring for {} [chosen with {:?}]", - subshape_id, manifold.subshape_index_pair - ); + // println!( + // "Restoring for {} [chosen with {:?}]", + // subshape_id, manifold.subshape_index_pair + // ); // Use dummy shapes for the dispatch. #[cfg(feature = "dim2")] diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 78f26bb..52ba9b7 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,11 +1,11 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyIntersections}; +use crate::geometry::{Collider, ContactManifold, Shape, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { - interferences: WAABBHierarchyIntersections, + interferences: Vec, local_aabb2: AABB, old_interferences: Vec, old_manifolds: Vec, @@ -14,7 +14,7 @@ pub struct TrimeshShapeContactGeneratorWorkspace { impl TrimeshShapeContactGeneratorWorkspace { pub fn new() -> Self { Self { - interferences: WAABBHierarchyIntersections::new(), + interferences: Vec::new(), local_aabb2: AABB::new_invalid(), old_interferences: Vec::new(), old_manifolds: Vec::new(), @@ -74,7 +74,7 @@ fn do_generate_contacts( let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? std::mem::swap( &mut workspace.old_interferences, - workspace.interferences.computed_interferences_mut(), + &mut workspace.interferences, ); std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds); ctxt.pair.manifolds.clear(); @@ -108,16 +108,17 @@ fn do_generate_contacts( // workspace.old_manifolds.len() // ); + workspace.interferences.clear(); trimesh1 .waabbs() - .compute_interferences_with(local_aabb2, &mut workspace.interferences); + .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } /* * Dispatch to the specific solver by keeping the previous manifold if we already had one. */ - let new_interferences = workspace.interferences.computed_interferences(); + let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut old_manifolds_it = workspace.old_manifolds.drain(..); diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4f72778..562f962 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -36,11 +36,15 @@ pub type AABB = ncollide::bounding_volume::AABB; pub type ContactEvent = ncollide::pipeline::ContactEvent; /// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not). pub type ProximityEvent = ncollide::pipeline::ProximityEvent; +/// A ray that can be cast against colliders. +pub type Ray = ncollide::query::Ray; +/// The intersection between a ray and a collider. +pub type RayIntersection = ncollide::query::RayIntersection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; -pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections}; -pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent; +pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; +pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; #[cfg(feature = "dim2")] @@ -48,12 +52,11 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] pub(crate) use self::polyhedron_feature3d::PolyhedronFace; -#[cfg(feature = "simd-is-enabled")] -pub(crate) use self::waabb::WAABB; +pub(crate) use self::waabb::{WRay, WAABB}; +pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; mod ball; -mod broad_phase; mod broad_phase_multi_sap; mod capsule; mod collider; @@ -75,6 +78,6 @@ mod proximity_detector; pub(crate) mod sat; pub(crate) mod triangle; mod trimesh; -#[cfg(feature = "simd-is-enabled")] mod waabb; +mod wquadtree; //mod z_order; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 1a36511..ebe0a79 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -14,13 +14,16 @@ use crate::geometry::proximity_detector::{ // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent, + ProximityPair, RemovedCollider, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] //use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::data::pubsub::Subscription; use crate::ncollide::query::Proximity; use crate::pipeline::EventHandler; +use std::collections::HashMap; //use simba::simd::SimdValue; /// The narrow-phase responsible for computing precise contact information between colliders. @@ -28,6 +31,7 @@ use crate::pipeline::EventHandler; pub struct NarrowPhase { contact_graph: InteractionGraph, proximity_graph: InteractionGraph, + removed_colliders: Option>, // ball_ball: Vec, // Workspace: Vec<*mut ContactPair>, // shape_shape: Vec, // Workspace: Vec<*mut ContactPair>, // ball_ball_prox: Vec, // Workspace: Vec<*mut ProximityPair>, @@ -42,6 +46,7 @@ impl NarrowPhase { Self { contact_graph: InteractionGraph::new(), proximity_graph: InteractionGraph::new(), + removed_colliders: None, // ball_ball: Vec::new(), // shape_shape: Vec::new(), // ball_ball_prox: Vec::new(), @@ -73,45 +78,84 @@ impl NarrowPhase { // &mut self.contact_graph.interactions // } - pub(crate) fn remove_colliders( + /// Maintain the narrow-phase internal state by taking collider removal into account. + pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) { + // Ensure we already subscribed. + if self.removed_colliders.is_none() { + self.removed_colliders = Some(colliders.removed_colliders.subscribe()); + } + + let mut cursor = self.removed_colliders.take().unwrap(); + + // TODO: avoid these hash-maps. + // They are necessary to handle the swap-remove done internally + // by the contact/proximity graphs when a node is removed. + let mut prox_id_remap = HashMap::new(); + let mut contact_id_remap = HashMap::new(); + + for i in 0.. { + if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) { + let proximity_graph_id = prox_id_remap + .get(&collider.handle) + .copied() + .unwrap_or(collider.proximity_graph_index); + let contact_graph_id = contact_id_remap + .get(&collider.handle) + .copied() + .unwrap_or(collider.contact_graph_index); + + self.remove_collider( + proximity_graph_id, + contact_graph_id, + colliders, + bodies, + &mut prox_id_remap, + &mut contact_id_remap, + ); + } else { + break; + } + } + + colliders.removed_colliders.ack(&mut cursor); + self.removed_colliders = Some(cursor); + } + + pub(crate) fn remove_collider<'a>( &mut self, - handles: &[ColliderHandle], + proximity_graph_id: ColliderGraphIndex, + contact_graph_id: ColliderGraphIndex, colliders: &mut ColliderSet, bodies: &mut RigidBodySet, + prox_id_remap: &mut HashMap, + contact_id_remap: &mut HashMap, ) { - for handle in handles { - if let Some(collider) = colliders.get(*handle) { - let proximity_graph_id = collider.proximity_graph_index; - let contact_graph_id = collider.contact_graph_index; + // 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, true) + } - // 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) - } + if let Some(parent) = colliders.get(b).map(|c| c.parent) { + bodies.wake_up(parent, true) + } + } - if let Some(parent) = colliders.get(b).map(|c| c.parent) { - bodies.wake_up(parent) - } - } + // We have to manage the fact that one other collider will + // have its graph index changed because of the node's swap-remove. + if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) { + if let Some(replacement) = colliders.get_mut(replacement) { + replacement.proximity_graph_index = proximity_graph_id; + } else { + prox_id_remap.insert(replacement, proximity_graph_id); + } + } - // We have to manage the fact that one other collider will - // have its graph index changed because of the node's swap-remove. - if let Some(replacement) = self - .proximity_graph - .remove_node(proximity_graph_id) - .and_then(|h| colliders.get_mut(h)) - { - replacement.proximity_graph_index = proximity_graph_id; - } - - if let Some(replacement) = self - .contact_graph - .remove_node(contact_graph_id) - .and_then(|h| colliders.get_mut(h)) - { - replacement.contact_graph_index = contact_graph_id; - } + if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) { + if let Some(replacement) = colliders.get_mut(replacement) { + replacement.contact_graph_index = contact_graph_id; + } else { + contact_id_remap.insert(replacement, contact_graph_id); } } } @@ -119,6 +163,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 +263,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 +299,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 +407,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/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index 3dd7381..edf3085 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,11 +1,11 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, Shape, Trimesh, WAABBHierarchyIntersections}; +use crate::geometry::{Collider, Proximity, Shape, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { - interferences: WAABBHierarchyIntersections, + interferences: Vec, local_aabb2: AABB, old_interferences: Vec, } @@ -13,7 +13,7 @@ pub struct TrimeshShapeProximityDetectorWorkspace { impl TrimeshShapeProximityDetectorWorkspace { pub fn new() -> Self { Self { - interferences: WAABBHierarchyIntersections::new(), + interferences: Vec::new(), local_aabb2: AABB::new_invalid(), old_interferences: Vec::new(), } @@ -67,19 +67,20 @@ fn do_detect_proximity( let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? std::mem::swap( &mut workspace.old_interferences, - &mut workspace.interferences.computed_interferences_mut(), + &mut workspace.interferences, ); + workspace.interferences.clear(); trimesh1 .waabbs() - .compute_interferences_with(local_aabb2, &mut workspace.interferences); + .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } /* * Dispatch to the specific solver by keeping the previous manifold if we already had one. */ - let new_interferences = workspace.interferences.computed_interferences(); + let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut best_proximity = Proximity::Disjoint; diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index 62731b6..b6e23e7 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,13 +1,19 @@ -use crate::geometry::{Triangle, WAABBHierarchy}; +use crate::geometry::{Triangle, WQuadtree}; use crate::math::{Isometry, Point}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +#[cfg(feature = "dim3")] +use { + crate::geometry::{Ray, RayIntersection}, + ncollide::query::RayCast, +}; + #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A triangle mesh. pub struct Trimesh { - waabb_tree: WAABBHierarchy, + wquadtree: WQuadtree, aabb: AABB, vertices: Vec>, indices: Vec>, @@ -25,41 +31,24 @@ impl Trimesh { "A triangle mesh must contain at least one triangle." ); - // z-sort the indices. - // indices.sort_unstable_by(|idx, jdx| { - // let ti = Triangle::new( - // vertices[idx[0] as usize], - // vertices[idx[1] as usize], - // vertices[idx[2] as usize], - // ); - // let tj = Triangle::new( - // vertices[jdx[0] as usize], - // vertices[jdx[1] as usize], - // vertices[jdx[2] as usize], - // ); - // let center_i = (ti.a.coords + ti.b.coords + ti.c.coords) / 3.0; - // let center_j = (tj.a.coords + tj.b.coords + tj.c.coords) / 3.0; - // crate::geometry::z_cmp_floats(center_i.as_slice(), center_j.as_slice()) - // .unwrap_or(std::cmp::Ordering::Equal) - // }); let aabb = AABB::from_points(&vertices); + let data = indices.iter().enumerate().map(|(i, idx)| { + let aabb = Triangle::new( + vertices[idx[0] as usize], + vertices[idx[1] as usize], + vertices[idx[2] as usize], + ) + .local_bounding_volume(); + (i, aabb) + }); - let aabbs: Vec<_> = indices - .iter() - .map(|idx| { - Triangle::new( - vertices[idx[0] as usize], - vertices[idx[1] as usize], - vertices[idx[2] as usize], - ) - .local_bounding_volume() - }) - .collect(); - - let waabb_tree = WAABBHierarchy::new(&aabbs); + let mut wquadtree = WQuadtree::new(); + // NOTE: we apply no dilation factor because we won't + // update this tree dynamically. + wquadtree.clear_and_rebuild(data, 0.0); Self { - waabb_tree, + wquadtree, aabb, vertices, indices, @@ -71,8 +60,8 @@ impl Trimesh { self.aabb.transform_by(pos) } - pub(crate) fn waabbs(&self) -> &WAABBHierarchy { - &self.waabb_tree + pub(crate) fn waabbs(&self) -> &WQuadtree { + &self.wquadtree } /// The number of triangles forming this mesh. @@ -120,3 +109,53 @@ impl Trimesh { } } } + +#[cfg(feature = "dim3")] +impl RayCast for Trimesh { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option { + // FIXME: do a best-first search. + let mut intersections = Vec::new(); + let ls_ray = ray.inverse_transform_by(m); + self.wquadtree + .cast_ray(&ls_ray, max_toi, &mut intersections); + let mut best: Option = None; + + for inter in intersections { + let tri = self.triangle(inter); + if let Some(inter) = tri.toi_and_normal_with_ray(m, ray, max_toi, solid) { + if let Some(curr) = &mut best { + if curr.toi > inter.toi { + *curr = inter; + } + } else { + best = Some(inter); + } + } + } + + best + } + + fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: f32) -> bool { + // FIXME: do a best-first search. + let mut intersections = Vec::new(); + let ls_ray = ray.inverse_transform_by(m); + self.wquadtree + .cast_ray(&ls_ray, max_toi, &mut intersections); + + for inter in intersections { + let tri = self.triangle(inter); + if tri.intersects_ray(m, ray, max_toi) { + return true; + } + } + + false + } +} diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index c3853bc..645ac04 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,8 +1,27 @@ -#[cfg(feature = "serde-serialize")] -use crate::math::DIM; -use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH}; +use crate::geometry::Ray; +use crate::math::{Point, Vector, DIM, SIMD_WIDTH}; +use crate::utils; use ncollide::bounding_volume::AABB; -use simba::simd::{SimdPartialOrd, SimdValue}; +use num::{One, Zero}; +use { + crate::math::{SimdBool, SimdFloat}, + simba::simd::{SimdPartialOrd, SimdValue}, +}; + +#[derive(Debug, Copy, Clone)] +pub(crate) struct WRay { + pub origin: Point, + pub dir: Vector, +} + +impl WRay { + pub fn splat(ray: Ray) -> Self { + Self { + origin: Point::splat(ray.origin), + dir: Vector::splat(ray.dir), + } + } +} #[derive(Debug, Copy, Clone)] pub(crate) struct WAABB { @@ -28,6 +47,7 @@ impl serde::Serialize for WAABB { .coords .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), ); + let mut waabb = serializer.serialize_struct("WAABB", 2)?; waabb.serialize_field("mins", &mins)?; waabb.serialize_field("maxs", &maxs)?; @@ -73,8 +93,8 @@ impl<'de> serde::Deserialize<'de> for WAABB { } impl WAABB { - pub fn new(mins: Point, maxs: Point) -> Self { - Self { mins, maxs } + pub fn new_invalid() -> Self { + Self::splat(AABB::new_invalid()) } pub fn splat(aabb: AABB) -> Self { @@ -84,8 +104,73 @@ impl WAABB { } } + pub fn dilate_by_factor(&mut self, factor: SimdFloat) { + let dilation = (self.maxs - self.mins) * factor; + self.mins -= dilation; + self.maxs += dilation; + } + + pub fn replace(&mut self, i: usize, aabb: AABB) { + self.mins.replace(i, aabb.mins); + self.maxs.replace(i, aabb.maxs); + } + + pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool { + let _0 = SimdFloat::zero(); + let _1 = SimdFloat::one(); + let _infinity = SimdFloat::splat(f32::MAX); + + let mut hit = SimdBool::splat(true); + let mut tmin = SimdFloat::zero(); + let mut tmax = max_toi; + + // TODO: could this be optimized more considering we really just need a boolean answer? + for i in 0usize..DIM { + let is_not_zero = ray.dir[i].simd_ne(_0); + let is_zero_test = + ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i]); + let is_not_zero_test = { + let denom = _1 / ray.dir[i]; + let mut inter_with_near_plane = + ((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -_infinity); + let mut inter_with_far_plane = + ((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, _infinity); + + let gt = inter_with_near_plane.simd_gt(inter_with_far_plane); + utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane); + + tmin = tmin.simd_max(inter_with_near_plane); + tmax = tmax.simd_min(inter_with_far_plane); + + tmin.simd_le(tmax) + }; + + hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test); + } + + hit + } + #[cfg(feature = "dim2")] - pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { + pub fn contains(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.mins.x) + & self.mins.y.simd_le(other.mins.y) + & self.maxs.x.simd_ge(other.maxs.x) + & self.maxs.y.simd_ge(other.maxs.y) + } + + #[cfg(feature = "dim3")] + pub fn contains(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.mins.x) + & self.mins.y.simd_le(other.mins.y) + & self.mins.z.simd_le(other.mins.z) + & self.maxs.x.simd_ge(other.maxs.x) + & self.maxs.y.simd_ge(other.maxs.y) + & self.maxs.z.simd_ge(other.maxs.z) + } + + #[cfg(feature = "dim2")] + pub fn intersects(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) & other.mins.x.simd_le(self.maxs.x) & self.mins.y.simd_le(other.maxs.y) @@ -93,7 +178,7 @@ impl WAABB { } #[cfg(feature = "dim3")] - pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { + pub fn intersects(&self, other: &WAABB) -> SimdBool { self.mins.x.simd_le(other.maxs.x) & other.mins.x.simd_le(self.maxs.x) & self.mins.y.simd_le(other.maxs.y) @@ -101,6 +186,13 @@ impl WAABB { & self.mins.z.simd_le(other.maxs.z) & other.mins.z.simd_le(self.maxs.z) } + + pub fn to_merged_aabb(&self) -> AABB { + AABB::new( + self.mins.coords.map(|e| e.simd_horizontal_min()).into(), + self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), + ) + } } impl From<[AABB; SIMD_WIDTH]> for WAABB { diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs new file mode 100644 index 0000000..fce04eb --- /dev/null +++ b/src/geometry/wquadtree.rs @@ -0,0 +1,560 @@ +use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB}; +use crate::geometry::{WRay, WAABB}; +use crate::math::Point; +#[cfg(feature = "dim3")] +use crate::math::Vector; +use crate::simd::{SimdFloat, SIMD_WIDTH}; +use ncollide::bounding_volume::BoundingVolume; +use simba::simd::{SimdBool, SimdValue}; +use std::collections::VecDeque; +use std::ops::Range; + +pub trait IndexedData: Copy { + fn default() -> Self; + fn index(&self) -> usize; +} + +impl IndexedData for usize { + fn default() -> Self { + u32::MAX as usize + } + + fn index(&self) -> usize { + *self + } +} + +impl IndexedData for ColliderHandle { + fn default() -> Self { + ColliderSet::invalid_handle() + } + + fn index(&self) -> usize { + self.into_raw_parts().0 + } +} + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct NodeIndex { + index: u32, // Index of the addressed node in the `nodes` array. + lane: u8, // SIMD lane of the addressed node. +} + +impl NodeIndex { + fn new(index: u32, lane: u8) -> Self { + Self { index, lane } + } + + fn invalid() -> Self { + Self { + index: u32::MAX, + lane: 0, + } + } +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct WQuadtreeNode { + waabb: WAABB, + // Index of the nodes of the 4 nodes represented by self. + // If this is a leaf, it contains the proxy ids instead. + children: [u32; 4], + parent: NodeIndex, + leaf: bool, // TODO: pack this with the NodexIndex.lane? + dirty: bool, // TODO: move this to a separate bitvec? +} + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct WQuadtreeProxy { + node: NodeIndex, + data: T, // The collider data. TODO: only set the collider generation here? +} + +impl WQuadtreeProxy { + fn invalid() -> Self { + Self { + node: NodeIndex::invalid(), + data: T::default(), + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug)] +pub struct WQuadtree { + nodes: Vec, + dirty_nodes: VecDeque, + proxies: Vec>, +} + +// FIXME: this should be generic too. +impl WQuadtree { + pub fn pre_update(&mut self, data: ColliderHandle) { + let id = data.into_raw_parts().0; + let node_id = self.proxies[id].node.index; + let node = &mut self.nodes[node_id as usize]; + if !node.dirty { + node.dirty = true; + self.dirty_nodes.push_back(node_id); + } + } + + pub fn update(&mut self, colliders: &ColliderSet, dilation_factor: f32) { + // Loop on the dirty leaves. + let dilation_factor = SimdFloat::splat(dilation_factor); + + while let Some(id) = self.dirty_nodes.pop_front() { + // NOTE: this will data the case where we reach the root of the tree. + if let Some(node) = self.nodes.get(id as usize) { + // Compute the new WAABB. + let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; + for (child_id, new_aabb) in node.children.iter().zip(new_aabbs.iter_mut()) { + if node.leaf { + // We are in a leaf: compute the colliders' AABBs. + if let Some(proxy) = self.proxies.get(*child_id as usize) { + let collider = &colliders[proxy.data]; + *new_aabb = collider.compute_aabb(); + } + } else { + // We are in an internal node: compute the children's AABBs. + if let Some(node) = self.nodes.get(*child_id as usize) { + *new_aabb = node.waabb.to_merged_aabb(); + } + } + } + + let node = &mut self.nodes[id as usize]; + let new_waabb = WAABB::from(new_aabbs); + if !node.waabb.contains(&new_waabb).all() { + node.waabb = new_waabb; + node.waabb.dilate_by_factor(dilation_factor); + self.dirty_nodes.push_back(node.parent.index); + } + node.dirty = false; + } + } + } +} + +impl WQuadtree { + pub fn new() -> Self { + WQuadtree { + nodes: Vec::new(), + dirty_nodes: VecDeque::new(), + proxies: Vec::new(), + } + } + + pub fn clear_and_rebuild( + &mut self, + data: impl ExactSizeIterator, + dilation_factor: f32, + ) { + self.nodes.clear(); + self.proxies.clear(); + + // Create proxies. + let mut indices = Vec::with_capacity(data.len()); + let mut aabbs = vec![AABB::new_invalid(); data.len()]; + self.proxies = vec![WQuadtreeProxy::invalid(); data.len()]; + + for (data, aabb) in data { + let index = data.index(); + if index >= self.proxies.len() { + self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); + aabbs.resize(index + 1, AABB::new_invalid()); + } + + self.proxies[index].data = data; + aabbs[index] = aabb; + indices.push(index); + } + + // Build the tree recursively. + let root_node = WQuadtreeNode { + waabb: WAABB::new_invalid(), + children: [1, u32::MAX, u32::MAX, u32::MAX], + parent: NodeIndex::invalid(), + leaf: false, + dirty: false, + }; + + self.nodes.push(root_node); + let root_id = NodeIndex::new(0, 0); + let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id, dilation_factor); + self.nodes[0].waabb = WAABB::from([ + aabb, + AABB::new_invalid(), + AABB::new_invalid(), + AABB::new_invalid(), + ]); + } + + fn do_recurse_build( + &mut self, + indices: &mut [usize], + aabbs: &[AABB], + parent: NodeIndex, + dilation_factor: f32, + ) -> (u32, AABB) { + if indices.len() <= 4 { + // Leaf case. + let my_id = self.nodes.len(); + let mut my_aabb = AABB::new_invalid(); + let mut leaf_aabbs = [AABB::new_invalid(); 4]; + let mut proxy_ids = [u32::MAX; 4]; + + for (k, id) in indices.iter().enumerate() { + my_aabb.merge(&aabbs[*id]); + leaf_aabbs[k] = aabbs[*id]; + proxy_ids[k] = *id as u32; + self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); + } + + let mut node = WQuadtreeNode { + waabb: WAABB::from(leaf_aabbs), + children: proxy_ids, + parent, + leaf: true, + dirty: false, + }; + + node.waabb + .dilate_by_factor(SimdFloat::splat(dilation_factor)); + self.nodes.push(node); + return (my_id as u32, my_aabb); + } + + // Compute the center and variance along each dimension. + // In 3D we compute the variance to not-subdivide the dimension with lowest variance. + // Therefore variance computation is not needed in 2D because we only have 2 dimension + // to split in the first place. + let mut center = Point::origin(); + #[cfg(feature = "dim3")] + let mut variance = Vector::zeros(); + + let denom = 1.0 / (indices.len() as f32); + + for i in &*indices { + let coords = aabbs[*i].center().coords; + center += coords * denom; + #[cfg(feature = "dim3")] + { + variance += coords.component_mul(&coords) * denom; + } + } + + #[cfg(feature = "dim3")] + { + variance = variance - center.coords.component_mul(¢er.coords); + } + + // Find the axis with minimum variance. This is the axis along + // which we are **not** subdividing our set. + #[allow(unused_mut)] // Does not need to be mutable in 2D. + let mut subdiv_dims = [0, 1]; + #[cfg(feature = "dim3")] + { + let min = variance.imin(); + subdiv_dims[0] = (min + 1) % 3; + subdiv_dims[1] = (min + 2) % 3; + } + + // Split the set along the two subdiv_dims dimensions. + // TODO: should we split wrt. the median instead of the average? + // TODO: we should ensure each subslice contains at least 4 elements each (or less if + // indices has less than 16 elements in the first place. + let (left, right) = split_indices_wrt_dim(indices, &aabbs, ¢er, subdiv_dims[0]); + + let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, ¢er, subdiv_dims[1]); + let (right_bottom, right_top) = + split_indices_wrt_dim(right, &aabbs, ¢er, subdiv_dims[1]); + + // println!( + // "Recursing on children: {}, {}, {}, {}", + // left_bottom.len(), + // left_top.len(), + // right_bottom.len(), + // right_top.len() + // ); + + let node = WQuadtreeNode { + waabb: WAABB::new_invalid(), + children: [0; 4], // Will be set after the recursive call + parent, + leaf: false, + dirty: false, + }; + + let id = self.nodes.len() as u32; + self.nodes.push(node); + + // Recurse! + let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0), dilation_factor); + let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1), dilation_factor); + let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2), dilation_factor); + let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3), dilation_factor); + + // Now we know the indices of the grand-nodes. + self.nodes[id as usize].children = [a.0, b.0, c.0, d.0]; + self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]); + self.nodes[id as usize] + .waabb + .dilate_by_factor(SimdFloat::splat(dilation_factor)); + + // TODO: will this chain of .merged be properly optimized? + let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1); + (id, my_aabb) + } + + // FIXME: implement a visitor pattern to merge intersect_aabb + // and intersect_ray into a single method. + pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec) { + if self.nodes.is_empty() { + return; + } + + // Special case for the root. + let mut stack = vec![0u32]; + let waabb = WAABB::splat(*aabb); + while let Some(inode) = stack.pop() { + let node = self.nodes[inode as usize]; + let intersections = node.waabb.intersects(&waabb); + let bitmask = intersections.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 { + if node.leaf { + // We found a leaf! + // Unfortunately, invalid AABBs return a intersection as well. + if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { + out.push(proxy.data); + } + } else { + // Internal node, visit the child. + // Unfortunately, we have this check because invalid AABBs + // return a intersection as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push(node.children[ii]); + } + } + } + } + } + } + + pub fn cast_ray(&self, ray: &Ray, max_toi: f32, out: &mut Vec) { + if self.nodes.is_empty() { + return; + } + + // Special case for the root. + let mut stack = vec![0u32]; + let wray = WRay::splat(*ray); + let wmax_toi = SimdFloat::splat(max_toi); + while let Some(inode) = stack.pop() { + let node = self.nodes[inode as usize]; + let hits = node.waabb.intersects_ray(&wray, wmax_toi); + let bitmask = hits.bitmask(); + + for ii in 0..SIMD_WIDTH { + if (bitmask & (1 << ii)) != 0 { + if node.leaf { + // We found a leaf! + // Unfortunately, invalid AABBs return a hit as well. + if let Some(proxy) = self.proxies.get(node.children[ii] as usize) { + out.push(proxy.data); + } + } else { + // Internal node, visit the child. + // Un fortunately, we have this check because invalid AABBs + // return a hit as well. + if node.children[ii] as usize <= self.nodes.len() { + stack.push(node.children[ii]); + } + } + } + } + } + } +} + +#[allow(dead_code)] +struct WQuadtreeIncrementalBuilderStep { + range: Range, + parent: NodeIndex, +} + +#[allow(dead_code)] +struct WQuadtreeIncrementalBuilder { + quadtree: WQuadtree, + to_insert: Vec, + aabbs: Vec, + indices: Vec, +} + +#[allow(dead_code)] +impl WQuadtreeIncrementalBuilder { + pub fn new() -> Self { + Self { + quadtree: WQuadtree::new(), + to_insert: Vec::new(), + aabbs: Vec::new(), + indices: Vec::new(), + } + } + + pub fn update_single_depth(&mut self) { + if let Some(to_insert) = self.to_insert.pop() { + let indices = &mut self.indices[to_insert.range]; + + // Leaf case. + if indices.len() <= 4 { + let id = self.quadtree.nodes.len(); + let mut aabb = AABB::new_invalid(); + let mut leaf_aabbs = [AABB::new_invalid(); 4]; + let mut proxy_ids = [u32::MAX; 4]; + + for (k, id) in indices.iter().enumerate() { + aabb.merge(&self.aabbs[*id]); + leaf_aabbs[k] = self.aabbs[*id]; + proxy_ids[k] = *id as u32; + } + + let node = WQuadtreeNode { + waabb: WAABB::from(leaf_aabbs), + children: proxy_ids, + parent: to_insert.parent, + leaf: true, + dirty: false, + }; + + self.quadtree.nodes[to_insert.parent.index as usize].children + [to_insert.parent.lane as usize] = id as u32; + self.quadtree.nodes[to_insert.parent.index as usize] + .waabb + .replace(to_insert.parent.lane as usize, aabb); + self.quadtree.nodes.push(node); + return; + } + + // Compute the center and variance along each dimension. + // In 3D we compute the variance to not-subdivide the dimension with lowest variance. + // Therefore variance computation is not needed in 2D because we only have 2 dimension + // to split in the first place. + let mut center = Point::origin(); + #[cfg(feature = "dim3")] + let mut variance = Vector::zeros(); + + let denom = 1.0 / (indices.len() as f32); + let mut aabb = AABB::new_invalid(); + + for i in &*indices { + let coords = self.aabbs[*i].center().coords; + aabb.merge(&self.aabbs[*i]); + center += coords * denom; + #[cfg(feature = "dim3")] + { + variance += coords.component_mul(&coords) * denom; + } + } + + #[cfg(feature = "dim3")] + { + variance = variance - center.coords.component_mul(¢er.coords); + } + + // Find the axis with minimum variance. This is the axis along + // which we are **not** subdividing our set. + #[allow(unused_mut)] // Does not need to be mutable in 2D. + let mut subdiv_dims = [0, 1]; + #[cfg(feature = "dim3")] + { + let min = variance.imin(); + subdiv_dims[0] = (min + 1) % 3; + subdiv_dims[1] = (min + 2) % 3; + } + + // Split the set along the two subdiv_dims dimensions. + // TODO: should we split wrt. the median instead of the average? + // TODO: we should ensure each subslice contains at least 4 elements each (or less if + // indices has less than 16 elements in the first place. + let (left, right) = + split_indices_wrt_dim(indices, &self.aabbs, ¢er, subdiv_dims[0]); + + let (left_bottom, left_top) = + split_indices_wrt_dim(left, &self.aabbs, ¢er, subdiv_dims[1]); + let (right_bottom, right_top) = + split_indices_wrt_dim(right, &self.aabbs, ¢er, subdiv_dims[1]); + + let node = WQuadtreeNode { + waabb: WAABB::new_invalid(), + children: [0; 4], // Will be set after the recursive call + parent: to_insert.parent, + leaf: false, + dirty: false, + }; + + let id = self.quadtree.nodes.len() as u32; + self.quadtree.nodes.push(node); + + // Recurse! + let a = left_bottom.len(); + let b = a + left_top.len(); + let c = b + right_bottom.len(); + let d = c + right_top.len(); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: 0..a, + parent: NodeIndex::new(id, 0), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: a..b, + parent: NodeIndex::new(id, 1), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: b..c, + parent: NodeIndex::new(id, 2), + }); + self.to_insert.push(WQuadtreeIncrementalBuilderStep { + range: c..d, + parent: NodeIndex::new(id, 3), + }); + + self.quadtree.nodes[to_insert.parent.index as usize].children + [to_insert.parent.lane as usize] = id as u32; + self.quadtree.nodes[to_insert.parent.index as usize] + .waabb + .replace(to_insert.parent.lane as usize, aabb); + } + } +} + +fn split_indices_wrt_dim<'a>( + indices: &'a mut [usize], + aabbs: &[AABB], + split_point: &Point, + dim: usize, +) -> (&'a mut [usize], &'a mut [usize]) { + let mut icurr = 0; + let mut ilast = indices.len() - 1; + + // The loop condition we can just do 0..indices.len() + // instead of the test icurr < ilast because we know + // we will iterate exactly once per index. + for _ in 0..indices.len() { + let i = indices[icurr]; + let center = aabbs[i].center(); + + if center[dim] > split_point[dim] { + indices.swap(icurr, ilast); + ilast -= 1; + } else { + icurr += 1; + } + } + + indices.split_at_mut(icurr) +} diff --git a/src/lib.rs b/src/lib.rs index 118ac23..3674717 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -44,7 +44,6 @@ macro_rules! enable_flush_to_zero( } ); -#[cfg(feature = "simd-is-enabled")] macro_rules! array( ($callback: expr; SIMD_WIDTH) => { { @@ -139,7 +138,6 @@ pub mod utils; #[cfg(feature = "dim2")] /// Math primitives used throughout Rapier. pub mod math { - #[cfg(feature = "simd-is-enabled")] pub use super::simd::*; use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2}; @@ -182,7 +180,6 @@ pub mod math { #[cfg(feature = "dim3")] /// Math primitives used throughout Rapier. pub mod math { - #[cfg(feature = "simd-is-enabled")] pub use super::simd::*; use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3}; @@ -220,6 +217,19 @@ pub mod math { pub type SdpMatrix = crate::utils::SdpMatrix3; } +#[cfg(not(feature = "simd-is-enabled"))] +mod simd { + use simba::simd::{AutoBoolx4, AutoF32x4}; + /// The number of lanes of a SIMD number. + pub const SIMD_WIDTH: usize = 4; + /// SIMD_WIDTH - 1 + pub const SIMD_LAST_INDEX: usize = 3; + /// A SIMD float with SIMD_WIDTH lanes. + pub type SimdFloat = AutoF32x4; + /// A SIMD bool with SIMD_WIDTH lanes. + pub type SimdBool = AutoBoolx4; +} + #[cfg(feature = "simd-is-enabled")] mod simd { #[allow(unused_imports)] @@ -233,16 +243,16 @@ mod simd { /// SIMD_WIDTH - 1 pub const SIMD_LAST_INDEX: usize = 3; #[cfg(not(feature = "simd-nightly"))] - /// A SIMD float with SIMD_WIDTH lanes. + /// A SIMD float with SIMD_WIDTH lanes. pub type SimdFloat = WideF32x4; #[cfg(not(feature = "simd-nightly"))] - /// A SIMD bool with SIMD_WIDTH lanes. + /// A SIMD bool with SIMD_WIDTH lanes. pub type SimdBool = WideBoolF32x4; #[cfg(feature = "simd-nightly")] - /// A SIMD float with SIMD_WIDTH lanes. + /// A SIMD float with SIMD_WIDTH lanes. pub type SimdFloat = f32x4; #[cfg(feature = "simd-nightly")] - /// A bool float with SIMD_WIDTH lanes. + /// A bool float with SIMD_WIDTH lanes. pub type SimdBool = m32x4; // pub const SIMD_WIDTH: usize = 8; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 0184295..5a19e52 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,6 +1,6 @@ //! Physics pipeline structures. -use crate::dynamics::{JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{JointSet, RigidBodySet}; use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; use crate::pipeline::EventHandler; @@ -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); @@ -84,28 +84,4 @@ impl CollisionPipeline { bodies.modified_inactive_set.clear(); } - - /// Remove a rigid-body and all its associated data. - pub fn remove_rigid_body( - &mut self, - handle: RigidBodyHandle, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - ) -> Option { - // Remove the body. - let body = bodies.remove_internal(handle)?; - - // Remove this rigid-body from the broad-phase and narrow-phase. - broad_phase.remove_colliders(&body.colliders, colliders); - narrow_phase.remove_colliders(&body.colliders, colliders, bodies); - - // Remove all colliders attached to this body. - for collider in &body.colliders { - colliders.remove_internal(*collider); - } - - Some(body) - } } diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 6298d18..287de9d 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -3,7 +3,9 @@ pub use collision_pipeline::CollisionPipeline; pub use event_handler::{ChannelEventCollector, EventHandler}; pub use physics_pipeline::PhysicsPipeline; +pub use query_pipeline::QueryPipeline; mod collision_pipeline; mod event_handler; mod physics_pipeline; +mod query_pipeline; diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 8449477..47fd260 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -3,12 +3,11 @@ use crate::counters::Counters; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; -use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet, - ContactManifoldIndex, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, }; use crate::math::Vector; use crate::pipeline::EventHandler; @@ -71,10 +70,20 @@ impl PhysicsPipeline { joints: &mut JointSet, events: &dyn EventHandler, ) { - // println!("Step"); self.counters.step_started(); + 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 + // 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()); @@ -245,63 +249,46 @@ impl PhysicsPipeline { bodies.modified_inactive_set.clear(); self.counters.step_completed(); } - - /// Remove a collider and all its associated data. - pub fn remove_collider( - &mut self, - handle: ColliderHandle, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - ) -> Option { - broad_phase.remove_colliders(&[handle], colliders); - narrow_phase.remove_colliders(&[handle], colliders, bodies); - let collider = colliders.remove_internal(handle)?; - - if let Some(parent) = bodies.get_mut_internal(collider.parent) { - parent.remove_collider_internal(handle, &collider); - bodies.wake_up(collider.parent); - } - - Some(collider) - } - - /// Remove a rigid-body and all its associated data. - pub fn remove_rigid_body( - &mut self, - handle: RigidBodyHandle, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - joints: &mut JointSet, - ) -> Option { - // Remove the body. - let body = bodies.remove_internal(handle)?; - - // Remove this rigid-body from the broad-phase and narrow-phase. - broad_phase.remove_colliders(&body.colliders, colliders); - narrow_phase.remove_colliders(&body.colliders, colliders, bodies); - - // Remove all joints attached to this body. - joints.remove_rigid_body(body.joint_graph_index, bodies); - - // Remove all colliders attached to this body. - for collider in &body.colliders { - colliders.remove_internal(*collider); - } - - Some(body) - } } #[cfg(test)] mod test { - use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; - use crate::geometry::{BroadPhase, ColliderSet, NarrowPhase}; + use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet}; + use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; + 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(); @@ -310,42 +297,65 @@ mod test { let mut bf = BroadPhase::new(); let mut nf = NarrowPhase::new(); - let mut set = RigidBodySet::new(); - let rb = RigidBodyBuilder::new_dynamic().build(); + let mut bodies = RigidBodySet::new(); // Check that removing the body right after inserting it works. - let h1 = set.insert(rb.clone()); - pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + // We add two dynamic bodies, one kinematic body and one static body before removing + // them. This include a non-regression test where deleting a kimenatic body crashes. + let rb = RigidBodyBuilder::new_dynamic().build(); + let h1 = bodies.insert(rb.clone()); + let h2 = bodies.insert(rb.clone()); + + // The same but with a kinematic body. + let rb = RigidBodyBuilder::new_kinematic().build(); + let h3 = bodies.insert(rb.clone()); + + // The same but with a static body. + let rb = RigidBodyBuilder::new_static().build(); + let h4 = bodies.insert(rb.clone()); + + let to_delete = [h1, h2, h3, h4]; + for h in &to_delete { + bodies.remove(*h, &mut colliders, &mut joints); + } + + pipeline.step( + &Vector::zeros(), + &IntegrationParameters::default(), + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut joints, + &(), + ); } #[test] fn rigid_body_removal_snapshot_handle_determinism() { 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 set = RigidBodySet::new(); + let mut bodies = RigidBodySet::new(); let rb = RigidBodyBuilder::new_dynamic().build(); - let h1 = set.insert(rb.clone()); - let h2 = set.insert(rb.clone()); - let h3 = set.insert(rb.clone()); + let h1 = bodies.insert(rb.clone()); + let h2 = bodies.insert(rb.clone()); + let h3 = bodies.insert(rb.clone()); - pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); - pipeline.remove_rigid_body(h3, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); - pipeline.remove_rigid_body(h2, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + bodies.remove(h1, &mut colliders, &mut joints); + bodies.remove(h3, &mut colliders, &mut joints); + bodies.remove(h2, &mut colliders, &mut joints); - let ser_set = bincode::serialize(&set).unwrap(); - let mut set2: RigidBodySet = bincode::deserialize(&ser_set).unwrap(); + let ser_bodies = bincode::serialize(&bodies).unwrap(); + let mut bodies2: RigidBodySet = bincode::deserialize(&ser_bodies).unwrap(); - let h1a = set.insert(rb.clone()); - let h2a = set.insert(rb.clone()); - let h3a = set.insert(rb.clone()); + let h1a = bodies.insert(rb.clone()); + let h2a = bodies.insert(rb.clone()); + let h3a = bodies.insert(rb.clone()); - let h1b = set2.insert(rb.clone()); - let h2b = set2.insert(rb.clone()); - let h3b = set2.insert(rb.clone()); + let h1b = bodies2.insert(rb.clone()); + let h2b = bodies2.insert(rb.clone()); + let h3b = bodies2.insert(rb.clone()); assert_eq!(h1a, h1b); assert_eq!(h2a, h2b); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs new file mode 100644 index 0000000..32f59fc --- /dev/null +++ b/src/pipeline/query_pipeline.rs @@ -0,0 +1,113 @@ +use crate::dynamics::RigidBodySet; +use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree}; + +/// A pipeline for performing queries on all the colliders of a scene. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct QueryPipeline { + quadtree: WQuadtree, + tree_built: bool, + dilation_factor: f32, +} + +impl Default for QueryPipeline { + fn default() -> Self { + Self::new() + } +} + +impl QueryPipeline { + /// Initializes an empty query pipeline. + pub fn new() -> Self { + Self { + quadtree: WQuadtree::new(), + tree_built: false, + dilation_factor: 0.01, + } + } + + /// Update the acceleration structure on the query pipeline. + pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { + if !self.tree_built { + let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + // FIXME: uncomment this once we handle insertion/removals properly. + // self.tree_built = true; + return; + } + + for (_, body) in bodies + .iter_active_dynamic() + .chain(bodies.iter_active_kinematic()) + { + for handle in &body.colliders { + self.quadtree.pre_update(*handle) + } + } + + self.quadtree.update(colliders, self.dilation_factor); + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + pub fn cast_ray<'a>( + &self, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: f32, + ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { + // TODO: avoid allocation? + let mut inter = Vec::new(); + self.quadtree.cast_ray(ray, max_toi, &mut inter); + + let mut best = f32::MAX; + let mut result = None; + + for handle in inter { + let collider = &colliders[handle]; + if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if inter.toi < best { + best = inter.toi; + result = Some((handle, collider, inter)); + } + } + } + + result + } + + /// Find the all intersections between a ray and a set of collider and passes them to a callback. + /// + /// # Parameters + /// - `position`: the position of this shape. + /// - `ray`: the ray to cast. + /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. + /// - `callback`: function executed on each collider for which a ray intersection has been found. + /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, + /// this method will exit early, ignory any further raycast. + pub fn interferences_with_ray<'a>( + &self, + colliders: &'a ColliderSet, + ray: &Ray, + max_toi: f32, + mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, + ) { + // TODO: avoid allocation? + let mut inter = Vec::new(); + self.quadtree.cast_ray(ray, max_toi, &mut inter); + + for handle in inter { + let collider = &colliders[handle]; + if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if !callback(handle, collider, inter) { + return; + } + } + } + } +} diff --git a/src/utils.rs b/src/utils.rs index 91ce41c..ecdd4fd 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -5,13 +5,15 @@ use crate::dynamics::RigidBodyHandle; use indexmap::IndexMap as HashMap; use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; use num::Zero; -#[cfg(feature = "simd-is-enabled")] use simba::simd::SimdValue; #[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))] use std::collections::HashMap; use std::ops::{Add, Mul}; -#[cfg(feature = "simd-is-enabled")] -use {crate::simd::SimdFloat, na::SimdPartialOrd, num::One}; +use { + crate::simd::{SimdBool, SimdFloat}, + na::SimdPartialOrd, + num::One, +}; // pub(crate) const SIN_10_DEGREES: f32 = 0.17364817766; // pub(crate) const COS_10_DEGREES: f32 = 0.98480775301; @@ -31,6 +33,16 @@ pub(crate) fn inv(val: f32) -> f32 { } } +/// Conditionally swaps each lanes of `a` with those of `b`. +/// +/// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then +/// `a.extract(i)` is swapped with `b.extract(i)`. +pub fn simd_swap(do_swap: SimdBool, a: &mut SimdFloat, b: &mut SimdFloat) { + let _a = *a; + *a = b.select(do_swap, *a); + *b = _a.select(do_swap, *b); +} + /// Trait to copy the sign of each component of one scalar/vector/matrix to another. pub trait WSign: Sized { // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652 @@ -77,7 +89,6 @@ impl> WSign> for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WSign for SimdFloat { fn copy_sign_to(self, to: SimdFloat) -> SimdFloat { self.simd_copysign(to) @@ -102,7 +113,6 @@ impl WComponent for f32 { } } -#[cfg(feature = "simd-is-enabled")] impl WComponent for SimdFloat { type Element = f32; @@ -319,7 +329,6 @@ impl WDot for f32 { } } -#[cfg(feature = "simd-is-enabled")] impl WCrossMatrix for Vector3 { type CrossMat = Matrix3; @@ -334,7 +343,6 @@ impl WCrossMatrix for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WCrossMatrix for Vector2 { type CrossMat = Vector2; @@ -344,7 +352,6 @@ impl WCrossMatrix for Vector2 { } } -#[cfg(feature = "simd-is-enabled")] impl WCross> for Vector3 { type Result = Vector3; @@ -353,7 +360,6 @@ impl WCross> for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WCross> for SimdFloat { type Result = Vector2; @@ -362,7 +368,6 @@ impl WCross> for SimdFloat { } } -#[cfg(feature = "simd-is-enabled")] impl WCross> for Vector2 { type Result = SimdFloat; @@ -373,7 +378,6 @@ impl WCross> for Vector2 { } } -#[cfg(feature = "simd-is-enabled")] impl WDot> for Vector3 { type Result = SimdFloat; @@ -382,7 +386,6 @@ impl WDot> for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WDot> for Vector2 { type Result = SimdFloat; @@ -391,7 +394,6 @@ impl WDot> for Vector2 { } } -#[cfg(feature = "simd-is-enabled")] impl WDot for SimdFloat { type Result = SimdFloat; @@ -445,7 +447,6 @@ impl WAngularInertia for f32 { } } -#[cfg(feature = "simd-is-enabled")] impl WAngularInertia for SimdFloat { type AngVector = SimdFloat; type LinVector = Vector2; @@ -860,7 +861,6 @@ impl WAngularInertia for SdpMatrix3 { } } -#[cfg(feature = "simd-is-enabled")] impl WAngularInertia for SdpMatrix3 { type AngVector = Vector3; type LinVector = Vector3; diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index 07cef1e..c25ff1f 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -98,10 +98,10 @@ impl Box2dWorld { fn insert_joints(&mut self, joints: &JointSet) { for joint in joints.iter() { - let body_a = self.rapier2box2d[&joint.body1]; - let body_b = self.rapier2box2d[&joint.body2]; + let body_a = self.rapier2box2d[&joint.1.body1]; + let body_b = self.rapier2box2d[&joint.1.body2]; - match &joint.params { + match &joint.1.params { JointParams::BallJoint(params) => { let def = RevoluteJointDef { body_a, @@ -158,7 +158,7 @@ impl Box2dWorld { } fn create_fixture(collider: &Collider, body: &mut b2::MetaBody) { - let center = na_vec_to_b2_vec(collider.delta().translation.vector); + let center = na_vec_to_b2_vec(collider.position_wrt_parent().translation.vector); let mut fixture_def = b2::FixtureDef::new(); fixture_def.restitution = 0.0; @@ -182,7 +182,7 @@ impl Box2dWorld { let points: Vec<_> = poly .vertices() .iter() - .map(|p| collider.delta() * p) + .map(|p| collider.position_wrt_parent() * p) .map(|p| na_vec_to_b2_vec(p.coords)) .collect(); let b2_shape = b2::PolygonShape::new_with(&points); @@ -229,7 +229,7 @@ impl Box2dWorld { for coll_handle in body.colliders() { let collider = &mut colliders[*coll_handle]; - collider.set_position_debug(pos * collider.delta()); + collider.set_position_debug(pos * collider.position_wrt_parent()); } } } diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 62ab37e..217da48 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/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index c9ff2d1..c2a7fb1 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -66,10 +66,10 @@ impl NPhysicsWorld { } for joint in joints.iter() { - let b1 = BodyPartHandle(rapier2nphysics[&joint.body1], 0); - let b2 = BodyPartHandle(rapier2nphysics[&joint.body2], 0); + let b1 = BodyPartHandle(rapier2nphysics[&joint.1.body1], 0); + let b2 = BodyPartHandle(rapier2nphysics[&joint.1.body2], 0); - match &joint.params { + match &joint.1.params { JointParams::FixedJoint(params) => { let c = FixedConstraint::new( b1, diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index b06fd7e..ec2e2bf 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -196,7 +196,7 @@ impl PhysxWorld { } } else { physx_sys::PxShapeFlags { - mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8, + mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8, // | physx_sys::PxShapeFlag::eSCENE_QUERY_SHAPE as u8, } }; @@ -261,10 +261,10 @@ impl PhysxWorld { fn setup_joints(&mut self, joints: &JointSet) { unsafe { for joint in joints.iter() { - let actor1 = self.rapier2physx[&joint.body1]; - let actor2 = self.rapier2physx[&joint.body2]; + let actor1 = self.rapier2physx[&joint.1.body1]; + let actor2 = self.rapier2physx[&joint.1.body2]; - match &joint.params { + match &joint.1.params { JointParams::BallJoint(params) => { let frame1 = physx::transform::gl_to_px_tf( Isometry3::new(params.local_anchor1.coords, na::zero()).into_glam(), diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 92aea5a..3d7fd7d 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -21,9 +21,9 @@ use na::{self, Point2, Point3, Vector3}; use rapier::dynamics::{ ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent}; +use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray}; use rapier::math::Vector; -use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline}; +use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; #[cfg(feature = "fluids")] use salva::{coupling::ColliderCouplingSet, object::FluidHandle, LiquidWorld}; @@ -179,6 +179,7 @@ pub struct PhysicsState { pub colliders: ColliderSet, pub joints: JointSet, pub pipeline: PhysicsPipeline, + pub query_pipeline: QueryPipeline, } impl PhysicsState { @@ -190,6 +191,7 @@ impl PhysicsState { colliders: ColliderSet::new(), joints: JointSet::new(), pipeline: PhysicsPipeline::new(), + query_pipeline: QueryPipeline::new(), } } } @@ -197,6 +199,7 @@ impl PhysicsState { pub struct TestbedState { pub running: RunMode, pub draw_colls: bool, + pub highlighted_body: Option, // pub grabbed_object: Option, // pub grabbed_object_constraint: Option, pub grabbed_object_plane: (Point3, Vector3), @@ -240,7 +243,7 @@ pub struct Testbed { hide_counters: bool, // persistant_contacts: HashMap, font: Rc, - // cursor_pos: Point2, + cursor_pos: Point2, events: PhysicsEvents, event_handler: ChannelEventCollector, ui: Option, @@ -301,6 +304,7 @@ impl Testbed { let state = TestbedState { running: RunMode::Running, draw_colls: false, + highlighted_body: None, // grabbed_object: None, // grabbed_object_constraint: None, grabbed_object_plane: (Point3::origin(), na::zero()), @@ -349,7 +353,7 @@ impl Testbed { hide_counters: true, // persistant_contacts: HashMap::new(), font: Font::default(), - // cursor_pos: Point2::new(0.0f32, 0.0), + cursor_pos: Point2::new(0.0f32, 0.0), ui, event_handler, events, @@ -408,6 +412,9 @@ impl Testbed { .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true); self.time = 0.0; self.state.timestep_id = 0; + self.state.highlighted_body = None; + self.physics.query_pipeline = QueryPipeline::new(); + self.physics.pipeline = PhysicsPipeline::new(); self.physics.pipeline.counters.enable(); #[cfg(all(feature = "dim2", feature = "other-backends"))] @@ -668,6 +675,10 @@ impl Testbed { &self.event_handler, ); + self.physics + .query_pipeline + .update(&self.physics.bodies, &self.physics.colliders); + #[cfg(feature = "fluids")] { fluids_time = instant::now(); @@ -802,13 +813,9 @@ impl Testbed { let num_to_delete = (colliders.len() / 10).max(1); for to_delete in &colliders[..num_to_delete] { - self.physics.pipeline.remove_collider( - to_delete[0], - &mut self.physics.broad_phase, - &mut self.physics.narrow_phase, - &mut self.physics.bodies, - &mut self.physics.colliders, - ); + self.physics + .colliders + .remove(to_delete[0], &mut self.physics.bodies); } } WindowEvent::Key(Key::D, Action::Release, _) => { @@ -817,21 +824,22 @@ 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); for to_delete in &dynamic_bodies[..num_to_delete] { - self.physics.pipeline.remove_rigid_body( + self.physics.bodies.remove( *to_delete, - &mut self.physics.broad_phase, - &mut self.physics.narrow_phase, - &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, ); } } + WindowEvent::CursorPos(x, y, _) => { + self.cursor_pos.x = x as f32; + self.cursor_pos.y = y as f32; + } _ => {} } @@ -1146,37 +1154,46 @@ impl Testbed { self.state.grabbed_object = None; self.state.grabbed_object_constraint = None; } - WindowEvent::CursorPos(x, y, modifiers) => { - self.cursor_pos.x = x as f32; - self.cursor_pos.y = y as f32; - - // update the joint - if let Some(joint) = self.state.grabbed_object_constraint { - let size = window.size(); - let (pos, dir) = self - .graphics - .camera() - .unproject(&self.cursor_pos, &na::convert(size)); - let (ref ppos, ref pdir) = self.state.grabbed_object_plane; - - if let Some(inter) = query::ray_toi_with_plane(ppos, pdir, &Ray::new(pos, dir)) - { - let joint = self - .constraints - .get_mut(joint) - .unwrap() - .downcast_mut::>() - .unwrap(); - joint.set_anchor_1(pos + dir * inter) - } - } - - event.inhibited = modifiers.contains(Modifiers::Shift); - } _ => {} } */ } + + #[cfg(feature = "dim2")] + fn highlight_hovered_body(&mut self, _window: &Window) { + // Do nothing for now. + } + + #[cfg(feature = "dim3")] + fn highlight_hovered_body(&mut self, window: &Window) { + if let Some(highlighted_body) = self.state.highlighted_body { + if let Some(nodes) = self.graphics.body_nodes_mut(highlighted_body) { + for node in nodes { + node.unselect() + } + } + } + + let size = window.size(); + let (pos, dir) = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(size)); + let ray = Ray::new(pos, dir); + let hit = self + .physics + .query_pipeline + .cast_ray(&self.physics.colliders, &ray, f32::MAX); + + if let Some((_, collider, _)) = hit { + if self.physics.bodies[collider.parent()].is_dynamic() { + self.state.highlighted_body = Some(collider.parent()); + for node in self.graphics.body_nodes_mut(collider.parent()).unwrap() { + node.select() + } + } + } + } } type CameraEffects<'a> = ( @@ -1287,9 +1304,9 @@ impl State for Testbed { if let Ok(w) = snapshot.restore() { self.clear(window); self.graphics.clear(window); + self.set_world(w.3, w.4, w.5); self.physics.broad_phase = w.1; self.physics.narrow_phase = w.2; - self.set_world(w.3, w.4, w.5); self.state.timestep_id = w.0; } } @@ -1347,7 +1364,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; } } @@ -1446,6 +1463,10 @@ impl State for Testbed { &self.event_handler, ); + self.physics + .query_pipeline + .update(&self.physics.bodies, &self.physics.colliders); + #[cfg(feature = "fluids")] { fluids_time = instant::now(); @@ -1550,7 +1571,9 @@ impl State for Testbed { } } - self.graphics.draw(&self.physics.colliders, window); + self.highlight_hovered_body(window); + self.graphics + .draw(&self.physics.bodies, &self.physics.colliders, window); #[cfg(feature = "fluids")] { @@ -1627,26 +1650,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 ); }