From 46d976d97bc9334004a58a19bc9cab3ea78e9569 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 19 Nov 2022 16:05:46 +0100 Subject: [PATCH 01/10] Allow disabling colliders, rigid-bodies and impulse joints --- src/control/character_controller.rs | 2 +- src/dynamics/island_manager.rs | 7 +- src/dynamics/joint/generic_joint.rs | 38 +++ .../joint/impulse_joint/impulse_joint_set.rs | 46 +++- .../multibody_joint/multibody_joint_set.rs | 17 +- src/dynamics/rigid_body.rs | 34 +++ src/dynamics/rigid_body_components.rs | 16 +- .../broad_phase_multi_sap/broad_phase.rs | 16 +- src/geometry/collider.rs | 41 ++++ src/geometry/collider_components.rs | 18 ++ src/geometry/collider_set.rs | 25 ++ src/pipeline/collision_pipeline.rs | 11 + src/pipeline/physics_pipeline.rs | 13 + src/pipeline/query_pipeline.rs | 6 +- src/pipeline/user_changes.rs | 225 +++++++++++------- 15 files changed, 408 insertions(+), 107 deletions(-) diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index ebed952..28ba475 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -133,7 +133,7 @@ impl Default for KinematicCharacterController { pub struct EffectiveCharacterMovement { /// The movement to apply. pub translation: Vector, - /// Is the character touching the ground after applying `EffictiveKineamticMovement::translation`? + /// Is the character touching the ground after applying `EffectiveKineamticMovement::translation`? pub grounded: bool, } diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 1295f14..edd2579 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -98,7 +98,8 @@ impl IslandManager { let rb = bodies.index_mut_internal(handle); rb.activation.wake_up(strong); - if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) { + if rb.is_enabled() && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) + { rb.ids.active_set_id = self.active_dynamic_set.len(); self.active_dynamic_set.push(handle); } @@ -256,12 +257,12 @@ impl IslandManager { // in contact or joined with this collider. push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); - for inter in impulse_joints.attached_joints(handle) { + for inter in impulse_joints.attached_enabled_joints(handle) { let other = crate::utils::select_other((inter.0, inter.1), handle); self.stack.push(other); } - for other in multibody_joints.attached_bodies(handle) { + for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { self.stack.push(other); } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index bb1598d..4f0a791 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -5,6 +5,7 @@ use crate::utils::{WBasis, WReal}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; +use crate::geometry::ColliderEnabled; #[cfg(feature = "dim3")] bitflags::bitflags! { @@ -182,6 +183,19 @@ impl JointMotor { } } +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// Enum indicating whether or not a joint is enabled. +pub enum JointEnabled { + /// The joint is enabled. + Enabled, + /// The joint wasn’t disabled by the user explicitly but it is attached to + /// a disabled rigid-body. + DisabledByAttachedBody, + /// The joint is disabled by the user explicitly. + Disabled, +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] /// A generic joint. @@ -208,6 +222,8 @@ pub struct GenericJoint { pub motors: [JointMotor; SPATIAL_DIM], /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, + /// Whether or not the joint is enabled. + pub enabled: JointEnabled, } impl Default for GenericJoint { @@ -222,6 +238,7 @@ impl Default for GenericJoint { limits: [JointLimits::default(); SPATIAL_DIM], motors: [JointMotor::default(); SPATIAL_DIM], contacts_enabled: true, + enabled: JointEnabled::Enabled, } } } @@ -260,6 +277,27 @@ impl GenericJoint { } } + /// Is this joint enabled? + pub fn is_enabled(&self) -> bool { + self.enabled == JointEnabled::Enabled + } + + /// Set whether this joint is enabled or not. + pub fn set_enabled(&mut self, enabled: bool) { + match self.enabled { + JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => { + if !enabled { + self.enabled = JointEnabled::Disabled; + } + } + JointEnabled::Disabled => { + if enabled { + self.enabled = JointEnabled::Enabled; + } + } + } + } + /// Add the specified axes to the set of axes locked by this joint. pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self { self.locked_axes |= axes; diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 7409172..f21f950 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -72,11 +72,11 @@ impl ImpulseJointSet { } /// Iterates through all the joints between two rigid-bodies. - pub fn joints_between<'a>( - &'a self, + pub fn joints_between( + &self, body1: RigidBodyHandle, body2: RigidBodyHandle, - ) -> impl Iterator { + ) -> impl Iterator { self.rb_graph_ids .get(body1.0) .zip(self.rb_graph_ids.get(body2.0)) @@ -86,15 +86,15 @@ impl ImpulseJointSet { } /// Iterates through all the impulse joints attached to the given rigid-body. - pub fn attached_joints<'a>( - &'a self, + pub fn attached_joints( + &self, body: RigidBodyHandle, ) -> impl Iterator< Item = ( RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, - &'a ImpulseJoint, + &ImpulseJoint, ), > { self.rb_graph_ids @@ -104,6 +104,35 @@ impl ImpulseJointSet { .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2)) } + /// Iterates through all the impulse joints attached to the given rigid-body. + pub fn map_attached_joints_mut<'a>( + &'a mut self, + body: RigidBodyHandle, + mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint), + ) { + self.rb_graph_ids.get(body.0).into_iter().for_each(|id| { + for inter in self.joint_graph.interactions_with_mut(*id) { + (f)(inter.0, inter.1, inter.3.handle, inter.3) + } + }) + } + + /// Iterates through all the enabled impulse joints attached to the given rigid-body. + pub fn attached_enabled_joints( + &self, + body: RigidBodyHandle, + ) -> impl Iterator< + Item = ( + RigidBodyHandle, + RigidBodyHandle, + ImpulseJointHandle, + &ImpulseJoint, + ), + > { + self.attached_joints(body) + .filter(|inter| inter.3.data.is_enabled()) + } + /// Is the given joint handle valid? pub fn contains(&self, handle: ImpulseJointHandle) -> bool { self.joint_ids.contains(handle.0) @@ -246,7 +275,7 @@ impl ImpulseJointSet { ImpulseJointHandle(handle) } - /// Retrieve all the impulse_joints happening between two active bodies. + /// Retrieve all the enabled impulse joints happening between two active bodies. // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. pub(crate) fn select_active_interactions( &self, @@ -264,7 +293,8 @@ impl ImpulseJointSet { let rb1 = &bodies[joint.body1]; let rb2 = &bodies[joint.body2]; - if (rb1.is_dynamic() || rb2.is_dynamic()) + if joint.data.is_enabled() + && (rb1.is_dynamic() || rb2.is_dynamic()) && (!rb1.is_dynamic() || !rb1.is_sleeping()) && (!rb2.is_dynamic() || !rb2.is_sleeping()) { diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index c25de73..a4b338a 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -372,7 +372,7 @@ impl MultibodyJointSet { } /// Iterate through the handles of all the rigid-bodies attached to this rigid-body - /// by an multibody_joint. + /// by a multibody_joint. pub fn attached_bodies<'a>( &'a self, body: RigidBodyHandle, @@ -384,6 +384,21 @@ impl MultibodyJointSet { .map(move |inter| crate::utils::select_other((inter.0, inter.1), body)) } + /// Iterate through the handles of all the rigid-bodies attached to this rigid-body + /// by an enabled multibody_joint. + pub fn bodies_attached_with_enabled_joint<'a>( + &'a self, + body: RigidBodyHandle, + ) -> impl Iterator + 'a { + self.attached_bodies(body).filter(move |other| { + if let Some((_, _, link)) = self.joint_between(body, *other) { + link.joint.data.is_enabled() + } else { + false + } + }) + } + /// Iterates through all the multibodies on this set. pub fn multibodies(&self) -> impl Iterator { self.multibodies.iter().map(|e| e.1) diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9c38eee..a6384b7 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -35,6 +35,7 @@ pub struct RigidBody { pub(crate) body_type: RigidBodyType, /// The dominance group this rigid-body is part of. pub(crate) dominance: RigidBodyDominance, + pub(crate) enabled: bool, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -61,6 +62,7 @@ impl RigidBody { changes: RigidBodyChanges::all(), body_type: RigidBodyType::Dynamic, dominance: RigidBodyDominance::default(), + enabled: true, user_data: 0, } } @@ -81,6 +83,28 @@ impl RigidBody { &mut self.activation } + /// Is this rigid-body enabled? + pub fn is_enabled(&self) -> bool { + self.enabled + } + + /// Sets whether this rigid-body is enabled or not. + pub fn set_enabled(&mut self, enabled: bool) { + if enabled != self.enabled { + if enabled { + // NOTE: this is probably overkill, but it makes sure we don’t + // forget anything that needs to be updated because the rigid-body + // was basically interpreted as if it was removed while it was + // disabled. + self.changes = RigidBodyChanges::all(); + } else { + self.changes |= RigidBodyChanges::ENABLED_OR_DISABLED; + } + + self.enabled = enabled; + } + } + /// The linear damping coefficient of this rigid-body. #[inline] pub fn linear_damping(&self) -> Real { @@ -963,6 +987,8 @@ pub struct RigidBodyBuilder { pub ccd_enabled: bool, /// The dominance group of the rigid-body to be built. pub dominance_group: i8, + /// Will the rigid-body being built be enabled? + pub enabled: bool, /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. pub user_data: u128, } @@ -984,6 +1010,7 @@ impl RigidBodyBuilder { sleeping: false, ccd_enabled: false, dominance_group: 0, + enabled: true, user_data: 0, } } @@ -1230,6 +1257,12 @@ impl RigidBodyBuilder { self } + /// Enable or disable the rigid-body after its creation. + pub fn enabled(mut self, enabled: bool) -> Self { + self.enabled = enabled; + self + } + /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); @@ -1252,6 +1285,7 @@ impl RigidBodyBuilder { rb.damping.angular_damping = self.angular_damping; rb.forces.gravity_scale = self.gravity_scale; rb.dominance = RigidBodyDominance(self.dominance_group); + rb.enabled = self.enabled; rb.enable_ccd(self.ccd_enabled); if self.can_sleep && self.sleeping { diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index cabf6ca..eb94c6b 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -112,6 +112,8 @@ bitflags::bitflags! { const DOMINANCE = 1 << 5; /// Flag indicating that the local mass-properties of this rigid-body must be recomputed. const LOCAL_MASS_PROPERTIES = 1 << 6; + /// Flag indicating that the rigid-body was enabled or disabled. + const ENABLED_OR_DISABLED = 1 << 7; } } @@ -329,12 +331,14 @@ impl RigidBodyMassProps { for handle in &attached_colliders.0 { if let Some(co) = colliders.get(*handle) { - if let Some(co_parent) = co.parent { - let to_add = co - .mprops - .mass_properties(&*co.shape) - .transform_by(&co_parent.pos_wrt_parent); - self.local_mprops += to_add; + if co.is_enabled() { + if let Some(co_parent) = co.parent { + let to_add = co + .mprops + .mass_properties(&*co.shape) + .transform_by(&co_parent.pos_wrt_parent); + self.local_mprops += to_add; + } } } } diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs index 915017d..0667184 100644 --- a/src/geometry/broad_phase_multi_sap/broad_phase.rs +++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs @@ -176,7 +176,11 @@ impl BroadPhase { /// This method will actually remove from the proxy list all the proxies /// marked as deletable by `self.predelete_proxy`, making their proxy /// handles re-usable by new proxies. - fn complete_removals(&mut self, removed_colliders: &[ColliderHandle]) { + fn complete_removals( + &mut self, + colliders: &mut ColliderSet, + removed_colliders: &[ColliderHandle], + ) { // If there is no layer, there is nothing to remove. if self.layers.is_empty() { return; @@ -224,6 +228,11 @@ impl BroadPhase { self.proxies.remove(proxy_id); } } + + if let Some(co) = colliders.get_mut_internal(*removed) { + // Reset the proxy index. + co.bf_data.proxy_index = crate::INVALID_U32; + } } } @@ -460,9 +469,10 @@ impl BroadPhase { // NOTE: we use `get` because the collider may no longer // exist if it has been removed. if let Some(co) = colliders.get_mut_internal(*handle) { - if !co.changes.needs_broad_phase_update() { + if !co.is_enabled() || !co.changes.needs_broad_phase_update() { continue; } + let mut new_proxy_id = co.bf_data.proxy_index; if self.handle_modified_collider( @@ -496,7 +506,7 @@ impl BroadPhase { // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller // layers to possible remove regions from larger layers that would become empty that way. - self.complete_removals(removed_colliders); + self.complete_removals(colliders, removed_colliders); } /// Propagate regions from the smallest layers up to the larger layers. diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 95ae273..6759b81 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -7,6 +7,7 @@ use crate::geometry::{ use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM}; use crate::parry::transformation::vhacd::VHACDParameters; use crate::pipeline::{ActiveEvents, ActiveHooks}; +use crate::prelude::ColliderEnabled; use na::Unit; use parry::bounding_volume::Aabb; use parry::shape::{Shape, TriMeshFlags}; @@ -154,6 +155,32 @@ impl Collider { } } + /// Is this collider enabled? + pub fn is_enabled(&self) -> bool { + match self.flags.enabled { + ColliderEnabled::Enabled => true, + _ => false, + } + } + + /// Sets whether or not this collider is enabled. + pub fn set_enabled(&mut self, enabled: bool) { + match self.flags.enabled { + ColliderEnabled::Enabled | ColliderEnabled::DisabledByParent => { + if !enabled { + self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED); + self.flags.enabled = ColliderEnabled::Disabled; + } + } + ColliderEnabled::Disabled => { + if enabled { + self.changes.insert(ColliderChanges::ENABLED_OR_DISABLED); + self.flags.enabled = ColliderEnabled::Enabled; + } + } + } + } + /// Sets the translational part of this collider's position. pub fn set_translation(&mut self, translation: Vector) { self.changes.insert(ColliderChanges::POSITION); @@ -402,6 +429,8 @@ pub struct ColliderBuilder { pub collision_groups: InteractionGroups, /// The solver groups for the collider being built. pub solver_groups: InteractionGroups, + /// Will the collider being built be enabled? + pub enabled: bool, /// The total force magnitude beyond which a contact force event can be emitted. pub contact_force_event_threshold: Real, } @@ -424,6 +453,7 @@ impl ColliderBuilder { active_collision_types: ActiveCollisionTypes::default(), active_hooks: ActiveHooks::empty(), active_events: ActiveEvents::empty(), + enabled: true, contact_force_event_threshold: 0.0, } } @@ -834,6 +864,12 @@ impl ColliderBuilder { self } + /// Enable or disable the collider after its creation. + pub fn enabled(mut self, enabled: bool) -> Self { + self.enabled = enabled; + self + } + /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { let shape = self.shape.clone(); @@ -849,6 +885,11 @@ impl ColliderBuilder { active_collision_types: self.active_collision_types, active_hooks: self.active_hooks, active_events: self.active_events, + enabled: if self.enabled { + ColliderEnabled::Enabled + } else { + ColliderEnabled::Disabled + }, }; let changes = ColliderChanges::all(); let pos = ColliderPosition(self.position); diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs index 29a24b5..0e65bac 100644 --- a/src/geometry/collider_components.rs +++ b/src/geometry/collider_components.rs @@ -64,6 +64,8 @@ bitflags::bitflags! { /// This flags is automatically set by the `PhysicsPipeline` when the `RigidBodyChanges::DOMINANCE` /// or `RigidBodyChanges::TYPE` of the parent rigid-body of this collider is detected. const PARENT_EFFECTIVE_DOMINANCE = 1 << 7; // NF update. + /// Flag indicating that whether or not the collider is enabled was changed. + const ENABLED_OR_DISABLED = 1 << 8; // BF & NF updates. } } @@ -372,6 +374,19 @@ impl Default for ActiveCollisionTypes { } } +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// Enum indicating whether or not a collider is enabled. +pub enum ColliderEnabled { + /// The collider is enabled. + Enabled, + /// The collider wasn’t disabled by the user explicitly but it is attached to + /// a disabled rigid-body. + DisabledByParent, + /// The collider is disabled by the user explicitly. + Disabled, +} + #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A set of flags for controlling collision/intersection filtering, modification, and events. @@ -389,6 +404,8 @@ pub struct ColliderFlags { pub active_hooks: ActiveHooks, /// The events enabled for this collider. pub active_events: ActiveEvents, + /// Whether or not the collider is enabled. + pub enabled: ColliderEnabled, } impl Default for ColliderFlags { @@ -399,6 +416,7 @@ impl Default for ColliderFlags { solver_groups: InteractionGroups::all(), active_hooks: ActiveHooks::empty(), active_events: ActiveEvents::empty(), + enabled: ColliderEnabled::Enabled, } } } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index b141445..1069f1b 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -41,6 +41,14 @@ impl ColliderSet { self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c)) } + /// Iterate through all the enabled colliders on this set. + pub fn iter_enabled(&self) -> impl Iterator { + self.colliders + .iter() + .map(|(h, c)| (ColliderHandle(h), c)) + .filter(|(_, c)| c.is_enabled()) + } + /// Iterates mutably through all the colliders on this set. #[cfg(not(feature = "dev-remove-slow-accessors"))] pub fn iter_mut(&mut self) -> impl Iterator { @@ -52,6 +60,12 @@ impl ColliderSet { }) } + /// Iterates mutably through all the enabled colliders on this set. + #[cfg(not(feature = "dev-remove-slow-accessors"))] + pub fn iter_enabled_mut(&mut self) -> impl Iterator { + self.iter_mut().filter(|(_, c)| c.is_enabled()) + } + /// The number of colliders on this set. pub fn len(&self) -> usize { self.colliders.len() @@ -268,6 +282,17 @@ impl ColliderSet { pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { self.colliders.get_mut(handle.0) } + + // Just a very long name instead of `.get_mut` to make sure + // this is really the method we wanted to use instead of `get_mut_internal`. + pub(crate) fn get_mut_internal_with_modification_tracking( + &mut self, + handle: ColliderHandle, + ) -> Option<&mut Collider> { + let result = self.colliders.get_mut(handle.0)?; + Self::mark_as_modified(handle, result, &mut self.modified_colliders); + Some(result) + } } impl Index for ColliderSet { diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index ac92a04..631388b 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -127,9 +127,20 @@ impl CollisionPipeline { None, bodies, colliders, + &mut ImpulseJointSet::new(), + &mut MultibodyJointSet::new(), &modified_bodies, &mut modified_colliders, ); + + // Disabled colliders are treated as if they were removed. + removed_colliders.extend( + modified_colliders + .iter() + .copied() + .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), + ); + self.detect_collisions( prediction_distance, broad_phase, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 391b39a..f05a3f0 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -422,6 +422,7 @@ impl PhysicsPipeline { // Apply modifications. let mut modified_colliders = colliders.take_modified(); let mut removed_colliders = colliders.take_removed(); + super::user_changes::handle_user_changes_to_colliders( bodies, colliders, @@ -433,10 +434,22 @@ impl PhysicsPipeline { Some(islands), bodies, colliders, + impulse_joints, + multibody_joints, &modified_bodies, &mut modified_colliders, ); + // Disabled colliders are treated as if they were removed. + // NOTE: this must be called here, after handle_user_changes_to_rigid_bodies to take into + // account colliders disabled because of their parent rigid-body. + removed_colliders.extend( + modified_colliders + .iter() + .copied() + .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), + ); + // TODO: do this only on user-change. // TODO: do we want some kind of automatic inverse kinematics? for multibody in &mut multibody_joints.multibodies { diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 00825e2..6682370 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -358,12 +358,12 @@ impl QueryPipeline { fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) { match self.mode { QueryPipelineMode::CurrentPosition => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { f(h, co.shape.compute_aabb(&co.pos)) } } QueryPipelineMode::SweepTestWithNextPosition => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { if let Some(co_parent) = co.parent { let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position; let next_position = rb_next_pos * co_parent.pos_wrt_parent; @@ -374,7 +374,7 @@ impl QueryPipeline { } } QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - for (h, co) in self.colliders.iter() { + for (h, co) in self.colliders.iter_enabled() { if let Some(co_parent) = co.parent { let rb = &self.bodies[co_parent.handle]; let predicted_pos = rb.pos.integrate_forces_and_velocities( diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index e7da68a..adfd023 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -1,7 +1,10 @@ use crate::dynamics::{ - IslandManager, RigidBodyChanges, RigidBodyHandle, RigidBodySet, RigidBodyType, + ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges, + RigidBodyHandle, RigidBodySet, RigidBodyType, +}; +use crate::geometry::{ + ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet, }; -use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet}; pub(crate) fn handle_user_changes_to_colliders( bodies: &mut RigidBodySet, @@ -21,10 +24,12 @@ pub(crate) fn handle_user_changes_to_colliders( } } - if co - .changes - .intersects(ColliderChanges::SHAPE | ColliderChanges::LOCAL_MASS_PROPERTIES) - { + if co.changes.intersects( + ColliderChanges::SHAPE + | ColliderChanges::LOCAL_MASS_PROPERTIES + | ColliderChanges::ENABLED_OR_DISABLED + | ColliderChanges::PARENT, + ) { if let Some(rb) = co .parent .and_then(|p| bodies.get_mut_internal_with_modification_tracking(p.handle)) @@ -40,12 +45,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( mut islands: Option<&mut IslandManager>, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, modified_bodies: &[RigidBodyHandle], modified_colliders: &mut Vec, ) { enum FinalAction { - UpdateActiveKinematicSetId, - UpdateActiveDynamicSetId, + UpdateActiveKinematicSetId(usize), + UpdateActiveDynamicSetId(usize), + RemoveFromIsland, } for handle in modified_bodies { @@ -62,89 +70,95 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( let mut activation = rb.activation; { - // The body's status changed. We need to make sure - // it is on the correct active set. - if let Some(islands) = islands.as_deref_mut() { - if changes.contains(RigidBodyChanges::TYPE) { - match rb.body_type { - RigidBodyType::Dynamic => { - // Remove from the active kinematic set if it was there. - if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { - islands.active_kinematic_set.swap_remove(ids.active_set_id); - final_action = Some(( - FinalAction::UpdateActiveKinematicSetId, - ids.active_set_id, - )); - } + if rb.is_enabled() { + // The body's status changed. We need to make sure + // it is on the correct active set. + if let Some(islands) = islands.as_deref_mut() { + if changes.contains(RigidBodyChanges::TYPE) { + match rb.body_type { + RigidBodyType::Dynamic => { + // Remove from the active kinematic set if it was there. + if islands.active_kinematic_set.get(ids.active_set_id) + == Some(handle) + { + islands.active_kinematic_set.swap_remove(ids.active_set_id); + final_action = Some(FinalAction::UpdateActiveKinematicSetId( + ids.active_set_id, + )); + } - // Add to the active dynamic set. - activation.wake_up(true); - // Make sure the sleep change flag is set (even if for some - // reasons the rigid-body was already awake) to make - // sure the code handling sleeping change adds the body to - // the active_dynamic_set. - changes.set(RigidBodyChanges::SLEEP, true); - } - RigidBodyType::KinematicVelocityBased - | RigidBodyType::KinematicPositionBased => { - // Remove from the active dynamic set if it was there. - if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) { - islands.active_dynamic_set.swap_remove(ids.active_set_id); - final_action = Some(( - FinalAction::UpdateActiveDynamicSetId, - ids.active_set_id, - )); + // Add to the active dynamic set. + activation.wake_up(true); + // Make sure the sleep change flag is set (even if for some + // reasons the rigid-body was already awake) to make + // sure the code handling sleeping change adds the body to + // the active_dynamic_set. + changes.set(RigidBodyChanges::SLEEP, true); } + RigidBodyType::KinematicVelocityBased + | RigidBodyType::KinematicPositionBased => { + // Remove from the active dynamic set if it was there. + if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) + { + islands.active_dynamic_set.swap_remove(ids.active_set_id); + final_action = Some(FinalAction::UpdateActiveDynamicSetId( + ids.active_set_id, + )); + } - // Add to the active kinematic set. - if islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); + // Add to the active kinematic set. + if islands.active_kinematic_set.get(ids.active_set_id) + != Some(handle) + { + ids.active_set_id = islands.active_kinematic_set.len(); + islands.active_kinematic_set.push(*handle); + } } + RigidBodyType::Fixed => {} } - RigidBodyType::Fixed => {} } - } - // Update the positions of the colliders. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.colliders - .update_positions(colliders, modified_colliders, &rb.pos.position); - - if rb.is_kinematic() - && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) + // Update the active kinematic set. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); + if rb.is_kinematic() + && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) + { + ids.active_set_id = islands.active_kinematic_set.len(); + islands.active_kinematic_set.push(*handle); + } + } + + // Push the body to the active set if it is not + // sleeping and if it is not already inside of the active set. + if changes.contains(RigidBodyChanges::SLEEP) + && rb.is_enabled() + && !activation.sleeping // May happen if the body was put to sleep manually. + && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. + && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) + { + ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. + islands.active_dynamic_set.push(*handle); } } + } - // Push the body to the active set if it is not - // sleeping and if it is not already inside of the active set. - if changes.contains(RigidBodyChanges::SLEEP) - && !activation.sleeping // May happen if the body was put to sleep manually. - && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. - && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) - { - ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. - islands.active_dynamic_set.push(*handle); - } - } else { - // We don't use islands. So just update the colliders' positions. - if changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS) - { - rb.colliders - .update_positions(colliders, modified_colliders, &rb.pos.position); - } + // Update the colliders' positions. + if changes.contains(RigidBodyChanges::POSITION) + || changes.contains(RigidBodyChanges::COLLIDERS) + { + rb.colliders + .update_positions(colliders, modified_colliders, &rb.pos.position); } if changes.contains(RigidBodyChanges::DOMINANCE) || changes.contains(RigidBodyChanges::TYPE) { for handle in rb.colliders.0.iter() { + // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking` + // here because that would modify the `modified_colliders` inside of the `ColliderSet` + // instead of the one passed to this method. let co = colliders.index_mut_internal(*handle); if !co.changes.contains(ColliderChanges::MODIFIED) { modified_colliders.push(*handle); @@ -165,6 +179,43 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( ); } + if changes.contains(RigidBodyChanges::ENABLED_OR_DISABLED) { + // Propagate the rigid-body’s enabled/disable status to its colliders. + for handle in rb.colliders.0.iter() { + // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking` + // here because that would modify the `modified_colliders` inside of the `ColliderSet` + // instead of the one passed to this method. + let co = colliders.index_mut_internal(*handle); + if !co.changes.contains(ColliderChanges::MODIFIED) { + modified_colliders.push(*handle); + } + + if rb.enabled && co.flags.enabled == ColliderEnabled::DisabledByParent { + co.flags.enabled = ColliderEnabled::Enabled; + } else if !rb.enabled && co.flags.enabled == ColliderEnabled::Enabled { + co.flags.enabled = ColliderEnabled::DisabledByParent; + } + + co.changes |= ColliderChanges::MODIFIED | ColliderChanges::ENABLED_OR_DISABLED; + } + + // Propagate the rigid-body’s enabled/disable status to its attached impulse joints. + impulse_joints.map_attached_joints_mut(*handle, |_, _, _, joint| { + if rb.enabled && joint.data.enabled == JointEnabled::DisabledByAttachedBody { + joint.data.enabled = JointEnabled::Enabled; + } else if !rb.enabled && joint.data.enabled == JointEnabled::Enabled { + joint.data.enabled = JointEnabled::DisabledByAttachedBody; + } + }); + + // FIXME: Propagate the rigid-body’s enabled/disable status to its attached multibody joints. + + // Remove the rigid-body from the island manager. + if !rb.enabled { + final_action = Some(FinalAction::RemoveFromIsland); + } + } + rb.changes = RigidBodyChanges::empty(); rb.ids = ids; rb.activation = activation; @@ -172,15 +223,25 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // Adjust some ids, if needed. if let Some(islands) = islands.as_deref_mut() { - if let Some((action, id)) = final_action { - let active_set = match action { - FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set, - FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set, + if let Some(action) = final_action { + match action { + FinalAction::RemoveFromIsland => { + let ids = rb.ids; + islands.rigid_body_removed(*handle, &ids, bodies); + } + FinalAction::UpdateActiveKinematicSetId(id) => { + let active_set = &mut islands.active_kinematic_set; + if id < active_set.len() { + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; + } + } + FinalAction::UpdateActiveDynamicSetId(id) => { + let active_set = &mut islands.active_dynamic_set; + if id < active_set.len() { + bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; + } + } }; - - if id < active_set.len() { - bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; - } } } } From 683baf6bf77cfb41227ea6ed4a42499d1e051cdf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 26 Nov 2022 17:45:14 +0100 Subject: [PATCH 02/10] Allow the PhysicsPipeline and CollisionPipeline to update the QueryPipeline incrementally --- benchmarks3d/all_benchmarks3.rs | 2 + benchmarks3d/many_static3.rs | 52 +++++++++++++ src/dynamics/ccd/ccd_solver.rs | 2 - src/dynamics/joint/generic_joint.rs | 1 - src/geometry/broad_phase_qbvh.rs | 88 +++++++++++++++++++++ src/geometry/collider_set.rs | 1 + src/geometry/mod.rs | 6 +- src/pipeline/collision_pipeline.rs | 7 +- src/pipeline/physics_pipeline.rs | 24 ++++-- src/pipeline/query_pipeline.rs | 116 +++++++++------------------- src/pipeline/user_changes.rs | 2 +- src_testbed/harness/mod.rs | 8 +- 12 files changed, 212 insertions(+), 97 deletions(-) create mode 100644 benchmarks3d/many_static3.rs create mode 100644 src/geometry/broad_phase_qbvh.rs diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs index 6a3f756..bca730b 100644 --- a/benchmarks3d/all_benchmarks3.rs +++ b/benchmarks3d/all_benchmarks3.rs @@ -20,6 +20,7 @@ mod joint_fixed3; mod joint_prismatic3; mod joint_revolute3; mod keva3; +mod many_static3; mod pyramid3; mod stacks3; mod trimesh3; @@ -54,6 +55,7 @@ pub fn main() { ("CCD", ccd3::init_world), ("Compound", compound3::init_world), ("Convex polyhedron", convex_polyhedron3::init_world), + ("Many static", many_static3::init_world), ("Heightfield", heightfield3::init_world), ("Stacks", stacks3::init_world), ("Pyramid", pyramid3::init_world), diff --git a/benchmarks3d/many_static3.rs b/benchmarks3d/many_static3.rs new file mode 100644 index 0000000..3a0dbea --- /dev/null +++ b/benchmarks3d/many_static3.rs @@ -0,0 +1,52 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Create the balls + */ + let num = 50; + let rad = 1.0; + + let shift = rad * 2.0 + 1.0; + let centerx = shift * (num as f32) / 2.0; + let centery = shift / 2.0; + let centerz = shift * (num as f32) / 2.0; + + for i in 0..num { + for j in 0usize..num { + for k in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift - centerz; + + let status = if j < num - 1 { + RigidBodyType::Fixed + } else { + RigidBodyType::Dynamic + }; + let density = 0.477; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y, z]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(density); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); +} diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 9e0ab8e..9b06a80 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -123,7 +123,6 @@ impl CCDSolver { ) -> Option { // Update the query pipeline. self.query_pipeline.update_with_mode( - islands, bodies, colliders, QueryPipelineMode::SweepTestWithPredictedPosition { dt }, @@ -245,7 +244,6 @@ impl CCDSolver { // Update the query pipeline. self.query_pipeline.update_with_mode( - islands, bodies, colliders, QueryPipelineMode::SweepTestWithNextPosition, diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 4f0a791..cbd9649 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -5,7 +5,6 @@ use crate::utils::{WBasis, WReal}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; -use crate::geometry::ColliderEnabled; #[cfg(feature = "dim3")] bitflags::bitflags! { diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs new file mode 100644 index 0000000..22ca562 --- /dev/null +++ b/src/geometry/broad_phase_qbvh.rs @@ -0,0 +1,88 @@ +use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet}; +use parry::bounding_volume::BoundingVolume; +use parry::math::Real; +use parry::partitioning::Qbvh; +use parry::partitioning::QbvhUpdateWorkspace; +use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct BroadPhase { + qbvh: Qbvh, + stack: Vec<(u32, u32)>, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + workspace: QbvhUpdateWorkspace, +} + +impl Default for BroadPhase { + fn default() -> Self { + Self::new() + } +} + +impl BroadPhase { + pub fn new() -> Self { + Self { + qbvh: Qbvh::new(), + stack: vec![], + workspace: QbvhUpdateWorkspace::default(), + } + } + + #[allow(dead_code)] // This broad-phase is just experimental right now. + pub fn update( + &mut self, + prediction_distance: Real, + colliders: &ColliderSet, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + events: &mut Vec, + ) { + let margin = 0.01; + + if modified_colliders.is_empty() { + return; + } + + // Visitor to find collision pairs. + let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new( + |co1: &ColliderHandle, co2: &ColliderHandle| { + events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2))); + true + }, + ); + + let full_rebuild = self.qbvh.raw_nodes().is_empty(); + + if full_rebuild { + self.qbvh.clear_and_rebuild( + colliders.iter().map(|(handle, collider)| { + ( + handle, + collider.compute_aabb().loosened(prediction_distance / 2.0), + ) + }), + margin, + ); + self.qbvh + .traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack); + } else { + for modified in modified_colliders { + self.qbvh.pre_update_or_insert(*modified); + } + + for removed in removed_colliders { + self.qbvh.remove(*removed); + } + + let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| { + colliders[*handle] + .compute_aabb() + .loosened(prediction_distance / 2.0) + }); + self.qbvh + .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack); + self.qbvh.rebalance(margin, &mut self.workspace); + } + } +} diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 1069f1b..4bc2a03 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -285,6 +285,7 @@ impl ColliderSet { // Just a very long name instead of `.get_mut` to make sure // this is really the method we wanted to use instead of `get_mut_internal`. + #[allow(dead_code)] pub(crate) fn get_mut_internal_with_modification_tracking( &mut self, handle: ColliderHandle, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4f3ada8..a91c4b9 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,6 +1,9 @@ //! Structures related to geometry: colliders, shapes, etc. -pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair}; +pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; + +pub use self::broad_phase_multi_sap::BroadPhase; +// pub use self::broad_phase_qbvh::BroadPhase; pub use self::collider_components::*; pub use self::contact_pair::{ ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags, @@ -199,5 +202,6 @@ mod interaction_graph; mod interaction_groups; mod narrow_phase; +mod broad_phase_qbvh; mod collider; mod collider_set; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 631388b..d0beedd 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -5,7 +5,7 @@ use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase, }; use crate::math::Real; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline}; use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -111,6 +111,7 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + query_pipeline: Option<&mut QueryPipeline>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -154,6 +155,10 @@ impl CollisionPipeline { true, ); + if let Some(queries) = query_pipeline { + queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); removed_colliders.clear(); } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f05a3f0..7226063 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -14,7 +14,7 @@ use crate::geometry::{ ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex, }; use crate::math::{Real, Vector}; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline}; use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; /// The physics pipeline, responsible for stepping the whole physics simulation. @@ -404,6 +404,7 @@ impl PhysicsPipeline { impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, ccd_solver: &mut CCDSolver, + mut query_pipeline: Option<&mut QueryPipeline>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -468,13 +469,17 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, - &modified_colliders[..], - &mut removed_colliders, + &modified_colliders, + &removed_colliders, hooks, events, true, ); + if let Some(queries) = query_pipeline.as_deref_mut() { + queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); removed_colliders.clear(); @@ -595,13 +600,22 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, - &mut modified_colliders, - &mut removed_colliders, + &modified_colliders, + &[], hooks, events, false, ); + if let Some(queries) = query_pipeline.as_deref_mut() { + queries.update_incremental( + colliders, + &modified_colliders, + &[], + remaining_substeps == 0, + ); + } + self.clear_modified_colliders(colliders, &mut modified_colliders); } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 6682370..8a667de 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,10 +1,10 @@ -use crate::dynamics::{IslandManager, RigidBodyHandle}; +use crate::dynamics::RigidBodyHandle; use crate::geometry::{ Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection, }; use crate::math::{Isometry, Point, Real, Vector}; use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; -use parry::partitioning::QbvhDataGenerator; +use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace}; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -30,8 +30,9 @@ pub struct QueryPipeline { )] query_dispatcher: Arc, qbvh: Qbvh, - tree_built: bool, dilation_factor: Real, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + workspace: QbvhUpdateWorkspace, } struct QueryPipelineAsCompositeShape<'a> { @@ -310,8 +311,8 @@ impl QueryPipeline { Self { query_dispatcher: Arc::new(d), qbvh: Qbvh::new(), - tree_built: false, dilation_factor: 0.01, + workspace: QbvhUpdateWorkspace::default(), } } @@ -320,25 +321,39 @@ impl QueryPipeline { &*self.query_dispatcher } - /// Update the acceleration structure on the query pipeline. - pub fn update( + /// Update the query pipeline incrementally, avoiding a complete rebuild of its + /// internal data-structure. + pub fn update_incremental( &mut self, - islands: &IslandManager, - bodies: &RigidBodySet, colliders: &ColliderSet, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + refit_and_rebalance: bool, ) { - self.update_with_mode( - islands, - bodies, - colliders, - QueryPipelineMode::CurrentPosition, - ) + for modified in modified_colliders { + self.qbvh.pre_update_or_insert(*modified); + } + + for removed in removed_colliders { + self.qbvh.remove(*removed); + } + + if refit_and_rebalance { + let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { + colliders[*handle].compute_aabb() + }); + self.qbvh.rebalance(0.0, &mut self.workspace); + } + } + + /// Update the acceleration structure on the query pipeline. + pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { + self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition) } /// Update the acceleration structure on the query pipeline. pub fn update_with_mode( &mut self, - islands: &IslandManager, bodies: &RigidBodySet, colliders: &ColliderSet, mode: QueryPipelineMode, @@ -392,71 +407,12 @@ impl QueryPipeline { } } - if !self.tree_built { - let generator = DataGenerator { - bodies, - colliders, - mode, - }; - self.qbvh.clear_and_rebuild(generator, self.dilation_factor); - - // FIXME: uncomment this once we handle insertion/removals properly. - // self.tree_built = true; - return; - } - - for handle in islands.iter_active_bodies() { - let rb = &bodies[handle]; - for handle in &rb.colliders.0 { - self.qbvh.pre_update(*handle) - } - } - - match mode { - QueryPipelineMode::CurrentPosition => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - co.shape.compute_aabb(&co.pos) - }, - self.dilation_factor, - ); - } - QueryPipelineMode::SweepTestWithNextPosition => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - if let Some(parent) = &co.parent { - let rb_next_pos = &bodies[parent.handle].pos.next_position; - let next_position = rb_next_pos * parent.pos_wrt_parent; - co.shape.compute_swept_aabb(&co.pos, &next_position) - } else { - co.shape.compute_aabb(&co.pos) - } - }, - self.dilation_factor, - ); - } - QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - self.qbvh.update( - |handle| { - let co = &colliders[*handle]; - if let Some(parent) = co.parent { - let rb = &bodies[parent.handle]; - let predicted_pos = rb.pos.integrate_forces_and_velocities( - dt, &rb.forces, &rb.vels, &rb.mprops, - ); - - let next_position = predicted_pos * parent.pos_wrt_parent; - co.shape.compute_swept_aabb(&co.pos, &next_position) - } else { - co.shape.compute_aabb(&co.pos) - } - }, - self.dilation_factor, - ); - } - } + let generator = DataGenerator { + bodies, + colliders, + mode, + }; + self.qbvh.clear_and_rebuild(generator, self.dilation_factor); } /// Find the closest intersection between a ray and a set of collider. diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index adfd023..00096de 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -46,7 +46,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, - multibody_joints: &mut MultibodyJointSet, + _multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies modified_bodies: &[RigidBodyHandle], modified_colliders: &mut Vec, ) { diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 2ed9745..91d4da9 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -215,6 +215,7 @@ impl Harness { &mut physics.impulse_joints, &mut physics.multibody_joints, &mut physics.ccd_solver, + &mut physics.query_pipeline, &*physics.hooks, event_handler, ); @@ -233,16 +234,11 @@ impl Harness { &mut self.physics.impulse_joints, &mut self.physics.multibody_joints, &mut self.physics.ccd_solver, + Some(&mut self.physics.query_pipeline), &*self.physics.hooks, &self.event_handler, ); - self.physics.query_pipeline.update( - &self.physics.islands, - &self.physics.bodies, - &self.physics.colliders, - ); - for plugin in &mut self.plugins { plugin.step(&mut self.physics, &self.state) } From 7e95cba09dbaccdc60e9bc79a9d280577618843a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 27 Nov 2022 12:13:08 +0100 Subject: [PATCH 03/10] Fix ABA problem in incremental query pipeline update --- src/pipeline/query_pipeline.rs | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 8a667de..a248426 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -330,14 +330,21 @@ impl QueryPipeline { removed_colliders: &[ColliderHandle], refit_and_rebalance: bool, ) { - for modified in modified_colliders { - self.qbvh.pre_update_or_insert(*modified); - } - + // We remove first. This is needed to avoid the ABA problem: if a collider was removed + // and another added right after with the same handle index, we can remove first, and + // then update the new one (but only if its actually exists, to address the case where + // a collider was added/modified and then removed during the same frame). for removed in removed_colliders { self.qbvh.remove(*removed); } + for modified in modified_colliders { + // Check that the collider still exists as it may have been removed. + if colliders.contains(*modified) { + self.qbvh.pre_update_or_insert(*modified); + } + } + if refit_and_rebalance { let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| { colliders[*handle].compute_aabb() From 87feb3a48d8dc006ad6050b4227cf3d02028088d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 27 Nov 2022 15:15:10 +0100 Subject: [PATCH 04/10] Update changelog --- CHANGELOG.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5d31a08..44d00d7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,8 +1,21 @@ + +## Unreleased +### Added +- Add `RigidBody::set_enabled`, `RigidBody::is_enabled`, `RigidBodyBuilder::enabled` to enable/disable a rigid-body + without having to delete it. +- Add `Collider::set_enabled`, `Collider::is_enabled`, `ColliderBuilder::enabled` to enable/disable a collider + without having to delete it. +- Add `GenericJoint::set_enabled`, `GenericJoint::is_enabled` to enable/disable a joint without having to delete it. + +### Modified +- Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this + argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time as + these other pipelines. In that case, calling `QueryPipeline::update` a `PhysicsPipeline::step` isn’t needed. + ## v0.16.1 (10 Nov. 2022) ### Fix - Fixed docs build on `docs.rs`. - ## v0.16.0 (30 Oct. 2022) ### Added - Implement `Copy` for `CharacterCollision`. From 849f398031c61b4894d5b8ca229037c026c9186d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 4 Dec 2022 18:27:38 +0100 Subject: [PATCH 05/10] Add a dynamic ray-cast vehicle controller --- examples3d/all_examples3.rs | 2 + examples3d/vehicle_controller3.rs | 174 +++++ src/control/mod.rs | 6 + src/control/ray_cast_vehicle_controller.rs | 808 +++++++++++++++++++++ src/dynamics/rigid_body.rs | 6 + src_testbed/testbed.rs | 65 +- 6 files changed, 1060 insertions(+), 1 deletion(-) create mode 100644 examples3d/vehicle_controller3.rs create mode 100644 src/control/ray_cast_vehicle_controller.rs diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 749cec0..194d7cb 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -44,6 +44,7 @@ mod primitives3; mod restitution3; mod sensor3; mod trimesh3; +mod vehicle_controller3; fn demo_name_from_command_line() -> Option { let mut args = std::env::args(); @@ -101,6 +102,7 @@ pub fn main() { ("Restitution", restitution3::init_world), ("Sensor", sensor3::init_world), ("TriMesh", trimesh3::init_world), + ("Vehicle controller", vehicle_controller3::init_world), ("Keva tower", keva3::init_world), ("Newton cradle", newton_cradle3::init_world), ("(Debug) multibody_joints", debug_articulations3::init_world), diff --git a/examples3d/vehicle_controller3.rs b/examples3d/vehicle_controller3.rs new file mode 100644 index 0000000..622c008 --- /dev/null +++ b/examples3d/vehicle_controller3.rs @@ -0,0 +1,174 @@ +use rapier3d::control::{DynamicRayCastVehicleController, WheelTuning}; +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); + colliders.insert_with_parent(collider, floor_handle, &mut bodies); + + /* + * Vehicle we will control manually. + */ + let hw = 0.3; + let hh = 0.15; + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, 1.0, 0.0]); + let vehicle_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(hw, hh, hw); + colliders.insert_with_parent(collider, vehicle_handle, &mut bodies); + + let mut tuning = WheelTuning::default(); + tuning.suspension_stiffness = 100.0; + tuning.suspension_damping = 10.0; + let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle); + let wheel_positions = [ + point![hw, -hh, hw], + point![hw, -hh, -hw], + point![-hw, -hh, hw], + point![-hw, -hh, -hw], + ]; + + for pos in wheel_positions { + vehicle.add_wheel(pos, -Vector::y(), Vector::z(), hh, hh / 4.0, &tuning); + } + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = rad; + + for j in 0usize..4 { + for k in 0usize..4 { + for i in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift + centerx; + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + } + + /* + * Create some stairs. + */ + let stair_width = 1.0; + let stair_height = 0.1; + for i in 0..10 { + let x = i as f32 * stair_width / 2.0; + let y = i as f32 * stair_height * 1.5 + 3.0; + + let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0, stair_width) + .translation(vector![x, y, 0.0]); + colliders.insert(collider); + } + + /* + * Create a slope we can climb. + */ + let slope_angle = 0.2; + let slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height, slope_size) + .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0]) + .rotation(Vector::z() * slope_angle); + colliders.insert(collider); + + /* + * Create a slope we can’t climb. + */ + let impossible_slope_angle = 0.9; + let impossible_slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size) + .translation(vector![ + ground_size + slope_size * 2.0 + impossible_slope_size - 0.9, + -ground_height + 2.3, + 0.0 + ]) + .rotation(Vector::z() * impossible_slope_angle); + colliders.insert(collider); + + /* + * Create a moving platform. + */ + let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]); + // .rotation(-0.3); + let platform_handle = bodies.insert(body); + let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0); + colliders.insert_with_parent(collider, platform_handle, &mut bodies); + + /* + * More complex ground. + */ + let ground_size = Vector::new(10.0, 1.0, 10.0); + let nsubdivs = 20; + + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + (i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() + + (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos() + }); + + let collider = + ColliderBuilder::heightfield(heights, ground_size).translation(vector![-8.0, 5.0, 0.0]); + colliders.insert(collider); + + /* + * A tilting dynamic body with a limited joint. + */ + let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]); + let ground_handle = bodies.insert(ground); + let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]); + let handle = bodies.insert(body); + let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0); + colliders.insert_with_parent(collider, handle, &mut bodies); + let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]); + impulse_joints.insert(ground_handle, handle, joint, true); + + /* + * Setup a callback to move the platform. + */ + testbed.add_callback(move |_, physics, _, run_state| { + let linvel = vector![ + (run_state.time * 2.0).sin() * 2.0, + (run_state.time * 5.0).sin() * 1.5, + 0.0 + ]; + // let angvel = run_state.time.sin() * 0.5; + + // Update the velocity-based kinematic body by setting its velocity. + if let Some(platform) = physics.bodies.get_mut(platform_handle) { + platform.set_linvel(linvel, true); + // NOTE: interaction with rotating platforms isn’t handled very well yet. + // platform.set_angvel(angvel, true); + } + }); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.set_vehicle_controller(vehicle); + testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin()); +} diff --git a/src/control/mod.rs b/src/control/mod.rs index 0290f2c..3f7dea1 100644 --- a/src/control/mod.rs +++ b/src/control/mod.rs @@ -5,4 +5,10 @@ pub use self::character_controller::{ KinematicCharacterController, }; +#[cfg(feature = "dim3")] +pub use self::ray_cast_vehicle_controller::{DynamicRayCastVehicleController, Wheel, WheelTuning}; + mod character_controller; + +#[cfg(feature = "dim3")] +mod ray_cast_vehicle_controller; diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs new file mode 100644 index 0000000..42bb112 --- /dev/null +++ b/src/control/ray_cast_vehicle_controller.rs @@ -0,0 +1,808 @@ +//! A vehicle controller based on ray-casting, ported and modified from Bullet’s `btRaycastVehicle`. + +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderHandle, ColliderSet, Ray}; +use crate::math::{Point, Real, Rotation, Vector}; +use crate::pipeline::{QueryFilter, QueryPipeline}; +use crate::utils::{WCross, WDot}; + +/// A character controller to simulate vehicles using ray-casting for the wheels. +pub struct DynamicRayCastVehicleController { + wheels: Vec, + forward_ws: Vec>, + axle: Vec>, + /// The current forward speed of the vehicle. + pub current_vehicle_speed: Real, + + /// Handle of the vehicle’s chassis. + pub chassis: RigidBodyHandle, + /// The chassis’ local _up_ direction (`0 = x, 1 = y, 2 = z`) + pub index_up_axis: usize, + /// The chassis’ local _forward_ direction (`0 = x, 1 = y, 2 = z`) + pub index_forward_axis: usize, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +/// Parameters affecting the physical behavior of a wheel. +pub struct WheelTuning { + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub suspension_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub suspension_damping: Real, + /// The maximum distance the suspension can travel before and after its resting length. + pub max_suspension_travel: Real, + /// Parameter controlling how much traction the tire his. + /// + /// The larger the value, the more instantaneous braking will happen (with the risk of + /// causing the vehicle to flip if it’s too strong). + pub friction_slip: Real, + /// The maximum force applied by the suspension. + pub max_suspension_force: Real, +} + +impl Default for WheelTuning { + fn default() -> Self { + Self { + suspension_stiffness: 5.88, + suspension_compression: 0.83, + suspension_damping: 0.88, + max_suspension_travel: 5.0, + friction_slip: 10.5, + max_suspension_force: 6000.0, + } + } +} + +/// Objects used to initialize a wheel. +struct WheelDesc { + /// The position of the wheel, relative to the chassis. + pub chassis_connection_cs: Point, + /// The direction of the wheel’s suspension, relative to the chassis. + /// + /// The ray-casting will happen following this direction to detect the ground. + pub direction_cs: Vector, + /// The wheel’s axle axis, relative to the chassis. + pub axle_cs: Vector, + /// The rest length of the wheel’s suspension spring. + pub suspension_rest_length: Real, + /// The maximum distance the suspension can travel before and after its resting length. + pub max_suspension_travel: Real, + /// The wheel’s radius. + pub radius: Real, + + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub damping_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub damping_relaxation: Real, + /// Parameter controlling how much traction the tire his. + /// + /// The larger the value, the more instantaneous braking will happen (with the risk of + /// causing the vehicle to flip if it’s too strong). + pub friction_slip: Real, + /// The maximum force applied by the suspension. + pub max_suspension_force: Real, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +/// A wheel attached to a vehicle. +pub struct Wheel { + raycast_info: RayCastInfo, + + center: Point, + wheel_direction_ws: Vector, + wheel_axle_ws: Vector, + + /// The position of the wheel, relative to the chassis. + pub chassis_connection_point_cs: Point, + /// The direction of the wheel’s suspension, relative to the chassis. + /// + /// The ray-casting will happen following this direction to detect the ground. + pub direction_cs: Vector, + /// The wheel’s axle axis, relative to the chassis. + pub axle_cs: Vector, + /// The rest length of the wheel’s suspension spring. + pub suspension_rest_length: Real, + /// The maximum distance the suspension can travel before and after its resting length. + pub max_suspension_travel: Real, + /// The wheel’s radius. + pub radius: Real, + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub damping_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub damping_relaxation: Real, + /// Parameter controlling how much traction the tire his. + /// + /// The larger the value, the more instantaneous braking will happen (with the risk of + /// causing the vehicle to flip if it’s too strong). + friction_slip: Real, + /// The wheel’s current rotation on its axle. + pub rotation: Real, + delta_rotation: Real, + roll_influence: Real, // TODO: make this public? + /// The maximum force applied by the suspension. + pub max_suspension_force: Real, + + /// The forward impulses applied by the wheel on the chassis. + pub forward_impulse: Real, + /// The side impulses applied by the wheel on the chassis. + pub side_impulse: Real, + + /// The steering angle for this wheel. + pub steering: Real, + /// The forward force applied by this wheel on the chassis. + pub engine_force: Real, + /// The maximum amount of braking impulse applied to slow down the vehicle. + pub brake: Real, + + clipped_inv_contact_dot_suspension: Real, + suspension_relative_velocity: Real, + /// The force applied by the suspension. + pub wheel_suspension_force: Real, + skid_info: Real, +} + +impl Wheel { + fn new(info: WheelDesc) -> Self { + Self { + raycast_info: RayCastInfo::default(), + suspension_rest_length: info.suspension_rest_length, + max_suspension_travel: info.max_suspension_travel, + radius: info.radius, + suspension_stiffness: info.suspension_stiffness, + damping_compression: info.damping_compression, + damping_relaxation: info.damping_relaxation, + chassis_connection_point_cs: info.chassis_connection_cs, + direction_cs: info.direction_cs, + axle_cs: info.axle_cs, + wheel_direction_ws: info.direction_cs, + wheel_axle_ws: info.axle_cs, + center: Point::origin(), + friction_slip: info.friction_slip, + steering: 0.0, + engine_force: 0.0, + rotation: 0.0, + delta_rotation: 0.0, + brake: 0.0, + roll_influence: 0.1, + clipped_inv_contact_dot_suspension: 0.0, + suspension_relative_velocity: 0.0, + wheel_suspension_force: 0.0, + max_suspension_force: info.max_suspension_force, + skid_info: 0.0, + side_impulse: 0.0, + forward_impulse: 0.0, + } + } + + /// The world-space center of the wheel. + pub fn center(&self) -> Point { + self.center + } + + /// The world-space direction of the wheel’s suspension. + pub fn suspension(&self) -> Vector { + self.wheel_direction_ws + } + + /// The world-space direction of the wheel’s axle. + pub fn axle(&self) -> Vector { + self.wheel_axle_ws + } +} + +#[derive(Copy, Clone, Debug, PartialEq, Default)] +struct RayCastInfo { + // set by raycaster + contact_normal_ws: Vector, //contact normal + contact_point_ws: Point, //raycast hitpoint + suspension_length: Real, + hard_point_ws: Point, //raycast starting point + is_in_contact: bool, + ground_object: Option, +} + +impl DynamicRayCastVehicleController { + /// Creates a new vehicle represented by the given rigid-body. + /// + /// Wheels have to be attached afterwards calling [`Self::add_wheel`]. + pub fn new(chassis: RigidBodyHandle) -> Self { + Self { + wheels: vec![], + forward_ws: vec![], + axle: vec![], + current_vehicle_speed: 0.0, + chassis, + index_up_axis: 1, + index_forward_axis: 0, + } + } + + // + // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed + // + /// Adds a wheel to this vehicle. + pub fn add_wheel( + &mut self, + chassis_connection_cs: Point, + direction_cs: Vector, + axle_cs: Vector, + suspension_rest_length: Real, + radius: Real, + tuning: &WheelTuning, + ) -> &mut Wheel { + let ci = WheelDesc { + chassis_connection_cs, + direction_cs, + axle_cs, + suspension_rest_length, + radius, + suspension_stiffness: tuning.suspension_stiffness, + damping_compression: tuning.suspension_compression, + damping_relaxation: tuning.suspension_damping, + friction_slip: tuning.friction_slip, + max_suspension_travel: tuning.max_suspension_travel, + max_suspension_force: tuning.max_suspension_force, + }; + + let wheel_id = self.wheels.len(); + self.wheels.push(Wheel::new(ci)); + + &mut self.wheels[wheel_id] + } + + #[cfg(feature = "dim2")] + fn update_wheel_transform(&mut self, chassis: &RigidBody, wheel_index: usize) { + self.update_wheel_transforms_ws(chassis, wheel_index); + let wheel = &mut self.wheels[wheel_index]; + wheel.center = (wheel.raycast_info.hard_point_ws + + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length) + .coords; + } + + #[cfg(feature = "dim3")] + fn update_wheel_transform(&mut self, chassis: &RigidBody, wheel_index: usize) { + self.update_wheel_transforms_ws(chassis, wheel_index); + let wheel = &mut self.wheels[wheel_index]; + + let steering_orn = Rotation::new(-wheel.wheel_direction_ws * wheel.steering); + wheel.wheel_axle_ws = steering_orn * (chassis.position() * wheel.axle_cs); + wheel.center = wheel.raycast_info.hard_point_ws + + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length; + } + + fn update_wheel_transforms_ws(&mut self, chassis: &RigidBody, wheel_id: usize) { + let wheel = &mut self.wheels[wheel_id]; + wheel.raycast_info.is_in_contact = false; + + let chassis_transform = chassis.position(); + + wheel.raycast_info.hard_point_ws = chassis_transform * wheel.chassis_connection_point_cs; + wheel.wheel_direction_ws = chassis_transform * wheel.direction_cs; + wheel.wheel_axle_ws = chassis_transform * wheel.axle_cs; + } + + fn ray_cast( + &mut self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + filter: QueryFilter, + chassis: &RigidBody, + wheel_id: usize, + ) { + let wheel = &mut self.wheels[wheel_id]; + let raylen = wheel.suspension_rest_length + wheel.radius; + let rayvector = wheel.wheel_direction_ws * raylen; + let source = wheel.raycast_info.hard_point_ws; + wheel.raycast_info.contact_point_ws = source + rayvector; + let ray = Ray::new(source, rayvector); + let hit = queries.cast_ray_and_get_normal(bodies, colliders, &ray, 1.0, true, filter); + + wheel.raycast_info.ground_object = None; + + if let Some((collider_hit, mut hit)) = hit { + if hit.toi == 0.0 { + let collider = &colliders[collider_hit]; + let up_ray = Ray::new(source + rayvector, -rayvector); + if let Some(hit2) = + collider + .shape + .cast_ray_and_get_normal(collider.position(), &up_ray, 1.0, false) + { + hit.normal = -hit2.normal; + } + + if hit.normal == Vector::zeros() { + // If the hit is still not defined, set the normal. + hit.normal = -wheel.wheel_direction_ws; + } + } + + wheel.raycast_info.contact_normal_ws = hit.normal; + wheel.raycast_info.is_in_contact = true; + wheel.raycast_info.ground_object = Some(collider_hit); + + let hit_distance = hit.toi * raylen; + wheel.raycast_info.suspension_length = hit_distance - wheel.radius; + + // clamp on max suspension travel + let min_suspension_length = wheel.suspension_rest_length - wheel.max_suspension_travel; + let max_suspension_length = wheel.suspension_rest_length + wheel.max_suspension_travel; + wheel.raycast_info.suspension_length = wheel + .raycast_info + .suspension_length + .clamp(min_suspension_length, max_suspension_length); + wheel.raycast_info.contact_point_ws = ray.point_at(hit.toi); + + let denominator = wheel + .raycast_info + .contact_normal_ws + .dot(&wheel.wheel_direction_ws); + let chassis_velocity_at_contact_point = + chassis.velocity_at_point(&wheel.raycast_info.contact_point_ws); + let proj_vel = wheel + .raycast_info + .contact_normal_ws + .dot(&chassis_velocity_at_contact_point); + + if denominator >= -0.1 { + wheel.suspension_relative_velocity = 0.0; + wheel.clipped_inv_contact_dot_suspension = 1.0 / 0.1; + } else { + let inv = -1.0 / denominator; + wheel.suspension_relative_velocity = proj_vel * inv; + wheel.clipped_inv_contact_dot_suspension = inv; + } + } else { + // No contact, put wheel info as in rest position + wheel.raycast_info.suspension_length = wheel.suspension_rest_length; + wheel.suspension_relative_velocity = 0.0; + wheel.raycast_info.contact_normal_ws = -wheel.wheel_direction_ws; + wheel.clipped_inv_contact_dot_suspension = 1.0; + } + } + + /// Updates the vehicle’s velocity based on its suspension, engine force, and brake. + pub fn update_vehicle( + &mut self, + dt: Real, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + filter: QueryFilter, + ) { + let num_wheels = self.wheels.len(); + let chassis = &bodies[self.chassis]; + + for i in 0..num_wheels { + self.update_wheel_transform(chassis, i); + } + + self.current_vehicle_speed = chassis.linvel().norm(); + + let forward_w = chassis.position() * Vector::ith(self.index_forward_axis, 1.0); + + if forward_w.dot(chassis.linvel()) < 0.0 { + self.current_vehicle_speed *= -1.0; + } + + // + // simulate suspension + // + + for wheel_id in 0..self.wheels.len() { + self.ray_cast(bodies, colliders, queries, filter, chassis, wheel_id); + } + + let chassis_mass = chassis.mass(); + self.update_suspension(chassis_mass); + + let chassis = bodies + .get_mut_internal_with_modification_tracking(self.chassis) + .unwrap(); + + for wheel in &mut self.wheels { + if wheel.engine_force > 0.0 { + chassis.wake_up(true); + } + + // apply suspension force + let mut suspension_force = wheel.wheel_suspension_force; + + if suspension_force > wheel.max_suspension_force { + suspension_force = wheel.max_suspension_force; + } + + let impulse = wheel.raycast_info.contact_normal_ws * suspension_force * dt; + chassis.apply_impulse_at_point(impulse, wheel.raycast_info.contact_point_ws, false); + } + + self.update_friction(bodies, colliders, dt); + + let chassis = bodies + .get_mut_internal_with_modification_tracking(self.chassis) + .unwrap(); + + for wheel in &mut self.wheels { + let vel = chassis.velocity_at_point(&wheel.raycast_info.hard_point_ws); + + if wheel.raycast_info.is_in_contact { + let mut fwd = chassis.position() * Vector::ith(self.index_forward_axis, 1.0); + let proj = fwd.dot(&wheel.raycast_info.contact_normal_ws); + fwd -= wheel.raycast_info.contact_normal_ws * proj; + + let proj2 = fwd.dot(&vel); + + wheel.delta_rotation = (proj2 * dt) / (wheel.radius); + wheel.rotation += wheel.delta_rotation; + } else { + wheel.rotation += wheel.delta_rotation; + } + + wheel.delta_rotation *= 0.99; //damping of rotation when not in contact + } + } + + /// Reference to all the wheels attached to this vehicle. + pub fn wheels(&self) -> &[Wheel] { + &self.wheels + } + + /// Mutable reference to all the wheels attached to this vehicle. + pub fn wheels_mut(&mut self) -> &mut [Wheel] { + &mut self.wheels + } + + fn update_suspension(&mut self, chassis_mass: Real) { + for w_it in 0..self.wheels.len() { + let wheels = &mut self.wheels[w_it]; + + if wheels.raycast_info.is_in_contact { + let mut force; + // Spring + { + let rest_length = wheels.suspension_rest_length; + let current_length = wheels.raycast_info.suspension_length; + let length_diff = rest_length - current_length; + + force = wheels.suspension_stiffness + * length_diff + * wheels.clipped_inv_contact_dot_suspension; + } + + // Damper + { + let projected_rel_vel = wheels.suspension_relative_velocity; + { + let susp_damping = if projected_rel_vel < 0.0 { + wheels.damping_compression + } else { + wheels.damping_relaxation + }; + force -= susp_damping * projected_rel_vel; + } + } + + // RESULT + wheels.wheel_suspension_force = (force * chassis_mass).max(0.0); + } else { + wheels.wheel_suspension_force = 0.0; + } + } + } + + const SIDE_FRICTION_STIFFNESS2: Real = 1.0; + + fn update_friction(&mut self, bodies: &mut RigidBodySet, colliders: &ColliderSet, dt: Real) { + let num_wheels = self.wheels.len(); + + if num_wheels == 0 { + return; + } + + self.forward_ws.resize(num_wheels, Default::default()); + self.axle.resize(num_wheels, Default::default()); + + let mut num_wheels_on_ground = 0; + + //TODO: collapse all those loops into one! + for wheel in &mut self.wheels { + let ground_object = wheel.raycast_info.ground_object; + + if ground_object.is_some() { + num_wheels_on_ground += 1; + } + + wheel.side_impulse = 0.0; + wheel.forward_impulse = 0.0; + } + + { + for i in 0..num_wheels { + let wheel = &mut self.wheels[i]; + let ground_object = wheel.raycast_info.ground_object; + + if ground_object.is_some() { + self.axle[i] = wheel.wheel_axle_ws; + + let surf_normal_ws = wheel.raycast_info.contact_normal_ws; + let proj = self.axle[i].dot(&surf_normal_ws); + self.axle[i] -= surf_normal_ws * proj; + self.axle[i] = self.axle[i] + .try_normalize(1.0e-5) + .unwrap_or_else(Vector::zeros); + self.forward_ws[i] = surf_normal_ws + .cross(&self.axle[i]) + .try_normalize(1.0e-5) + .unwrap_or_else(Vector::zeros); + + if let Some(ground_body) = ground_object + .and_then(|h| colliders[h].parent()) + .map(|h| &bodies[h]) + .filter(|b| b.is_dynamic()) + { + wheel.side_impulse = resolve_single_bilateral( + &bodies[self.chassis], + &wheel.raycast_info.contact_point_ws, + &ground_body, + &wheel.raycast_info.contact_point_ws, + &self.axle[i], + ); + } else { + wheel.side_impulse = resolve_single_unilateral( + &bodies[self.chassis], + &wheel.raycast_info.contact_point_ws, + &self.axle[i], + ); + } + + wheel.side_impulse *= Self::SIDE_FRICTION_STIFFNESS2; + } + } + } + + let side_factor = 1.0; + let fwd_factor = 0.5; + + let mut sliding = false; + { + for wheel_id in 0..num_wheels { + let wheel = &mut self.wheels[wheel_id]; + let ground_object = wheel.raycast_info.ground_object; + + let mut rolling_friction = 0.0; + + if ground_object.is_some() { + if wheel.engine_force != 0.0 { + rolling_friction = wheel.engine_force * dt; + } else { + let default_rolling_friction_impulse = 0.0; + let max_impulse = if wheel.brake != 0.0 { + wheel.brake + } else { + default_rolling_friction_impulse + }; + let contact_pt = WheelContactPoint::new( + &bodies[self.chassis], + ground_object + .and_then(|h| colliders[h].parent()) + .map(|h| &bodies[h]), + wheel.raycast_info.contact_point_ws, + self.forward_ws[wheel_id], + max_impulse, + ); + assert!(num_wheels_on_ground > 0); + rolling_friction = contact_pt.calc_rolling_friction(num_wheels_on_ground); + } + } + + //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) + + wheel.forward_impulse = 0.0; + wheel.skid_info = 1.0; + + if ground_object.is_some() { + let max_imp = wheel.wheel_suspension_force * dt * wheel.friction_slip; + let max_imp_side = max_imp; + let max_imp_squared = max_imp * max_imp_side; + assert!(max_imp_squared >= 0.0); + + wheel.forward_impulse = rolling_friction; + + let x = wheel.forward_impulse * fwd_factor; + let y = wheel.side_impulse * side_factor; + + let impulse_squared = x * x + y * y; + + if impulse_squared > max_imp_squared { + sliding = true; + + let factor = max_imp * crate::utils::inv(impulse_squared.sqrt()); + wheel.skid_info *= factor; + } + } + } + } + + if sliding { + for wheel in &mut self.wheels { + if wheel.side_impulse != 0.0 { + if wheel.skid_info < 1.0 { + wheel.forward_impulse *= wheel.skid_info; + wheel.side_impulse *= wheel.skid_info; + } + } + } + } + + // apply the impulses + { + let chassis = bodies + .get_mut_internal_with_modification_tracking(self.chassis) + .unwrap(); + + for wheel_id in 0..num_wheels { + let wheel = &self.wheels[wheel_id]; + let mut impulse_point = wheel.raycast_info.contact_point_ws; + + if wheel.forward_impulse != 0.0 { + chassis.apply_impulse_at_point( + self.forward_ws[wheel_id] * wheel.forward_impulse, + impulse_point, + false, + ); + } + if wheel.side_impulse != 0.0 { + let side_impulse = self.axle[wheel_id] * wheel.side_impulse; + + let v_chassis_world_up = + chassis.position().rotation * Vector::ith(self.index_up_axis, 1.0); + impulse_point -= v_chassis_world_up + * (v_chassis_world_up.dot(&(impulse_point - chassis.center_of_mass())) + * (1.0 - wheel.roll_influence)); + + chassis.apply_impulse_at_point(side_impulse, impulse_point, false); + + // TODO: apply friction impulse on the ground + // let ground_object = self.wheels[wheel_id].raycast_info.ground_object; + // ground_object.apply_impulse_at_point( + // -side_impulse, + // wheels.raycast_info.contact_point_ws, + // false, + // ); + } + } + } + } +} + +struct WheelContactPoint<'a> { + body0: &'a RigidBody, + body1: Option<&'a RigidBody>, + friction_position_world: Point, + friction_direction_world: Vector, + jac_diag_ab_inv: Real, + max_impulse: Real, +} + +impl<'a> WheelContactPoint<'a> { + pub fn new( + body0: &'a RigidBody, + body1: Option<&'a RigidBody>, + friction_position_world: Point, + friction_direction_world: Vector, + max_impulse: Real, + ) -> Self { + fn impulse_denominator(body: &RigidBody, pos: &Point, n: &Vector) -> Real { + let dpt = pos - body.center_of_mass(); + let gcross = dpt.gcross(*n); + let v = (body.mprops.effective_world_inv_inertia_sqrt + * (body.mprops.effective_world_inv_inertia_sqrt * gcross)) + .gcross(dpt); + // TODO: take the effective inv mass into account instead of the inv_mass? + body.mprops.local_mprops.inv_mass + n.dot(&v) + } + let denom0 = + impulse_denominator(body0, &friction_position_world, &friction_direction_world); + let denom1 = body1 + .map(|body1| { + impulse_denominator(body1, &friction_position_world, &friction_direction_world) + }) + .unwrap_or(0.0); + let relaxation = 1.0; + let jac_diag_ab_inv = relaxation / (denom0 + denom1); + + Self { + body0, + body1, + friction_position_world, + friction_direction_world, + jac_diag_ab_inv, + max_impulse, + } + } + + pub fn calc_rolling_friction(&self, num_wheels_on_ground: usize) -> Real { + let contact_pos_world = self.friction_position_world; + let max_impulse = self.max_impulse; + + let vel1 = self.body0.velocity_at_point(&contact_pos_world); + let vel2 = self + .body1 + .map(|b| b.velocity_at_point(&contact_pos_world)) + .unwrap_or_else(Vector::zeros); + let vel = vel1 - vel2; + let vrel = self.friction_direction_world.dot(&vel); + + // calculate friction that moves us to zero relative velocity + (-vrel * self.jac_diag_ab_inv / (num_wheels_on_ground as Real)) + .clamp(-max_impulse, max_impulse) + } +} + +fn resolve_single_bilateral( + body1: &RigidBody, + pt1: &Point, + body2: &RigidBody, + pt2: &Point, + normal: &Vector, +) -> Real { + let vel1 = body1.velocity_at_point(pt1); + let vel2 = body2.velocity_at_point(pt2); + let dvel = vel1 - vel2; + + let dpt1 = pt1 - body1.center_of_mass(); + let dpt2 = pt2 - body2.center_of_mass(); + let aj = dpt1.gcross(*normal); + let bj = dpt2.gcross(-*normal); + let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; + let ibj = body2.mprops.effective_world_inv_inertia_sqrt * bj; + + // TODO: take the effective_inv_mass into account? + let im1 = body1.mprops.local_mprops.inv_mass; + let im2 = body2.mprops.local_mprops.inv_mass; + + let jac_diag_ab = im1 + im2 + iaj.gdot(iaj) + ibj.gdot(ibj); + let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab); + let rel_vel = normal.dot(&dvel); + + //todo: move this into proper structure + let contact_damping = 0.2; + -contact_damping * rel_vel * jac_diag_ab_inv +} + +fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point, normal: &Vector) -> Real { + let vel1 = body1.velocity_at_point(pt1); + let dvel = vel1; + let dpt1 = pt1 - body1.center_of_mass(); + let aj = dpt1.gcross(*normal); + let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; + + // TODO: take the effective_inv_mass into account? + let im1 = body1.mprops.local_mprops.inv_mass; + let jac_diag_ab = im1 + iaj.gdot(iaj); + let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab); + let rel_vel = normal.dot(&dvel); + + //todo: move this into proper structure + let contact_damping = 0.2; + -contact_damping * rel_vel * jac_diag_ab_inv +} diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index a6384b7..26be983 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -146,6 +146,12 @@ impl RigidBody { } } + /// The world-space center-of-mass of this rigid-body. + #[inline] + pub fn center_of_mass(&self) -> &Point { + &self.mprops.world_com + } + /// The mass-properties of this rigid-body. #[inline] pub fn mass_properties(&self) -> &MassProperties { diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 180bfd4..50b77c8 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -9,6 +9,8 @@ use crate::{debug_render, ui}; use crate::{graphics::GraphicsManager, harness::RunState}; use na::{self, Point2, Point3, Vector3}; +#[cfg(feature = "dim3")] +use rapier::control::DynamicRayCastVehicleController; use rapier::control::KinematicCharacterController; use rapier::dynamics::{ ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation, @@ -100,6 +102,8 @@ pub struct TestbedState { pub draw_colls: bool, pub highlighted_body: Option, pub character_body: Option, + #[cfg(feature = "dim3")] + pub vehicle_controller: Option, // pub grabbed_object: Option, // pub grabbed_object_constraint: Option, pub grabbed_object_plane: (Point3, Vector3), @@ -178,6 +182,8 @@ impl TestbedApp { draw_colls: false, highlighted_body: None, character_body: None, + #[cfg(feature = "dim3")] + vehicle_controller: None, // grabbed_object: None, // grabbed_object_constraint: None, grabbed_object_plane: (Point3::origin(), na::zero()), @@ -456,6 +462,11 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { self.state.character_body = Some(handle); } + #[cfg(feature = "dim3")] + pub fn set_vehicle_controller(&mut self, controller: DynamicRayCastVehicleController) { + self.state.vehicle_controller = Some(controller); + } + pub fn allow_grabbing_behind_ground(&mut self, allow: bool) { self.state.can_grab_behind_ground = allow; } @@ -511,8 +522,12 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { .action_flags .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true); - self.state.character_body = None; self.state.highlighted_body = None; + self.state.character_body = None; + #[cfg(feature = "dim3")] + { + self.state.vehicle_controller = None; + } #[cfg(all(feature = "dim2", feature = "other-backends"))] { @@ -624,6 +639,50 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { self.plugins.0.push(Box::new(plugin)); } + #[cfg(feature = "dim3")] + fn update_vehicle_controller(&mut self, events: &Input) { + if self.state.running == RunMode::Stop { + return; + } + + if let Some(vehicle) = &mut self.state.vehicle_controller { + let mut engine_force = 0.0; + let mut steering_angle = 0.0; + + for key in events.get_pressed() { + match *key { + KeyCode::Right => { + steering_angle += -0.1; + } + KeyCode::Left => { + steering_angle += 0.1; + } + KeyCode::Up => { + engine_force += 0.1; + } + KeyCode::Down => { + engine_force += -0.1; + } + _ => {} + } + } + + let wheels = vehicle.wheels_mut(); + wheels[0].engine_force = engine_force; + wheels[0].steering = steering_angle; + wheels[1].engine_force = engine_force; + wheels[1].steering = steering_angle; + + vehicle.update_vehicle( + self.harness.physics.integration_parameters.dt, + &mut self.harness.physics.bodies, + &self.harness.physics.colliders, + &self.harness.physics.query_pipeline, + QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle.chassis), + ); + } + } + fn update_character_controller(&mut self, events: &Input) { if self.state.running == RunMode::Stop { return; @@ -1063,6 +1122,10 @@ fn update_testbed( testbed.handle_common_events(&*keys); testbed.update_character_controller(&*keys); + #[cfg(feature = "dim3")] + { + testbed.update_vehicle_controller(&*keys); + } } // Update UI From 8ef8680817af5b098c65ff5a8b6a1db8e629988a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 11 Dec 2022 15:18:11 +0100 Subject: [PATCH 06/10] Update parry and changelog --- CHANGELOG.md | 5 +- crates/rapier2d-f64/Cargo.toml | 2 +- crates/rapier2d/Cargo.toml | 2 +- crates/rapier3d-f64/Cargo.toml | 2 +- crates/rapier3d/Cargo.toml | 2 +- examples3d/vehicle_controller3.rs | 110 +++++++++++++----------------- src_testbed/testbed.rs | 16 ++--- 7 files changed, 64 insertions(+), 75 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 44d00d7..15dd631 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,10 +2,13 @@ ## Unreleased ### Added - Add `RigidBody::set_enabled`, `RigidBody::is_enabled`, `RigidBodyBuilder::enabled` to enable/disable a rigid-body - without having to delete it. + without having to delete it. Disabling a rigid-body attached to a multibody joint isn’t supported yet. - Add `Collider::set_enabled`, `Collider::is_enabled`, `ColliderBuilder::enabled` to enable/disable a collider without having to delete it. - Add `GenericJoint::set_enabled`, `GenericJoint::is_enabled` to enable/disable a joint without having to delete it. + Disabling a multibody joint isn’t supported yet. +- Add `DynamicRayCastVehicleController`, a vehicle controller based on ray-casting and dynamic rigid-bodies (mostly + a port of the vehicle controller from Bullet physics). ### Modified - Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index b949095..60ea169 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry2d-f64 = "^0.11.1" +parry2d-f64 = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 2cc9004..115069c 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry2d = "^0.11.1" +parry2d = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 929e688..79c2783 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry3d-f64 = "^0.11.1" +parry3d-f64 = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index 942e618..88c5281 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry3d = "^0.11.1" +parry3d = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/examples3d/vehicle_controller3.rs b/examples3d/vehicle_controller3.rs index 622c008..4f36d9f 100644 --- a/examples3d/vehicle_controller3.rs +++ b/examples3d/vehicle_controller3.rs @@ -29,7 +29,7 @@ pub fn init_world(testbed: &mut Testbed) { let hh = 0.15; let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, 1.0, 0.0]); let vehicle_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(hw, hh, hw); + let collider = ColliderBuilder::cuboid(hw * 2.0, hh, hw).density(100.0); colliders.insert_with_parent(collider, vehicle_handle, &mut bodies); let mut tuning = WheelTuning::default(); @@ -37,10 +37,10 @@ pub fn init_world(testbed: &mut Testbed) { tuning.suspension_damping = 10.0; let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle); let wheel_positions = [ - point![hw, -hh, hw], - point![hw, -hh, -hw], - point![-hw, -hh, hw], - point![-hw, -hh, -hw], + point![hw * 1.5, -hh, hw], + point![hw * 1.5, -hh, -hw], + point![-hw * 1.5, -hh, hw], + point![-hw * 1.5, -hh, -hw], ]; for pos in wheel_positions { @@ -57,7 +57,7 @@ pub fn init_world(testbed: &mut Testbed) { let centerx = shift * (num / 2) as f32; let centery = rad; - for j in 0usize..4 { + for j in 0usize..1 { for k in 0usize..4 { for i in 0..num { let x = i as f32 * shift - centerx; @@ -72,26 +72,12 @@ pub fn init_world(testbed: &mut Testbed) { } } - /* - * Create some stairs. - */ - let stair_width = 1.0; - let stair_height = 0.1; - for i in 0..10 { - let x = i as f32 * stair_width / 2.0; - let y = i as f32 * stair_height * 1.5 + 3.0; - - let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0, stair_width) - .translation(vector![x, y, 0.0]); - colliders.insert(collider); - } - /* * Create a slope we can climb. */ let slope_angle = 0.2; let slope_size = 2.0; - let collider = ColliderBuilder::cuboid(slope_size, ground_height, slope_size) + let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size) .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0]) .rotation(Vector::z() * slope_angle); colliders.insert(collider); @@ -110,60 +96,60 @@ pub fn init_world(testbed: &mut Testbed) { .rotation(Vector::z() * impossible_slope_angle); colliders.insert(collider); - /* - * Create a moving platform. - */ - let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]); - // .rotation(-0.3); - let platform_handle = bodies.insert(body); - let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0); - colliders.insert_with_parent(collider, platform_handle, &mut bodies); + // /* + // * Create a moving platform. + // */ + // let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]); + // // .rotation(-0.3); + // let platform_handle = bodies.insert(body); + // let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0); + // colliders.insert_with_parent(collider, platform_handle, &mut bodies); /* * More complex ground. */ - let ground_size = Vector::new(10.0, 1.0, 10.0); + let ground_size = Vector::new(10.0, 0.4, 10.0); let nsubdivs = 20; let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { - (i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() - + (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos() + -(i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() + - (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos() }); let collider = - ColliderBuilder::heightfield(heights, ground_size).translation(vector![-8.0, 5.0, 0.0]); + ColliderBuilder::heightfield(heights, ground_size).translation(vector![-7.0, 0.0, 0.0]); colliders.insert(collider); - /* - * A tilting dynamic body with a limited joint. - */ - let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]); - let ground_handle = bodies.insert(ground); - let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]); - let handle = bodies.insert(body); - let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0); - colliders.insert_with_parent(collider, handle, &mut bodies); - let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]); - impulse_joints.insert(ground_handle, handle, joint, true); + // /* + // * A tilting dynamic body with a limited joint. + // */ + // let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]); + // let ground_handle = bodies.insert(ground); + // let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]); + // let handle = bodies.insert(body); + // let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0); + // colliders.insert_with_parent(collider, handle, &mut bodies); + // let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]); + // impulse_joints.insert(ground_handle, handle, joint, true); - /* - * Setup a callback to move the platform. - */ - testbed.add_callback(move |_, physics, _, run_state| { - let linvel = vector![ - (run_state.time * 2.0).sin() * 2.0, - (run_state.time * 5.0).sin() * 1.5, - 0.0 - ]; - // let angvel = run_state.time.sin() * 0.5; - - // Update the velocity-based kinematic body by setting its velocity. - if let Some(platform) = physics.bodies.get_mut(platform_handle) { - platform.set_linvel(linvel, true); - // NOTE: interaction with rotating platforms isn’t handled very well yet. - // platform.set_angvel(angvel, true); - } - }); + // /* + // * Setup a callback to move the platform. + // */ + // testbed.add_callback(move |_, physics, _, run_state| { + // let linvel = vector![ + // (run_state.time * 2.0).sin() * 2.0, + // (run_state.time * 5.0).sin() * 1.5, + // 0.0 + // ]; + // // let angvel = run_state.time.sin() * 0.5; + // + // // Update the velocity-based kinematic body by setting its velocity. + // if let Some(platform) = physics.bodies.get_mut(platform_handle) { + // platform.set_linvel(linvel, true); + // // NOTE: interaction with rotating platforms isn’t handled very well yet. + // // platform.set_angvel(angvel, true); + // } + // }); /* * Set up the testbed. diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 50b77c8..1052c55 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -652,16 +652,16 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { for key in events.get_pressed() { match *key { KeyCode::Right => { - steering_angle += -0.1; + steering_angle += -0.7; } KeyCode::Left => { - steering_angle += 0.1; + steering_angle += 0.7; } KeyCode::Up => { - engine_force += 0.1; + engine_force += 30.0; } KeyCode::Down => { - engine_force += -0.1; + engine_force += -30.0; } _ => {} } @@ -1308,10 +1308,10 @@ fn update_testbed( || state.prev_flags.contains(TestbedStateFlags::WIREFRAME) != state.flags.contains(TestbedStateFlags::WIREFRAME) { - // graphics.toggle_wireframe_mode( - // &harness.physics.colliders, - // state.flags.contains(TestbedStateFlags::WIREFRAME), - // ) + graphics.toggle_wireframe_mode( + &harness.physics.colliders, + state.flags.contains(TestbedStateFlags::WIREFRAME), + ) } if state.prev_flags.contains(TestbedStateFlags::SLEEP) From 6f866329a67e2b2b6e7b08dadc50c13cd598ecbc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 11 Dec 2022 15:38:18 +0100 Subject: [PATCH 07/10] Fix wasm build --- src/dynamics/solver/velocity_constraint.rs | 1 + src/dynamics/solver/velocity_ground_constraint.rs | 1 + 2 files changed, 2 insertions(+) diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 7d2294e..a6b7384 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -214,6 +214,7 @@ impl VelocityConstraint { let constraint = if insert_at.is_none() { let new_len = out_constraints.len() + 1; unsafe { + #[allow(invalid_value)] out_constraints.resize_with(new_len, || { AnyVelocityConstraint::Nongrouped( std::mem::MaybeUninit::uninit().assume_init(), diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 19bf7e6..83bbd7e 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -105,6 +105,7 @@ impl VelocityGroundConstraint { let constraint = if insert_at.is_none() { let new_len = out_constraints.len() + 1; unsafe { + #[allow(invalid_value)] out_constraints.resize_with(new_len, || { AnyVelocityConstraint::NongroupedGround( std::mem::MaybeUninit::uninit().assume_init(), From cb9350fd802a6641597140c22e2a0ce4b2ebeb1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 11 Dec 2022 15:50:25 +0100 Subject: [PATCH 08/10] Fix tests --- examples3d/vehicle_controller3.rs | 2 +- src/pipeline/collision_pipeline.rs | 2 ++ src/pipeline/physics_pipeline.rs | 3 +++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/examples3d/vehicle_controller3.rs b/examples3d/vehicle_controller3.rs index 4f36d9f..1228c82 100644 --- a/examples3d/vehicle_controller3.rs +++ b/examples3d/vehicle_controller3.rs @@ -8,7 +8,7 @@ pub fn init_world(testbed: &mut Testbed) { */ let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let mut impulse_joints = ImpulseJointSet::new(); + let impulse_joints = ImpulseJointSet::new(); let multibody_joints = MultibodyJointSet::new(); /* diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index d0beedd..b086fbe 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -203,6 +203,7 @@ mod tests { &mut narrow_phase, &mut rigid_body_set, &mut collider_set, + None, &physics_hooks, &(), ); @@ -254,6 +255,7 @@ mod tests { &mut narrow_phase, &mut rigid_body_set, &mut collider_set, + None, &physics_hooks, &(), ); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 7226063..9ef4177 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -677,6 +677,7 @@ mod test { &mut impulse_joints, &mut multibody_joints, &mut CCDSolver::new(), + None, &(), &(), ); @@ -732,6 +733,7 @@ mod test { &mut impulse_joints, &mut multibody_joints, &mut CCDSolver::new(), + None, &(), &(), ); @@ -834,6 +836,7 @@ mod test { &mut impulse_joints, &mut multibody_joints, &mut ccd, + None, &physics_hooks, &event_handler, ); From 0207f8cf96a3b091ca851276f98f3b482e2a39f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 11 Dec 2022 17:47:16 +0100 Subject: [PATCH 09/10] Properly take initial sleeping state set by the user when creating a rigid-body --- CHANGELOG.md | 5 +++++ src/dynamics/island_manager.rs | 20 +++++++++++++------- src/dynamics/rigid_body.rs | 6 +++++- src/geometry/narrow_phase.rs | 10 ++++++++-- src/pipeline/physics_pipeline.rs | 17 +++++++++++++++-- src/pipeline/user_changes.rs | 9 --------- 6 files changed, 46 insertions(+), 21 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 15dd631..72c99a1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,11 @@ - Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time as these other pipelines. In that case, calling `QueryPipeline::update` a `PhysicsPipeline::step` isn’t needed. +- `RigidBody::set_body_type` now takes an extra boolean argument indicating if the rigid-body should be woken-up + (if it becomes dynamic). + +### Fix +- Fix bug resulting in rigid-bodies being awakened after they are created, even if they are created sleeping. ## v0.16.1 (10 Nov. 2022) ### Fix diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index edd2579..e196503 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -1,6 +1,6 @@ use crate::dynamics::{ - ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle, - RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity, + ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, + RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity, }; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; @@ -96,12 +96,18 @@ impl IslandManager { // attempting to wake-up a rigid-body that has already been deleted. if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { let rb = bodies.index_mut_internal(handle); - rb.activation.wake_up(strong); - if rb.is_enabled() && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) - { - rb.ids.active_set_id = self.active_dynamic_set.len(); - self.active_dynamic_set.push(handle); + // Check that the user didn’t change the sleeping state explicitly, in which + // case we don’t overwrite it. + if !rb.changes.contains(RigidBodyChanges::SLEEP) { + rb.activation.wake_up(strong); + + if rb.is_enabled() + && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) + { + rb.ids.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } } } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 26be983..bb1e9bf 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -135,7 +135,7 @@ impl RigidBody { } /// Sets the type of this rigid-body. - pub fn set_body_type(&mut self, status: RigidBodyType) { + pub fn set_body_type(&mut self, status: RigidBodyType, wake_up: bool) { if status != self.body_type { self.changes.insert(RigidBodyChanges::TYPE); self.body_type = status; @@ -143,6 +143,10 @@ impl RigidBody { if status == RigidBodyType::Fixed { self.vels = RigidBodyVelocity::zero(); } + + if self.is_dynamic() && wake_up { + self.wake_up(true); + } } } diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index ecd1623..dacca9e 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -299,7 +299,13 @@ impl NarrowPhase { } } - self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events); + self.handle_user_changes_on_colliders( + islands, + modified_colliders, + colliders, + bodies, + events, + ); } pub(crate) fn remove_collider( @@ -393,7 +399,7 @@ impl NarrowPhase { } } - pub(crate) fn handle_modified_colliders( + pub(crate) fn handle_user_changes_on_colliders( &mut self, mut islands: Option<&mut IslandManager>, modified_colliders: &[ColliderHandle], diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 9ef4177..358efed 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -5,7 +5,7 @@ use crate::counters::Counters; use crate::dynamics::IslandSolver; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyPosition, RigidBodyType, + RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType, }; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; @@ -77,6 +77,18 @@ impl PhysicsPipeline { } } + fn clear_modified_bodies( + &mut self, + bodies: &mut RigidBodySet, + modified_bodies: &mut Vec, + ) { + for handle in modified_bodies.drain(..) { + if let Some(rb) = bodies.get_mut_internal(handle) { + rb.changes = RigidBodyChanges::empty(); + } + } + } + fn detect_collisions( &mut self, integration_parameters: &IntegrationParameters, @@ -430,7 +442,7 @@ impl PhysicsPipeline { &modified_colliders[..], ); - let modified_bodies = bodies.take_modified(); + let mut modified_bodies = bodies.take_modified(); super::user_changes::handle_user_changes_to_rigid_bodies( Some(islands), bodies, @@ -481,6 +493,7 @@ impl PhysicsPipeline { } self.clear_modified_colliders(colliders, &mut modified_colliders); + self.clear_modified_bodies(bodies, &mut modified_bodies); removed_colliders.clear(); let mut remaining_time = integration_parameters.dt; diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index 00096de..0f4058c 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -86,14 +86,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( ids.active_set_id, )); } - - // Add to the active dynamic set. - activation.wake_up(true); - // Make sure the sleep change flag is set (even if for some - // reasons the rigid-body was already awake) to make - // sure the code handling sleeping change adds the body to - // the active_dynamic_set. - changes.set(RigidBodyChanges::SLEEP, true); } RigidBodyType::KinematicVelocityBased | RigidBodyType::KinematicPositionBased => { @@ -216,7 +208,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( } } - rb.changes = RigidBodyChanges::empty(); rb.ids = ids; rb.activation = activation; } From a1e255dbcdbfde270df32eeda59360493649c73f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 11 Dec 2022 17:52:51 +0100 Subject: [PATCH 10/10] Fix warnings --- src/pipeline/user_changes.rs | 6 +++--- src_testbed/harness/mod.rs | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index 0f4058c..eeda97a 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -65,9 +65,9 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( } let rb = bodies.index_mut_internal(*handle); - let mut changes = rb.changes; let mut ids = rb.ids; - let mut activation = rb.activation; + let changes = rb.changes; + let activation = rb.activation; { if rb.is_enabled() { @@ -126,7 +126,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // sleeping and if it is not already inside of the active set. if changes.contains(RigidBodyChanges::SLEEP) && rb.is_enabled() - && !activation.sleeping // May happen if the body was put to sleep manually. + && !rb.activation.sleeping // May happen if the body was put to sleep manually. && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) { diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 91d4da9..9608592 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -215,7 +215,7 @@ impl Harness { &mut physics.impulse_joints, &mut physics.multibody_joints, &mut physics.ccd_solver, - &mut physics.query_pipeline, + Some(&mut physics.query_pipeline), &*physics.hooks, event_handler, );