diff --git a/CHANGELOG.md b/CHANGELOG.md index f5b8ddc1..10447581 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,16 +23,25 @@ which was its hardcoded behaviour. ### Modified -- `RapierContext`, `RapierConfiguration` and `RenderToSimulationTime` are now a `Component` instead of resources. - - Rapier now supports multiple independent physics worlds, see example `multi_world3` for usage details. - - Migration guide: - - `ResMut` -> `WriteDefaultRapierContext` - - `Res` -> `ReadDefaultRapierContext` - - Access to `RapierConfiguration` and `RenderToSimulationTime` should query for it -on the responsible entity owning the `RenderContext`. - - If you are building a library on top of `bevy_rapier` and would want to support multiple independent physics worlds too, -you can check out the details of [#545](https://github.com/dimforge/bevy_rapier/pull/545) -to get more context and information. +- Rapier now supports multiple independent physics worlds, see example `multi_world3` for usage details. + - `RapierContext`, `RapierConfiguration` and `SimulationToRenderTime` are no longer a resource. + - They have been split in multiple `Component`s: + - `RapierContextColliders` + - `RapierContextJoints` + - `RapierContextSimulation` + - `RapierRigidBodySet` + - `SimulationToRenderTime` + - `RapierConfiguration` + - Migration guide: + - `ResMut` -> `WriteRapierContext` + - `Res` -> `ReadRapierContext` + - Access to `RapierConfiguration` and `SimulationToRenderTime` should query for it + on the responsible entity owning the `RenderContext`. + - See [`ray_casting`](bevy_rapier3d/examples/ray_casting3.rs) example for a usage example. + - Each entity managed by bevy_rapier has a `RapierContextEntityLink` pointing to the entity containing the components above. + - If you are building a library on top of `bevy_rapier` and would want to support multiple independent physics worlds too, + you can check out the details of [#545](https://github.com/dimforge/bevy_rapier/pull/545) + to find more information. ## v0.27.0 (07 July 2024) diff --git a/bevy_rapier2d/examples/debugdump2.rs b/bevy_rapier2d/examples/debugdump2.rs index 190a5079..4d133d91 100644 --- a/bevy_rapier2d/examples/debugdump2.rs +++ b/bevy_rapier2d/examples/debugdump2.rs @@ -1,6 +1,6 @@ //! Example using bevy_mod_debugdump to output a graph of systems execution order. //! run with: -//! `cargo run --example debugdump2 > dump.dot && dot -Tsvg dump.dot > dump.svg` +//! `cargo run --example debugdump2 > dump.dot && dot -Tsvg dump.dot > dump.svg` use bevy::prelude::*; use bevy_mod_debugdump::{schedule_graph, schedule_graph_dot}; diff --git a/bevy_rapier2d/examples/testbed2.rs b/bevy_rapier2d/examples/testbed2.rs index 8a433e22..f415fb1f 100644 --- a/bevy_rapier2d/examples/testbed2.rs +++ b/bevy_rapier2d/examples/testbed2.rs @@ -204,11 +204,12 @@ fn main() { OnExit(Examples::PlayerMovement2), ( cleanup, - |mut rapier_config: Query<&mut RapierConfiguration>, - ctxt: ReadDefaultRapierContext| { + |mut rapier_config: Query<&mut RapierConfiguration>, ctxt: ReadRapierContext| { let mut rapier_config = rapier_config.single_mut(); - rapier_config.gravity = - RapierConfiguration::new(ctxt.integration_parameters.length_unit).gravity; + rapier_config.gravity = RapierConfiguration::new( + ctxt.single().simulation.integration_parameters.length_unit, + ) + .gravity; }, ), ) diff --git a/bevy_rapier3d/examples/joints3.rs b/bevy_rapier3d/examples/joints3.rs index 2864d33b..4a229177 100644 --- a/bevy_rapier3d/examples/joints3.rs +++ b/bevy_rapier3d/examples/joints3.rs @@ -284,7 +284,7 @@ pub fn setup_physics(mut commands: Commands) { } pub fn print_impulse_revolute_joints( - context: ReadDefaultRapierContext, + context: ReadRapierContext, joints: Query<(Entity, &ImpulseJoint)>, ) { for (entity, impulse_joint) in joints.iter() { @@ -293,7 +293,7 @@ pub fn print_impulse_revolute_joints( println!( "angle for {}: {:?}", entity, - context.impulse_revolute_joint_angle(entity), + context.single().impulse_revolute_joint_angle(entity), ); } _ => {} diff --git a/bevy_rapier3d/examples/multi_world3.rs b/bevy_rapier3d/examples/multi_contexts3.rs similarity index 93% rename from bevy_rapier3d/examples/multi_world3.rs rename to bevy_rapier3d/examples/multi_contexts3.rs index dc5fa807..c00fecd3 100644 --- a/bevy_rapier3d/examples/multi_world3.rs +++ b/bevy_rapier3d/examples/multi_contexts3.rs @@ -30,9 +30,9 @@ fn main() { fn create_worlds(mut commands: Commands) { for i in 0..N_WORLDS { - let mut world = commands.spawn((RapierContext::default(), WorldId(i))); + let mut world = commands.spawn((RapierContextBundle::default(), WorldId(i))); if i == 0 { - world.insert(DefaultRapierContext); + world.insert((DefaultRapierContext, RapierContextBundle::default())); } } } @@ -75,7 +75,7 @@ fn change_world( } pub fn setup_physics( - context: Query<(Entity, &WorldId), With>, + context: Query<(Entity, &WorldId), With>, mut commands: Commands, ) { for (context_entity, id) in context.iter() { diff --git a/bevy_rapier3d/examples/rapier_context_component.rs b/bevy_rapier3d/examples/rapier_context_component.rs new file mode 100644 index 00000000..156793a2 --- /dev/null +++ b/bevy_rapier3d/examples/rapier_context_component.rs @@ -0,0 +1,61 @@ +use bevy::prelude::*; +use bevy_rapier3d::prelude::*; + +fn main() { + App::new() + .add_plugins(( + DefaultPlugins, + RapierPhysicsPlugin::::default(), + RapierDebugRenderPlugin::default(), + )) + .add_systems(Startup, (setup_physics, setup_graphics)) + .add_systems(PostUpdate, display_nb_colliders) + .run(); +} + +/// Demonstrates how to access a more specific component of [`RapierContext`] +fn display_nb_colliders( + query_context: Query<&RapierContextColliders, With>, + mut exit: EventWriter, +) { + let nb_colliders = query_context.single().colliders.len(); + println!("There are {nb_colliders} colliders."); + if nb_colliders > 0 { + exit.send(AppExit::Success); + } +} + +pub fn setup_physics(mut commands: Commands) { + /* + * Ground + */ + let ground_size = 5.1; + let ground_height = 0.1; + + let starting_y = -0.5 - ground_height; + + commands.spawn(( + TransformBundle::from(Transform::from_xyz(0.0, starting_y, 0.0)), + Collider::cuboid(ground_size, ground_height, ground_size), + )); + + for _ in 0..3 { + /* + * Create the cubes + */ + + commands.spawn(( + TransformBundle::default(), + RigidBody::Dynamic, + Collider::cuboid(0.5, 0.5, 0.5), + )); + } +} + +fn setup_graphics(mut commands: Commands) { + commands.spawn(Camera3dBundle { + transform: Transform::from_xyz(0.0, 3.0, -10.0) + .looking_at(Vec3::new(0.0, 0.0, 0.0), Vec3::Y), + ..Default::default() + }); +} diff --git a/bevy_rapier3d/examples/ray_casting3.rs b/bevy_rapier3d/examples/ray_casting3.rs index d5e45d9e..ae392025 100644 --- a/bevy_rapier3d/examples/ray_casting3.rs +++ b/bevy_rapier3d/examples/ray_casting3.rs @@ -76,7 +76,7 @@ pub fn setup_physics(mut commands: Commands) { pub fn cast_ray( mut commands: Commands, windows: Query<&Window, With>, - rapier_context: ReadDefaultRapierContext, + rapier_context: ReadRapierContext, cameras: Query<(&Camera, &GlobalTransform)>, ) { let window = windows.single(); @@ -91,9 +91,9 @@ pub fn cast_ray( let Some(ray) = camera.viewport_to_world(camera_transform, cursor_position) else { return; }; - + let context = rapier_context.single(); // Then cast the ray. - let hit = rapier_context.cast_ray( + let hit = context.cast_ray( ray.origin, ray.direction.into(), f32::MAX, diff --git a/bevy_rapier_benches3d/src/lib.rs b/bevy_rapier_benches3d/src/lib.rs index 6cf4a67d..bd6b16b8 100644 --- a/bevy_rapier_benches3d/src/lib.rs +++ b/bevy_rapier_benches3d/src/lib.rs @@ -9,7 +9,7 @@ use common::default_app; use common::wait_app_start; use bevy::prelude::*; -use bevy_rapier3d::plugin::RapierContext; +use bevy_rapier3d::plugin::context::RapierContextSimulation; pub fn custom_bencher(steps: usize, setup: impl Fn(&mut App)) { let mut app = default_app(); @@ -28,7 +28,7 @@ pub fn custom_bencher(steps: usize, setup: impl Fn(&mut App)) { let elapsed_time = timer_full_update.time() as f32; let rc = app .world_mut() - .query::<&RapierContext>() + .query::<&RapierContextSimulation>() .single(app.world()); rapier_step_times.push(rc.pipeline.counters.step_time.time() as f32); total_update_times.push(elapsed_time); diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index 3ce31619..7d76c206 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -2,7 +2,7 @@ use crate::geometry::{Collider, CollisionGroups, ShapeCastHit}; use crate::math::{Real, Rot, Vect}; use bevy::prelude::*; -use crate::plugin::RapierContext; +use crate::plugin::context::RapierContextColliders; pub use rapier::control::CharacterAutostep; pub use rapier::control::CharacterLength; use rapier::prelude::{ColliderSet, QueryFilterFlags}; @@ -26,7 +26,7 @@ pub struct CharacterCollision { impl CharacterCollision { pub(crate) fn from_raw( - ctxt: &RapierContext, + ctxt: &RapierContextColliders, c: &rapier::control::CharacterCollision, ) -> Option { Self::from_raw_with_set(&ctxt.colliders, c, true) @@ -37,7 +37,7 @@ impl CharacterCollision { c: &rapier::control::CharacterCollision, details_always_computed: bool, ) -> Option { - RapierContext::collider_entity_with_set(colliders, c.handle).map(|entity| { + RapierContextColliders::collider_entity_with_set(colliders, c.handle).map(|entity| { CharacterCollision { entity, character_translation: c.character_pos.translation.vector.into(), diff --git a/src/dynamics/revolute_joint.rs b/src/dynamics/revolute_joint.rs index 8d2bb33d..2dc6b51b 100644 --- a/src/dynamics/revolute_joint.rs +++ b/src/dynamics/revolute_joint.rs @@ -1,11 +1,14 @@ use crate::dynamics::{GenericJoint, GenericJointBuilder}; use crate::math::{Real, Vect}; -use crate::plugin::RapierContext; +use crate::plugin::context::RapierRigidBodySet; use bevy::prelude::Entity; use rapier::dynamics::{ JointAxesMask, JointAxis, JointLimits, JointMotor, MotorModel, RigidBodyHandle, RigidBodySet, }; +#[cfg(doc)] +use crate::prelude::RapierContext; + use super::TypedJoint; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -147,7 +150,7 @@ impl RevoluteJoint { /// Similarly [`RapierContext::impulse_revolute_joint_angle`] only takes a single entity as argument to compute that angle. /// /// # Parameters - /// - `bodies` : the rigid body set from [`RapierContext`] + /// - `bodies` : the rigid body set from [`RapierRigidBodySet`] /// - `body1`: the first rigid-body attached to this revolute joint, obtained through [`rapier::dynamics::ImpulseJoint`] or [`rapier::dynamics::MultibodyJoint`]. /// - `body2`: the second rigid-body attached to this revolute joint, obtained through [`rapier::dynamics::ImpulseJoint`] or [`rapier::dynamics::MultibodyJoint`]. pub fn angle_from_handles( @@ -167,13 +170,13 @@ impl RevoluteJoint { /// The angle along the free degree of freedom of this revolute joint in `[-π, π]`. /// /// # Parameters - /// - `bodies` : the rigid body set from [`RapierContext`] + /// - `bodies` : the rigid body set from [`RapierRigidBodySet`] /// - `body1`: the first rigid-body attached to this revolute joint. /// - `body2`: the second rigid-body attached to this revolute joint. - pub fn angle(&self, context: &RapierContext, body1: Entity, body2: Entity) -> f32 { - let rb1 = context.entity2body().get(&body1).unwrap(); - let rb2 = context.entity2body().get(&body2).unwrap(); - self.angle_from_handles(&context.bodies, *rb1, *rb2) + pub fn angle(&self, rigidbody_set: &RapierRigidBodySet, body1: Entity, body2: Entity) -> f32 { + let rb1 = rigidbody_set.entity2body().get(&body1).unwrap(); + let rb2 = rigidbody_set.entity2body().get(&body2).unwrap(); + self.angle_from_handles(&rigidbody_set.bodies, *rb1, *rb2) } } diff --git a/src/lib.rs b/src/lib.rs index 8b544df3..427c278b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -72,6 +72,8 @@ pub mod prelude { pub use crate::geometry::*; pub use crate::math::*; pub use crate::pipeline::*; + pub use crate::plugin::context::systemparams::*; + pub use crate::plugin::context::*; pub use crate::plugin::*; #[cfg(any(feature = "debug-render-3d", feature = "debug-render-2d"))] pub use crate::render::*; diff --git a/src/pipeline/events.rs b/src/pipeline/events.rs index 4a169932..857846b5 100644 --- a/src/pipeline/events.rs +++ b/src/pipeline/events.rs @@ -182,7 +182,7 @@ mod test { )); // 2 seconds should be plenty of time for the cube to fall on the // lowest collider. - for _ in 0..120 { + for _ in 0..1020 { app.update(); } let saved_collisions = app diff --git a/src/pipeline/query_filter.rs b/src/pipeline/query_filter.rs index b60df7f9..2d4b5cd3 100644 --- a/src/pipeline/query_filter.rs +++ b/src/pipeline/query_filter.rs @@ -5,11 +5,11 @@ pub use rapier::pipeline::QueryFilterFlags; use crate::geometry::CollisionGroups; #[cfg(doc)] -use crate::prelude::RapierContext; +use crate::prelude::RapierRigidBodySet; /// A filter that describes what collider should be included or excluded from a scene query. /// -/// For testing manually check [`RapierContext::with_query_filter`]. +/// For testing manually check [`RapierRigidBodySet::with_query_filter`]. #[derive(Copy, Clone, Default)] pub struct QueryFilter<'a> { /// Flags indicating what particular type of colliders should be excluded. diff --git a/src/plugin/configuration.rs b/src/plugin/configuration.rs index edb3529c..9a33c51d 100644 --- a/src/plugin/configuration.rs +++ b/src/plugin/configuration.rs @@ -1,3 +1,5 @@ +//! Components used to configure a simulation run by rapier, these are not modified by bevy_rapier. + use bevy::{ prelude::{Component, Resource}, reflect::Reflect, @@ -8,13 +10,6 @@ use crate::math::{Real, Vect}; #[cfg(doc)] use {crate::prelude::TransformInterpolation, rapier::dynamics::IntegrationParameters}; -/// Difference between simulation and rendering time -#[derive(Component, Default, Reflect)] -pub struct SimulationToRenderTime { - /// Difference between simulation and rendering time - pub diff: f32, -} - /// The different ways of adjusting the timestep length each frame. #[derive(Copy, Clone, Debug, PartialEq, Resource)] pub enum TimestepMode { diff --git a/src/plugin/context/mod.rs b/src/plugin/context/mod.rs index 63205b0d..81a56a7f 100644 --- a/src/plugin/context/mod.rs +++ b/src/plugin/context/mod.rs @@ -1,3 +1,5 @@ +//! These are components used and modified during a simulation frame. + pub mod systemparams; use bevy::prelude::*; @@ -19,131 +21,98 @@ use bevy::prelude::{Entity, EventWriter, GlobalTransform, Query}; use crate::control::{CharacterCollision, MoveShapeOptions, MoveShapeOutput}; use crate::dynamics::TransformInterpolation; use crate::parry::query::details::ShapeCastOptions; -use crate::plugin::configuration::{SimulationToRenderTime, TimestepMode}; +use crate::plugin::configuration::TimestepMode; use crate::prelude::{CollisionGroups, RapierRigidBodyHandle}; use rapier::control::CharacterAutostep; use rapier::geometry::DefaultBroadPhase; #[cfg(doc)] -use crate::prelude::{ImpulseJoint, MultibodyJoint, RevoluteJoint, TypedJoint}; +use crate::prelude::{ + systemparams::{RapierContext, ReadRapierContext}, + ImpulseJoint, MultibodyJoint, RevoluteJoint, TypedJoint, +}; + +/// Difference between simulation and rendering time +#[derive(Component, Default, Reflect, Clone)] +pub struct SimulationToRenderTime { + /// Difference between simulation and rendering time + pub diff: f32, +} -/// Marker component for to access the default [`RapierContext`]. +/// Marker component for to access the default [`ReadRapierContext`]. /// -/// This is used by [`systemparams::ReadDefaultRapierContext`] and other default accesses +/// This is use as the default marker filter for [`systemparams::ReadRapierContext`] and [`systemparams::WriteRapierContext`] /// to help with getting a reference to the correct RapierContext. /// /// If you're making a library, you might be interested in [`RapierContextEntityLink`] -/// and leverage a [`Query<&RapierContext>`] to find the correct [`RapierContext`] of an entity. +/// and leverage a [`Query`] to have precise access to relevant components (for example [`RapierContextSimulation`]). +/// +/// See the list of full components in [`RapierContext`] #[derive(Component, Reflect, Debug, Clone, Copy)] pub struct DefaultRapierContext; +/// A Bundle to regroup useful components for a rapier context. +#[derive(Bundle, Default)] +pub struct RapierContextBundle { + /// See [`RapierContextColliders`] + pub colliders: RapierContextColliders, + /// See [`RapierContextJoints`] + pub joints: RapierContextJoints, + /// See [`RapierQueryPipeline`] + pub query: RapierQueryPipeline, + /// See [`RapierContextSimulation`] + pub simulation: RapierContextSimulation, + /// See [`RapierRigidBodySet`] + pub bodies: RapierRigidBodySet, + /// See [`SimulationToRenderTime`] + pub simulation_to_render_time: SimulationToRenderTime, +} + /// This is a component applied to any entity containing a rapier handle component. -/// The inner Entity referred to has the component [`RapierContext`] responsible for handling +/// The inner Entity referred to has the component [`RapierContextSimulation`] +/// and others from [`crate::plugin::context`], responsible for handling /// its rapier data. #[derive(Component, Reflect, Debug, Clone, Copy, PartialEq, Eq, Hash)] pub struct RapierContextEntityLink(pub Entity); -/// The Rapier context, containing all the state of the physics engine. +/// The set of colliders part of the simulation. +/// +/// This should be attached on an entity with a [`RapierContextSimulation`] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Component)] -pub struct RapierContext { - /// The island manager, which detects what object is sleeping - /// (not moving much) to reduce computations. - pub islands: IslandManager, - /// The broad-phase, which detects potential contact pairs. - pub broad_phase: DefaultBroadPhase, - /// The narrow-phase, which computes contact points, tests intersections, - /// and maintain the contact and intersection graphs. - pub narrow_phase: NarrowPhase, - /// The set of rigid-bodies part of the simulation. - pub bodies: RigidBodySet, +#[derive(Component, Default, Debug, Clone)] +pub struct RapierContextColliders { /// The set of colliders part of the simulation. pub colliders: ColliderSet, - /// The set of impulse joints part of the simulation. - pub impulse_joints: ImpulseJointSet, - /// The set of multibody joints part of the simulation. - pub multibody_joints: MultibodyJointSet, - /// The solver, which handles Continuous Collision Detection (CCD). - pub ccd_solver: CCDSolver, - /// The physics pipeline, which advance the simulation step by step. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub pipeline: PhysicsPipeline, - /// The query pipeline, which performs scene queries (ray-casting, point projection, etc.) - pub query_pipeline: QueryPipeline, - /// The integration parameters, controlling various low-level coefficient of the simulation. - pub integration_parameters: IntegrationParameters, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) event_handler: Option>, - // For transform change detection. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) last_body_transform_set: HashMap, - // NOTE: these maps are needed to handle despawning. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) entity2body: HashMap, #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) entity2collider: HashMap, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) entity2impulse_joint: HashMap, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) entity2multibody_joint: HashMap, - // This maps the handles of colliders that have been deleted since the last - // physics update, to the entity they was attached to. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) deleted_colliders: HashMap, - - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) collision_events_to_send: Vec, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) contact_force_events_to_send: Vec, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) character_collisions_collector: Vec, -} - -impl Default for RapierContext { - fn default() -> Self { - Self { - islands: IslandManager::new(), - broad_phase: DefaultBroadPhase::new(), - narrow_phase: NarrowPhase::new(), - bodies: RigidBodySet::new(), - colliders: ColliderSet::new(), - impulse_joints: ImpulseJointSet::new(), - multibody_joints: MultibodyJointSet::new(), - ccd_solver: CCDSolver::new(), - pipeline: PhysicsPipeline::new(), - query_pipeline: QueryPipeline::new(), - integration_parameters: IntegrationParameters::default(), - event_handler: None, - last_body_transform_set: HashMap::new(), - entity2body: HashMap::new(), - entity2collider: HashMap::new(), - entity2impulse_joint: HashMap::new(), - entity2multibody_joint: HashMap::new(), - deleted_colliders: HashMap::new(), - collision_events_to_send: Vec::new(), - contact_force_events_to_send: Vec::new(), - character_collisions_collector: Vec::new(), - } - } } -impl RapierContext { +impl RapierContextColliders { /// If the collider attached to `entity` is attached to a rigid-body, this /// returns the `Entity` containing that rigid-body. - pub fn collider_parent(&self, entity: Entity) -> Option { + pub fn collider_parent( + &self, + rigidbody_set: &RapierRigidBodySet, + entity: Entity, + ) -> Option { self.entity2collider .get(&entity) .and_then(|h| self.colliders.get(*h)) .and_then(|co| co.parent()) - .and_then(|h| self.rigid_body_entity(h)) + .and_then(|h| rigidbody_set.rigid_body_entity(h)) } /// If entity is a rigid-body, this returns the collider `Entity`s attached /// to that rigid-body. - pub fn rigid_body_colliders(&self, entity: Entity) -> impl Iterator + '_ { - self.entity2body() + pub fn rigid_body_colliders<'a, 'b: 'a>( + &'a self, + entity: Entity, + rigidbody_set: &'b RapierRigidBodySet, + ) -> impl Iterator + '_ { + rigidbody_set + .entity2body() .get(&entity) - .and_then(|handle| self.bodies.get(*handle)) + .and_then(|handle| rigidbody_set.bodies.get(*handle)) .map(|body| { body.colliders() .iter() @@ -168,796 +137,956 @@ impl RapierContext { .map(|c| Entity::from_bits(c.user_data as u64)) } - /// Retrieve the Bevy entity the given Rapier rigid-body (identified by its handle) is attached. - pub fn rigid_body_entity(&self, handle: RigidBodyHandle) -> Option { - self.bodies - .get(handle) - .map(|c| Entity::from_bits(c.user_data as u64)) + /// The map from entities to collider handles. + pub fn entity2collider(&self) -> &HashMap { + &self.entity2collider } +} - /// Calls the closure `f` once after converting the given [`QueryFilter`] into a raw `rapier::QueryFilter`. - pub fn with_query_filter( +/// The sets of joints part of the simulation. +/// +/// This should be attached on an entity with a [`RapierContextSimulation`] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Component, Default, Debug, Clone)] +pub struct RapierContextJoints { + /// The set of impulse joints part of the simulation. + pub impulse_joints: ImpulseJointSet, + /// The set of multibody joints part of the simulation. + pub multibody_joints: MultibodyJointSet, + + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) entity2impulse_joint: HashMap, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) entity2multibody_joint: HashMap, +} + +impl RapierContextJoints { + /// The map from entities to impulse joint handles. + pub fn entity2impulse_joint(&self) -> &HashMap { + &self.entity2impulse_joint + } + + /// The map from entities to multibody joint handles. + pub fn entity2multibody_joint(&self) -> &HashMap { + &self.entity2multibody_joint + } +} + +/// The query pipeline, which performs scene queries (ray-casting, point projection, etc.) +/// +/// This should be attached on an entity with a [`RapierContext`] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Component, Default, Clone)] +pub struct RapierQueryPipeline { + /// The query pipeline, which performs scene queries (ray-casting, point projection, etc.) + pub query_pipeline: QueryPipeline, +} + +impl RapierQueryPipeline { + /// Updates the state of the query pipeline, based on the collider positions known + /// from the last timestep or the last call to `self.propagate_modified_body_positions_to_colliders()`. + pub fn update_query_pipeline(&mut self, colliders: &RapierContextColliders) { + self.query_pipeline.update(&colliders.colliders); + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// * `ray_origin`: the starting point of the ray to cast. + /// * `ray_dir`: the direction of the ray to cast. + /// * `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. + /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if + /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary + /// even if its starts inside of it. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + #[expect(clippy::too_many_arguments)] + pub fn cast_ray( &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, filter: QueryFilter, - f: impl FnOnce(RapierQueryFilter) -> T, - ) -> T { - Self::with_query_filter_elts( - &self.entity2collider, - &self.entity2body, - &self.colliders, - filter, - f, - ) + ) -> Option<(Entity, Real)> { + let ray = Ray::new(ray_origin.into(), ray_dir.into()); + + let (h, toi) = + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.cast_ray( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &ray, + max_toi, + solid, + filter, + ) + })?; + + rapier_colliders.collider_entity(h).map(|e| (e, toi)) } - /// Without borrowing the [`RapierContext`], calls the closure `f` once - /// after converting the given [`QueryFilter`] into a raw `rapier::QueryFilter`. - pub fn with_query_filter_elts( - entity2collider: &HashMap, - entity2body: &HashMap, - colliders: &ColliderSet, + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// * `ray_origin`: the starting point of the ray to cast. + /// * `ray_dir`: the direction of the ray to cast. + /// * `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. + /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if + /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary + /// even if its starts inside of it. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + #[expect(clippy::too_many_arguments)] + pub fn cast_ray_and_get_normal( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, filter: QueryFilter, - f: impl FnOnce(RapierQueryFilter) -> T, - ) -> T { - let mut rapier_filter = RapierQueryFilter { - flags: filter.flags, - groups: filter.groups.map(CollisionGroups::into), - exclude_collider: filter - .exclude_collider - .and_then(|c| entity2collider.get(&c).copied()), - exclude_rigid_body: filter - .exclude_rigid_body - .and_then(|b| entity2body.get(&b).copied()), - predicate: None, - }; + ) -> Option<(Entity, RayIntersection)> { + let ray = Ray::new(ray_origin.into(), ray_dir.into()); - if let Some(predicate) = filter.predicate { - let wrapped_predicate = |h: ColliderHandle, _: &rapier::geometry::Collider| { - Self::collider_entity_with_set(colliders, h) - .map(predicate) - .unwrap_or(false) - }; - rapier_filter.predicate = Some(&wrapped_predicate); - f(rapier_filter) - } else { - f(rapier_filter) - } + let (h, result) = + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.cast_ray_and_get_normal( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &ray, + max_toi, + solid, + filter, + ) + })?; + + rapier_colliders + .collider_entity(h) + .map(|e| (e, RayIntersection::from_rapier(result, ray_origin, ray_dir))) } - /// Advance the simulation, based on the given timestep mode. + /// Find the all intersections between a ray and a set of collider and passes them to a callback. + /// + /// # Parameters + /// * `ray_origin`: the starting point of the ray to cast. + /// * `ray_dir`: the direction of the ray to cast. + /// * `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively + /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. + /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if + /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary + /// even if its starts inside of it. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + /// * `callback`: function executed on each collider for which a ray intersection has been found. + /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, + /// this method will exit early, ignore any further raycast. #[allow(clippy::too_many_arguments)] - pub fn step_simulation( - &mut self, - gravity: Vect, - timestep_mode: TimestepMode, - events: Option<( - &EventWriter, - &EventWriter, - )>, - hooks: &dyn PhysicsHooks, - time: &Time, - sim_to_render_time: &mut SimulationToRenderTime, - mut interpolation_query: Option< - &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, - >, + pub fn intersections_with_ray( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + mut callback: impl FnMut(Entity, RayIntersection) -> bool, ) { - let event_queue = if events.is_some() { - Some(EventQueue { - deleted_colliders: &self.deleted_colliders, - collision_events: RwLock::new(Vec::new()), - contact_force_events: RwLock::new(Vec::new()), - }) - } else { - None + let ray = Ray::new(ray_origin.into(), ray_dir.into()); + let callback = |h, inter: rapier::prelude::RayIntersection| { + rapier_colliders + .collider_entity(h) + .map(|e| callback(e, RayIntersection::from_rapier(inter, ray_origin, ray_dir))) + .unwrap_or(true) }; - let event_handler = self - .event_handler - .as_deref() - .or_else(|| event_queue.as_ref().map(|q| q as &dyn EventHandler)) - .unwrap_or(&() as &dyn EventHandler); + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.intersections_with_ray( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &ray, + max_toi, + solid, + filter, + callback, + ) + }); + } - let mut executed_steps = 0; - match timestep_mode { - TimestepMode::Interpolated { - dt, - time_scale, - substeps, - } => { - self.integration_parameters.dt = dt; - - sim_to_render_time.diff += time.delta_seconds(); - - while sim_to_render_time.diff > 0.0 { - // NOTE: in this comparison we do the same computations we - // will do for the next `while` iteration test, to make sure we - // don't get bit by potential float inaccuracy. - if sim_to_render_time.diff - dt <= 0.0 { - if let Some(interpolation_query) = interpolation_query.as_mut() { - // This is the last simulation step to be executed in the loop - // Update the previous state transforms - for (handle, mut interpolation) in interpolation_query.iter_mut() { - if let Some(body) = self.bodies.get(handle.0) { - interpolation.start = Some(*body.position()); - interpolation.end = None; - } - } - } - } - - let mut substep_integration_parameters = self.integration_parameters; - substep_integration_parameters.dt = dt / (substeps as Real) * time_scale; - - for _ in 0..substeps { - self.pipeline.step( - &gravity.into(), - &substep_integration_parameters, - &mut self.islands, - &mut self.broad_phase, - &mut self.narrow_phase, - &mut self.bodies, - &mut self.colliders, - &mut self.impulse_joints, - &mut self.multibody_joints, - &mut self.ccd_solver, - None, - hooks, - event_handler, - ); - executed_steps += 1; - } - - sim_to_render_time.diff -= dt; - } - } - TimestepMode::Variable { - max_dt, - time_scale, - substeps, - } => { - self.integration_parameters.dt = (time.delta_seconds() * time_scale).min(max_dt); - - let mut substep_integration_parameters = self.integration_parameters; - substep_integration_parameters.dt /= substeps as Real; - - for _ in 0..substeps { - self.pipeline.step( - &gravity.into(), - &substep_integration_parameters, - &mut self.islands, - &mut self.broad_phase, - &mut self.narrow_phase, - &mut self.bodies, - &mut self.colliders, - &mut self.impulse_joints, - &mut self.multibody_joints, - &mut self.ccd_solver, - None, - hooks, - event_handler, - ); - executed_steps += 1; - } - } - TimestepMode::Fixed { dt, substeps } => { - self.integration_parameters.dt = dt; - - let mut substep_integration_parameters = self.integration_parameters; - substep_integration_parameters.dt = dt / (substeps as Real); + /// Gets the handle of up to one collider intersecting the given shape. + /// + /// # Parameters + /// * `shape_pos` - The position of the shape used for the intersection test. + /// * `shape` - The shape used for the intersection test. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn intersection_with_shape( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + shape_pos: Vect, + shape_rot: Rot, + shape: &Collider, + filter: QueryFilter, + ) -> Option { + let scaled_transform = (shape_pos, shape_rot).into(); + let mut scaled_shape = shape.clone(); + // TODO: how to set a good number of subdivisions, we don’t have access to the + // RapierConfiguration::scaled_shape_subdivision here. + scaled_shape.set_scale(shape.scale, 20); - for _ in 0..substeps { - self.pipeline.step( - &gravity.into(), - &substep_integration_parameters, - &mut self.islands, - &mut self.broad_phase, - &mut self.narrow_phase, - &mut self.bodies, - &mut self.colliders, - &mut self.impulse_joints, - &mut self.multibody_joints, - &mut self.ccd_solver, - None, - hooks, - event_handler, - ); - executed_steps += 1; - } - } - } - if let Some(mut event_queue) = event_queue { - // NOTE: event_queue and its inner locks are only accessed from - // within `self.pipeline.step` called above, so we can unwrap here safely. - self.collision_events_to_send = - std::mem::take(event_queue.collision_events.get_mut().unwrap()); - self.contact_force_events_to_send = - std::mem::take(event_queue.contact_force_events.get_mut().unwrap()); - } + let h = rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.intersection_with_shape( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &scaled_transform, + &*scaled_shape.raw, + filter, + ) + })?; - if executed_steps > 0 { - self.deleted_colliders.clear(); - } - } - /// Generates bevy events for any physics interactions that have happened - /// that are stored in the events list - pub fn send_bevy_events( - &mut self, - collision_event_writer: &mut EventWriter, - contact_force_event_writer: &mut EventWriter, - ) { - for collision_event in self.collision_events_to_send.drain(..) { - collision_event_writer.send(collision_event); - } - for contact_force_event in self.contact_force_events_to_send.drain(..) { - contact_force_event_writer.send(contact_force_event); - } + rapier_colliders.collider_entity(h) } - /// This method makes sure that the rigid-body positions have been propagated to - /// their attached colliders, without having to perform a simulation step. - pub fn propagate_modified_body_positions_to_colliders(&mut self) { - self.bodies - .propagate_modified_body_positions_to_colliders(&mut self.colliders); - } + /// Find the projection of a point on the closest collider. + /// + /// # Parameters + /// * `point` - The point to project. + /// * `solid` - If this is set to `true` then the collider shapes are considered to + /// be plain (if the point is located inside of a plain shape, its projection is the point + /// itself). If it is set to `false` the collider shapes are considered to be hollow + /// (if the point is located inside of an hollow shape, it is projected on the shape's + /// boundary). + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn project_point( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + point: Vect, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, PointProjection)> { + let (h, result) = + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.project_point( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &point.into(), + solid, + filter, + ) + })?; - /// Updates the state of the query pipeline, based on the collider positions known - /// from the last timestep or the last call to `self.propagate_modified_body_positions_to_colliders()`. - pub fn update_query_pipeline(&mut self) { - self.query_pipeline.update(&self.colliders); + rapier_colliders + .collider_entity(h) + .map(|e| (e, PointProjection::from_rapier(result))) } - /// The map from entities to rigid-body handles. - pub fn entity2body(&self) -> &HashMap { - &self.entity2body - } + /// Find all the colliders containing the given point. + /// + /// # Parameters + /// * `point` - The point used for the containment test. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + /// * `callback` - A function called with each collider with a shape containing the `point`. + /// If this callback returns `false`, this method will exit early, ignore any + /// further point projection. + pub fn intersections_with_point( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + point: Vect, + filter: QueryFilter, + mut callback: impl FnMut(Entity) -> bool, + ) { + #[allow(clippy::redundant_closure)] + // False-positive, we can't move callback, closure becomes `FnOnce` + let callback = |h| { + rapier_colliders + .collider_entity(h) + .map(|e| callback(e)) + .unwrap_or(true) + }; - /// The map from entities to collider handles. - pub fn entity2collider(&self) -> &HashMap { - &self.entity2collider + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.intersections_with_point( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &point.into(), + filter, + callback, + ) + }); } - /// The map from entities to impulse joint handles. - pub fn entity2impulse_joint(&self) -> &HashMap { - &self.entity2impulse_joint + /// Find the projection of a point on the closest collider. + /// + /// The results include the ID of the feature hit by the point. + /// + /// # Parameters + /// * `point` - The point to project. + /// * `solid` - If this is set to `true` then the collider shapes are considered to + /// be plain (if the point is located inside of a plain shape, its projection is the point + /// itself). If it is set to `false` the collider shapes are considered to be hollow + /// (if the point is located inside of an hollow shape, it is projected on the shape's + /// boundary). + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + pub fn project_point_and_get_feature( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + point: Vect, + filter: QueryFilter, + ) -> Option<(Entity, PointProjection, FeatureId)> { + let (h, proj, fid) = + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.project_point_and_get_feature( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &point.into(), + filter, + ) + })?; + + rapier_colliders + .collider_entity(h) + .map(|e| (e, PointProjection::from_rapier(proj), fid)) } - /// The map from entities to multibody joint handles. - pub fn entity2multibody_joint(&self) -> &HashMap { - &self.entity2multibody_joint + /// Finds all entities of all the colliders with an Aabb intersecting the given Aabb. + #[cfg(not(feature = "headless"))] + pub fn colliders_with_aabb_intersecting_aabb( + &self, + rapier_colliders: &RapierContextColliders, + aabb: bevy::render::primitives::Aabb, + mut callback: impl FnMut(Entity) -> bool, + ) { + #[cfg(feature = "dim2")] + let scaled_aabb = rapier::prelude::Aabb { + mins: aabb.min().xy().into(), + maxs: aabb.max().xy().into(), + }; + #[cfg(feature = "dim3")] + let scaled_aabb = rapier::prelude::Aabb { + mins: aabb.min().into(), + maxs: aabb.max().into(), + }; + #[allow(clippy::redundant_closure)] + // False-positive, we can't move callback, closure becomes `FnOnce` + let callback = |h: &ColliderHandle| { + rapier_colliders + .collider_entity(*h) + .map(|e| callback(e)) + .unwrap_or(true) + }; + self.query_pipeline + .colliders_with_aabb_intersecting_aabb(&scaled_aabb, callback); } - /// Attempts to move shape, optionally sliding or climbing obstacles. + /// Casts a shape at a constant linear velocity and retrieve the first collider it hits. + /// + /// This is similar to ray-casting except that we are casting a whole shape instead of just a + /// point (the ray origin). In the resulting `ShapeCastHit`, witness and normal 1 refer to the world + /// collider, and are in world space. /// /// # Parameters - /// * `movement`: the translational movement to apply. - /// * `shape`: the shape to move. - /// * `shape_translation`: the initial position of the shape. - /// * `shape_rotation`: the rotation of the shape. - /// * `shape_mass`: the mass of the shape to be considered by the impulse calculation if - /// `MoveShapeOptions::apply_impulse_to_dynamic_bodies` is set to true. - /// * `options`: configures the behavior of the automatic sliding and climbing. - /// * `filter`: indicates what collider or rigid-body needs to be ignored by the obstacle detection. - /// * `events`: callback run on each obstacle hit by the shape on its path. + /// * `shape_pos` - The initial translation of the shape to cast. + /// * `shape_rot` - The rotation of the shape to cast. + /// * `shape_vel` - The constant velocity of the shape to cast (i.e. the cast direction). + /// * `shape` - The shape to cast. + /// * `max_toi` - The maximum time-of-impact that can be reported by this cast. This effectively + /// limits the distance traveled by the shape to `shape_vel.norm() * maxToi`. + /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any + /// collider, two results are possible. If `stop_at_penetration` is `true` then, the + /// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` + /// then the nonlinear shape-casting will see if further motion wrt. the penetration normal + /// would result in tunnelling. If it does not (i.e. we have a separating velocity along + /// that normal) then the nonlinear shape-casting will attempt to find another impact, + /// at a time `> start_time` that could result in tunnelling. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. #[allow(clippy::too_many_arguments)] - pub fn move_shape( - &mut self, - movement: Vect, + pub fn cast_shape( + &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, + shape_pos: Vect, + shape_rot: Rot, + shape_vel: Vect, shape: &Collider, - shape_translation: Vect, - shape_rotation: Rot, - shape_mass: Real, - options: &MoveShapeOptions, + options: ShapeCastOptions, filter: QueryFilter, - mut events: impl FnMut(CharacterCollision), - ) -> MoveShapeOutput { + ) -> Option<(Entity, ShapeCastHit)> { + let scaled_transform = (shape_pos, shape_rot).into(); let mut scaled_shape = shape.clone(); // TODO: how to set a good number of subdivisions, we don’t have access to the // RapierConfiguration::scaled_shape_subdivision here. scaled_shape.set_scale(shape.scale, 20); - let up = options - .up - .try_into() - .expect("The up vector must be non-zero."); - let autostep = options.autostep.map(|autostep| CharacterAutostep { - max_height: autostep.max_height, - min_width: autostep.min_width, - include_dynamic_bodies: autostep.include_dynamic_bodies, - }); - let controller = rapier::control::KinematicCharacterController { - up, - offset: options.offset, - slide: options.slide, - autostep, - max_slope_climb_angle: options.max_slope_climb_angle, - min_slope_slide_angle: options.min_slope_slide_angle, - snap_to_ground: options.snap_to_ground, - normal_nudge_factor: options.normal_nudge_factor, - }; - - self.character_collisions_collector.clear(); - - // TODO: having to grab all the references to avoid having self in - // the closure is ugly. - let dt = self.integration_parameters.dt; - let colliders = &self.colliders; - let bodies = &mut self.bodies; - let query_pipeline = &self.query_pipeline; - let collisions = &mut self.character_collisions_collector; - collisions.clear(); - - let result = Self::with_query_filter_elts( - &self.entity2collider, - &self.entity2body, - &self.colliders, - filter, - move |filter| { - let result = controller.move_shape( - dt, - bodies, - colliders, - query_pipeline, - (&scaled_shape).into(), - &(shape_translation, shape_rotation).into(), - movement.into(), + let (h, result) = + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.cast_shape( + &rigidbody_set.bodies, + &rapier_colliders.colliders, + &scaled_transform, + &shape_vel.into(), + &*scaled_shape.raw, + options, filter, - |c| { - if let Some(collision) = - CharacterCollision::from_raw_with_set(colliders, &c, true) - { - events(collision); - } - collisions.push(c); - }, - ); - - if options.apply_impulse_to_dynamic_bodies { - controller.solve_character_collision_impulses( - dt, - bodies, - colliders, - query_pipeline, - (&scaled_shape).into(), - shape_mass, - collisions.iter(), - filter, - ) - } - - result - }, - ); - - MoveShapeOutput { - effective_translation: result.translation.into(), - grounded: result.grounded, - is_sliding_down_slope: result.is_sliding_down_slope, - } - } - - /// Find the closest intersection between a ray and a set of collider. - /// - /// # Parameters - /// * `ray_origin`: the starting point of the ray to cast. - /// * `ray_dir`: the direction of the ray to cast. - /// * `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. - /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if - /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary - /// even if its starts inside of it. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn cast_ray( - &self, - ray_origin: Vect, - ray_dir: Vect, - max_toi: Real, - solid: bool, - filter: QueryFilter, - ) -> Option<(Entity, Real)> { - let ray = Ray::new(ray_origin.into(), ray_dir.into()); + ) + })?; - let (h, toi) = self.with_query_filter(filter, move |filter| { - self.query_pipeline.cast_ray( - &self.bodies, - &self.colliders, - &ray, - max_toi, - solid, - filter, + rapier_colliders.collider_entity(h).map(|e| { + ( + e, + ShapeCastHit::from_rapier(result, options.compute_impact_geometry_on_penetration), ) - })?; - - self.collider_entity(h).map(|e| (e, toi)) + }) } - /// Find the closest intersection between a ray and a set of collider. + /* TODO: we need to wrap the NonlinearRigidMotion somehow. + * + /// Casts a shape with an arbitrary continuous motion and retrieve the first collider it hits. + /// + /// In the resulting `ShapeCastHit`, witness and normal 1 refer to the world collider, and are + /// in world space. /// /// # Parameters - /// * `ray_origin`: the starting point of the ray to cast. - /// * `ray_dir`: the direction of the ray to cast. - /// * `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. - /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if - /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary - /// even if its starts inside of it. + /// * `shape_motion` - The motion of the shape. + /// * `shape` - The shape to cast. + /// * `start_time` - The starting time of the interval where the motion takes place. + /// * `end_time` - The end time of the interval where the motion takes place. + /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any + /// collider, two results are possible. If `stop_at_penetration` is `true` then, the + /// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` + /// then the nonlinear shape-casting will see if further motion wrt. the penetration normal + /// would result in tunnelling. If it does not (i.e. we have a separating velocity along + /// that normal) then the nonlinear shape-casting will attempt to find another impact, + /// at a time `> start_time` that could result in tunnelling. /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn cast_ray_and_get_normal( + pub fn nonlinear_cast_shape( &self, - ray_origin: Vect, - ray_dir: Vect, - max_toi: Real, - solid: bool, + shape_motion: &NonlinearRigidMotion, + shape: &Collider, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, filter: QueryFilter, - ) -> Option<(Entity, RayIntersection)> { - let ray = Ray::new(ray_origin.into(), ray_dir.into()); + ) -> Option<(Entity, Toi)> { + let scaled_transform = (shape_pos, shape_rot).into(); + let mut scaled_shape = shape.clone(); + // TODO: how to set a good number of subdivisions, we don’t have access to the + // RapierConfiguration::scaled_shape_subdivision here. + scaled_shape.set_scale(shape.scale, 20); - let (h, result) = self.with_query_filter(filter, move |filter| { - self.query_pipeline.cast_ray_and_get_normal( - &self.bodies, + let (h, result) = rigidbody_set.with_query_filter(filter, move |filter| { + self.query_pipeline.nonlinear_cast_shape( + &rigidbody_set.bodies, &self.colliders, - &ray, - max_toi, - solid, + shape_motion, + &*scaled_shape.raw, + start_time, + end_time, + stop_at_penetration, filter, ) })?; - self.collider_entity(h) - .map(|e| (e, RayIntersection::from_rapier(result, ray_origin, ray_dir))) - } - - /// Find the all intersections between a ray and a set of collider and passes them to a callback. - /// - /// # Parameters - /// * `ray_origin`: the starting point of the ray to cast. - /// * `ray_dir`: the direction of the ray to cast. - /// * `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray. - /// * `solid`: if this is `true` an impact at time 0.0 (i.e. at the ray origin) is returned if - /// it starts inside of a shape. If this `false` then the ray will hit the shape's boundary - /// even if its starts inside of it. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - /// * `callback`: function executed on each collider for which a ray intersection has been found. - /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, - /// this method will exit early, ignore any further raycast. - #[allow(clippy::too_many_arguments)] - pub fn intersections_with_ray( - &self, - ray_origin: Vect, - ray_dir: Vect, - max_toi: Real, - solid: bool, - filter: QueryFilter, - mut callback: impl FnMut(Entity, RayIntersection) -> bool, - ) { - let ray = Ray::new(ray_origin.into(), ray_dir.into()); - let callback = |h, inter: rapier::prelude::RayIntersection| { - self.collider_entity(h) - .map(|e| callback(e, RayIntersection::from_rapier(inter, ray_origin, ray_dir))) - .unwrap_or(true) - }; - - self.with_query_filter(filter, move |filter| { - self.query_pipeline.intersections_with_ray( - &self.bodies, - &self.colliders, - &ray, - max_toi, - solid, - filter, - callback, - ) - }); + self.collider_entity(h).map(|e| (e, result)) } + */ - /// Gets the handle of up to one collider intersecting the given shape. + /// Retrieve all the colliders intersecting the given shape. /// /// # Parameters - /// * `shape_pos` - The position of the shape used for the intersection test. - /// * `shape` - The shape used for the intersection test. + /// * `shapePos` - The position of the shape to test. + /// * `shapeRot` - The orientation of the shape to test. + /// * `shape` - The shape to test. /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn intersection_with_shape( + /// * `callback` - A function called with the entities of each collider intersecting the `shape`. + #[expect(clippy::too_many_arguments)] + pub fn intersections_with_shape( &self, + rapier_colliders: &RapierContextColliders, + rigidbody_set: &RapierRigidBodySet, shape_pos: Vect, shape_rot: Rot, shape: &Collider, filter: QueryFilter, - ) -> Option { + mut callback: impl FnMut(Entity) -> bool, + ) { let scaled_transform = (shape_pos, shape_rot).into(); let mut scaled_shape = shape.clone(); // TODO: how to set a good number of subdivisions, we don’t have access to the // RapierConfiguration::scaled_shape_subdivision here. scaled_shape.set_scale(shape.scale, 20); - let h = self.with_query_filter(filter, move |filter| { - self.query_pipeline.intersection_with_shape( - &self.bodies, - &self.colliders, + #[allow(clippy::redundant_closure)] + // False-positive, we can't move callback, closure becomes `FnOnce` + let callback = |h| { + rapier_colliders + .collider_entity(h) + .map(|e| callback(e)) + .unwrap_or(true) + }; + + rigidbody_set.with_query_filter(rapier_colliders, filter, move |filter| { + self.query_pipeline.intersections_with_shape( + &rigidbody_set.bodies, + &rapier_colliders.colliders, &scaled_transform, &*scaled_shape.raw, filter, + callback, ) - })?; + }); + } + /// Without borrowing the [`RapierContext`], calls the closure `f` once + /// after converting the given [`QueryFilter`] into a raw [`RapierQueryFilter`]. + pub fn with_query_filter_elts( + entity2collider: &HashMap, + entity2body: &HashMap, + colliders: &ColliderSet, + filter: QueryFilter, + f: impl FnOnce(RapierQueryFilter) -> T, + ) -> T { + let mut rapier_filter = RapierQueryFilter { + flags: filter.flags, + groups: filter.groups.map(CollisionGroups::into), + exclude_collider: filter + .exclude_collider + .and_then(|c| entity2collider.get(&c).copied()), + exclude_rigid_body: filter + .exclude_rigid_body + .and_then(|b| entity2body.get(&b).copied()), + predicate: None, + }; - self.collider_entity(h) + if let Some(predicate) = filter.predicate { + let wrapped_predicate = |h: ColliderHandle, _: &rapier::geometry::Collider| { + RapierContextColliders::collider_entity_with_set(colliders, h) + .map(predicate) + .unwrap_or(false) + }; + rapier_filter.predicate = Some(&wrapped_predicate); + f(rapier_filter) + } else { + f(rapier_filter) + } } +} - /// Find the projection of a point on the closest collider. - /// - /// # Parameters - /// * `point` - The point to project. - /// * `solid` - If this is set to `true` then the collider shapes are considered to - /// be plain (if the point is located inside of a plain shape, its projection is the point - /// itself). If it is set to `false` the collider shapes are considered to be hollow - /// (if the point is located inside of an hollow shape, it is projected on the shape's - /// boundary). - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn project_point( +/// The set of rigid-bodies part of the simulation. +/// +/// This should be attached on an entity with a [`RapierContextSimulation`] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Component, Default, Clone)] +pub struct RapierRigidBodySet { + /// The set of rigid-bodies part of the simulation. + pub bodies: RigidBodySet, + /// NOTE: this map is needed to handle despawning. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) entity2body: HashMap, + + /// For transform change detection. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) last_body_transform_set: HashMap, +} + +impl RapierRigidBodySet { + /// Calls the closure `f` once after converting the given [`QueryFilter`] into a raw [`RapierQueryFilter`]. + pub fn with_query_filter( &self, - point: Vect, - solid: bool, + colliders: &RapierContextColliders, filter: QueryFilter, - ) -> Option<(Entity, PointProjection)> { - let (h, result) = self.with_query_filter(filter, move |filter| { - self.query_pipeline.project_point( - &self.bodies, - &self.colliders, - &point.into(), - solid, - filter, - ) - })?; + f: impl FnOnce(RapierQueryFilter) -> T, + ) -> T { + RapierQueryPipeline::with_query_filter_elts( + &colliders.entity2collider, + &self.entity2body, + &colliders.colliders, + filter, + f, + ) + } - self.collider_entity(h) - .map(|e| (e, PointProjection::from_rapier(result))) + /// The map from entities to rigid-body handles. + pub fn entity2body(&self) -> &HashMap { + &self.entity2body } - /// Find all the colliders containing the given point. - /// - /// # Parameters - /// * `point` - The point used for the containment test. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - /// * `callback` - A function called with each collider with a shape containing the `point`. - /// If this callback returns `false`, this method will exit early, ignore any - /// further point projection. - pub fn intersections_with_point( + /// Retrieve the Bevy entity the given Rapier rigid-body (identified by its handle) is attached. + pub fn rigid_body_entity(&self, handle: RigidBodyHandle) -> Option { + self.bodies + .get(handle) + .map(|c| Entity::from_bits(c.user_data as u64)) + } + + /// This method makes sure that the rigid-body positions have been propagated to + /// their attached colliders, without having to perform a simulation step. + pub fn propagate_modified_body_positions_to_colliders( &self, - point: Vect, - filter: QueryFilter, - mut callback: impl FnMut(Entity) -> bool, + colliders: &mut RapierContextColliders, ) { - #[allow(clippy::redundant_closure)] - // False-positive, we can't move callback, closure becomes `FnOnce` - let callback = |h| self.collider_entity(h).map(|e| callback(e)).unwrap_or(true); - - self.with_query_filter(filter, move |filter| { - self.query_pipeline.intersections_with_point( - &self.bodies, - &self.colliders, - &point.into(), - filter, - callback, - ) - }); + self.bodies + .propagate_modified_body_positions_to_colliders(&mut colliders.colliders); } - /// Find the projection of a point on the closest collider. + /// Computes the angle between the two bodies attached by the [`RevoluteJoint`] component (if any) referenced by the given `entity`. /// - /// The results include the ID of the feature hit by the point. + /// The angle is computed along the revolute joint’s principal axis. /// - /// # Parameters - /// * `point` - The point to project. - /// * `solid` - If this is set to `true` then the collider shapes are considered to - /// be plain (if the point is located inside of a plain shape, its projection is the point - /// itself). If it is set to `false` the collider shapes are considered to be hollow - /// (if the point is located inside of an hollow shape, it is projected on the shape's - /// boundary). - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn project_point_and_get_feature( + /// Parameter `entity` should have a [`ImpulseJoint`] component with a [`TypedJoint::RevoluteJoint`] variant as `data`. + pub fn impulse_revolute_joint_angle( &self, - point: Vect, - filter: QueryFilter, - ) -> Option<(Entity, PointProjection, FeatureId)> { - let (h, proj, fid) = self.with_query_filter(filter, move |filter| { - self.query_pipeline.project_point_and_get_feature( - &self.bodies, - &self.colliders, - &point.into(), - filter, - ) - })?; + joints: &RapierContextJoints, + entity: Entity, + ) -> Option { + let joint_handle = joints.entity2impulse_joint().get(&entity)?; + let impulse_joint = joints.impulse_joints.get(*joint_handle)?; + let revolute_joint = impulse_joint.data.as_revolute()?; + + let rb1 = &self.bodies[impulse_joint.body1]; + let rb2 = &self.bodies[impulse_joint.body2]; + Some(revolute_joint.angle(rb1.rotation(), rb2.rotation())) + } +} + +/// The Rapier context, containing parts of the state of the physics engine, specific to the simulation step. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Component)] +pub struct RapierContextSimulation { + /// The island manager, which detects what object is sleeping + /// (not moving much) to reduce computations. + pub islands: IslandManager, + /// The broad-phase, which detects potential contact pairs. + pub broad_phase: DefaultBroadPhase, + /// The narrow-phase, which computes contact points, tests intersections, + /// and maintain the contact and intersection graphs. + pub narrow_phase: NarrowPhase, + /// The solver, which handles Continuous Collision Detection (CCD). + pub ccd_solver: CCDSolver, + /// The physics pipeline, which advance the simulation step by step. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub pipeline: PhysicsPipeline, + /// The integration parameters, controlling various low-level coefficient of the simulation. + pub integration_parameters: IntegrationParameters, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) event_handler: Option>, + // This maps the handles of colliders that have been deleted since the last + // physics update, to the entity they was attached to. + /// NOTE: this map is needed to handle despawning. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) deleted_colliders: HashMap, + + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) collision_events_to_send: Vec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) contact_force_events_to_send: Vec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) character_collisions_collector: Vec, +} + +impl Default for RapierContextSimulation { + fn default() -> Self { + Self { + islands: IslandManager::new(), + broad_phase: DefaultBroadPhase::new(), + narrow_phase: NarrowPhase::new(), + ccd_solver: CCDSolver::new(), + pipeline: PhysicsPipeline::new(), + integration_parameters: IntegrationParameters::default(), + event_handler: None, + deleted_colliders: HashMap::new(), + collision_events_to_send: Vec::new(), + contact_force_events_to_send: Vec::new(), + character_collisions_collector: Vec::new(), + } + } +} + +impl RapierContextSimulation { + /// Advance the simulation, based on the given timestep mode. + #[allow(clippy::too_many_arguments)] + pub fn step_simulation( + &mut self, + colliders: &mut RapierContextColliders, + joints: &mut RapierContextJoints, + rigidbody_set: &mut RapierRigidBodySet, + gravity: Vect, + timestep_mode: TimestepMode, + events: Option<( + &EventWriter, + &EventWriter, + )>, + hooks: &dyn PhysicsHooks, + time: &Time, + sim_to_render_time: &mut SimulationToRenderTime, + mut interpolation_query: Option< + &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, + >, + ) { + let event_queue = if events.is_some() { + Some(EventQueue { + deleted_colliders: &self.deleted_colliders, + collision_events: RwLock::new(Vec::new()), + contact_force_events: RwLock::new(Vec::new()), + }) + } else { + None + }; + + let event_handler = self + .event_handler + .as_deref() + .or_else(|| event_queue.as_ref().map(|q| q as &dyn EventHandler)) + .unwrap_or(&() as &dyn EventHandler); + + let mut executed_steps = 0; + match timestep_mode { + TimestepMode::Interpolated { + dt, + time_scale, + substeps, + } => { + self.integration_parameters.dt = dt; + + sim_to_render_time.diff += time.delta_seconds(); + + while sim_to_render_time.diff > 0.0 { + // NOTE: in this comparison we do the same computations we + // will do for the next `while` iteration test, to make sure we + // don't get bit by potential float inaccuracy. + if sim_to_render_time.diff - dt <= 0.0 { + if let Some(interpolation_query) = interpolation_query.as_mut() { + // This is the last simulation step to be executed in the loop + // Update the previous state transforms + for (handle, mut interpolation) in interpolation_query.iter_mut() { + if let Some(body) = rigidbody_set.bodies.get(handle.0) { + interpolation.start = Some(*body.position()); + interpolation.end = None; + } + } + } + } + + let mut substep_integration_parameters = self.integration_parameters; + substep_integration_parameters.dt = dt / (substeps as Real) * time_scale; + + for _ in 0..substeps { + self.pipeline.step( + &gravity.into(), + &substep_integration_parameters, + &mut self.islands, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut rigidbody_set.bodies, + &mut colliders.colliders, + &mut joints.impulse_joints, + &mut joints.multibody_joints, + &mut self.ccd_solver, + None, + hooks, + event_handler, + ); + executed_steps += 1; + } + + sim_to_render_time.diff -= dt; + } + } + TimestepMode::Variable { + max_dt, + time_scale, + substeps, + } => { + self.integration_parameters.dt = (time.delta_seconds() * time_scale).min(max_dt); + + let mut substep_integration_parameters = self.integration_parameters; + substep_integration_parameters.dt /= substeps as Real; + + for _ in 0..substeps { + self.pipeline.step( + &gravity.into(), + &substep_integration_parameters, + &mut self.islands, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut rigidbody_set.bodies, + &mut colliders.colliders, + &mut joints.impulse_joints, + &mut joints.multibody_joints, + &mut self.ccd_solver, + None, + hooks, + event_handler, + ); + executed_steps += 1; + } + } + TimestepMode::Fixed { dt, substeps } => { + self.integration_parameters.dt = dt; + + let mut substep_integration_parameters = self.integration_parameters; + substep_integration_parameters.dt = dt / (substeps as Real); + + for _ in 0..substeps { + self.pipeline.step( + &gravity.into(), + &substep_integration_parameters, + &mut self.islands, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut rigidbody_set.bodies, + &mut colliders.colliders, + &mut joints.impulse_joints, + &mut joints.multibody_joints, + &mut self.ccd_solver, + None, + hooks, + event_handler, + ); + executed_steps += 1; + } + } + } + if let Some(mut event_queue) = event_queue { + // NOTE: event_queue and its inner locks are only accessed from + // within `self.pipeline.step` called above, so we can unwrap here safely. + self.collision_events_to_send = + std::mem::take(event_queue.collision_events.get_mut().unwrap()); + self.contact_force_events_to_send = + std::mem::take(event_queue.contact_force_events.get_mut().unwrap()); + } - self.collider_entity(h) - .map(|e| (e, PointProjection::from_rapier(proj), fid)) + if executed_steps > 0 { + self.deleted_colliders.clear(); + } } - - /// Finds all entities of all the colliders with an Aabb intersecting the given Aabb. - #[cfg(not(feature = "headless"))] - pub fn colliders_with_aabb_intersecting_aabb( - &self, - aabb: bevy::render::primitives::Aabb, - mut callback: impl FnMut(Entity) -> bool, + /// Generates bevy events for any physics interactions that have happened + /// that are stored in the events list + pub fn send_bevy_events( + &mut self, + collision_event_writer: &mut EventWriter, + contact_force_event_writer: &mut EventWriter, ) { - #[cfg(feature = "dim2")] - let scaled_aabb = rapier::prelude::Aabb { - mins: aabb.min().xy().into(), - maxs: aabb.max().xy().into(), - }; - #[cfg(feature = "dim3")] - let scaled_aabb = rapier::prelude::Aabb { - mins: aabb.min().into(), - maxs: aabb.max().into(), - }; - #[allow(clippy::redundant_closure)] - // False-positive, we can't move callback, closure becomes `FnOnce` - let callback = |h: &ColliderHandle| { - self.collider_entity(*h) - .map(|e| callback(e)) - .unwrap_or(true) - }; - self.query_pipeline - .colliders_with_aabb_intersecting_aabb(&scaled_aabb, callback); + for collision_event in self.collision_events_to_send.drain(..) { + collision_event_writer.send(collision_event); + } + for contact_force_event in self.contact_force_events_to_send.drain(..) { + contact_force_event_writer.send(contact_force_event); + } } - /// Casts a shape at a constant linear velocity and retrieve the first collider it hits. - /// - /// This is similar to ray-casting except that we are casting a whole shape instead of just a - /// point (the ray origin). In the resulting `ShapeCastHit`, witness and normal 1 refer to the world - /// collider, and are in world space. + /// Attempts to move shape, optionally sliding or climbing obstacles. /// /// # Parameters - /// * `shape_pos` - The initial translation of the shape to cast. - /// * `shape_rot` - The rotation of the shape to cast. - /// * `shape_vel` - The constant velocity of the shape to cast (i.e. the cast direction). - /// * `shape` - The shape to cast. - /// * `max_toi` - The maximum time-of-impact that can be reported by this cast. This effectively - /// limits the distance traveled by the shape to `shape_vel.norm() * maxToi`. - /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any - /// collider, two results are possible. If `stop_at_penetration` is `true` then, the - /// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` - /// then the nonlinear shape-casting will see if further motion wrt. the penetration normal - /// would result in tunnelling. If it does not (i.e. we have a separating velocity along - /// that normal) then the nonlinear shape-casting will attempt to find another impact, - /// at a time `> start_time` that could result in tunnelling. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + /// * `movement`: the translational movement to apply. + /// * `shape`: the shape to move. + /// * `shape_translation`: the initial position of the shape. + /// * `shape_rotation`: the rotation of the shape. + /// * `shape_mass`: the mass of the shape to be considered by the impulse calculation if + /// `MoveShapeOptions::apply_impulse_to_dynamic_bodies` is set to true. + /// * `options`: configures the behavior of the automatic sliding and climbing. + /// * `filter`: indicates what collider or rigid-body needs to be ignored by the obstacle detection. + /// * `events`: callback run on each obstacle hit by the shape on its path. #[allow(clippy::too_many_arguments)] - pub fn cast_shape( - &self, - shape_pos: Vect, - shape_rot: Rot, - shape_vel: Vect, - shape: &Collider, - options: ShapeCastOptions, - filter: QueryFilter, - ) -> Option<(Entity, ShapeCastHit)> { - let scaled_transform = (shape_pos, shape_rot).into(); - let mut scaled_shape = shape.clone(); - // TODO: how to set a good number of subdivisions, we don’t have access to the - // RapierConfiguration::scaled_shape_subdivision here. - scaled_shape.set_scale(shape.scale, 20); - - let (h, result) = self.with_query_filter(filter, move |filter| { - self.query_pipeline.cast_shape( - &self.bodies, - &self.colliders, - &scaled_transform, - &shape_vel.into(), - &*scaled_shape.raw, - options, - filter, - ) - })?; - - self.collider_entity(h).map(|e| { - ( - e, - ShapeCastHit::from_rapier(result, options.compute_impact_geometry_on_penetration), - ) - }) - } - - /* TODO: we need to wrap the NonlinearRigidMotion somehow. - * - /// Casts a shape with an arbitrary continuous motion and retrieve the first collider it hits. - /// - /// In the resulting `ShapeCastHit`, witness and normal 1 refer to the world collider, and are - /// in world space. - /// - /// # Parameters - /// * `shape_motion` - The motion of the shape. - /// * `shape` - The shape to cast. - /// * `start_time` - The starting time of the interval where the motion takes place. - /// * `end_time` - The end time of the interval where the motion takes place. - /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any - /// collider, two results are possible. If `stop_at_penetration` is `true` then, the - /// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` - /// then the nonlinear shape-casting will see if further motion wrt. the penetration normal - /// would result in tunnelling. If it does not (i.e. we have a separating velocity along - /// that normal) then the nonlinear shape-casting will attempt to find another impact, - /// at a time `> start_time` that could result in tunnelling. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - pub fn nonlinear_cast_shape( - &self, - shape_motion: &NonlinearRigidMotion, + pub fn move_shape( + &mut self, + rapier_colliders: &RapierContextColliders, + rapier_query_pipeline: &RapierQueryPipeline, + rigidbody_set: &mut RapierRigidBodySet, + movement: Vect, shape: &Collider, - start_time: Real, - end_time: Real, - stop_at_penetration: bool, + shape_translation: Vect, + shape_rotation: Rot, + shape_mass: Real, + options: &MoveShapeOptions, filter: QueryFilter, - ) -> Option<(Entity, Toi)> { - let scaled_transform = (shape_pos, shape_rot).into(); + mut events: impl FnMut(CharacterCollision), + ) -> MoveShapeOutput { let mut scaled_shape = shape.clone(); // TODO: how to set a good number of subdivisions, we don’t have access to the // RapierConfiguration::scaled_shape_subdivision here. scaled_shape.set_scale(shape.scale, 20); - let (h, result) = self.with_query_filter(filter, move |filter| { - self.query_pipeline.nonlinear_cast_shape( - &self.bodies, - &self.colliders, - shape_motion, - &*scaled_shape.raw, - start_time, - end_time, - stop_at_penetration, - filter, - ) - })?; + let up = options + .up + .try_into() + .expect("The up vector must be non-zero."); + let autostep = options.autostep.map(|autostep| CharacterAutostep { + max_height: autostep.max_height, + min_width: autostep.min_width, + include_dynamic_bodies: autostep.include_dynamic_bodies, + }); + let controller = rapier::control::KinematicCharacterController { + up, + offset: options.offset, + slide: options.slide, + autostep, + max_slope_climb_angle: options.max_slope_climb_angle, + min_slope_slide_angle: options.min_slope_slide_angle, + snap_to_ground: options.snap_to_ground, + normal_nudge_factor: options.normal_nudge_factor, + }; - self.collider_entity(h).map(|e| (e, result)) - } - */ + self.character_collisions_collector.clear(); - /// Retrieve all the colliders intersecting the given shape. - /// - /// # Parameters - /// * `shapePos` - The position of the shape to test. - /// * `shapeRot` - The orientation of the shape to test. - /// * `shape` - The shape to test. - /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. - /// * `callback` - A function called with the entities of each collider intersecting the `shape`. - pub fn intersections_with_shape( - &self, - shape_pos: Vect, - shape_rot: Rot, - shape: &Collider, - filter: QueryFilter, - mut callback: impl FnMut(Entity) -> bool, - ) { - let scaled_transform = (shape_pos, shape_rot).into(); - let mut scaled_shape = shape.clone(); - // TODO: how to set a good number of subdivisions, we don’t have access to the - // RapierConfiguration::scaled_shape_subdivision here. - scaled_shape.set_scale(shape.scale, 20); + // TODO: having to grab all the references to avoid having self in + // the closure is ugly. + let dt = self.integration_parameters.dt; + let colliders = &rapier_colliders.colliders; + let bodies = &mut rigidbody_set.bodies; + let query_pipeline = &rapier_query_pipeline.query_pipeline; + let collisions = &mut self.character_collisions_collector; + collisions.clear(); - #[allow(clippy::redundant_closure)] - // False-positive, we can't move callback, closure becomes `FnOnce` - let callback = |h| self.collider_entity(h).map(|e| callback(e)).unwrap_or(true); + let result = RapierQueryPipeline::with_query_filter_elts( + &rapier_colliders.entity2collider, + &rigidbody_set.entity2body, + &rapier_colliders.colliders, + filter, + move |filter| { + let result = controller.move_shape( + dt, + bodies, + colliders, + query_pipeline, + (&scaled_shape).into(), + &(shape_translation, shape_rotation).into(), + movement.into(), + filter, + |c| { + if let Some(collision) = + CharacterCollision::from_raw_with_set(colliders, &c, true) + { + events(collision); + } + collisions.push(c); + }, + ); - self.with_query_filter(filter, move |filter| { - self.query_pipeline.intersections_with_shape( - &self.bodies, - &self.colliders, - &scaled_transform, - &*scaled_shape.raw, - filter, - callback, - ) - }); - } + if options.apply_impulse_to_dynamic_bodies { + controller.solve_character_collision_impulses( + dt, + bodies, + colliders, + query_pipeline, + (&scaled_shape).into(), + shape_mass, + collisions.iter(), + filter, + ) + } - /// Computes the angle between the two bodies attached by the [`RevoluteJoint`] component (if any) referenced by the given `entity`. - /// - /// The angle is computed along the revolute joint’s principal axis. - /// - /// Parameter `entity` should have a [`ImpulseJoint`] component with a [`TypedJoint::RevoluteJoint`] variant as `data`. - pub fn impulse_revolute_joint_angle(&self, entity: Entity) -> Option { - let joint_handle = self.entity2impulse_joint().get(&entity)?; - let impulse_joint = self.impulse_joints.get(*joint_handle)?; - let revolute_joint = impulse_joint.data.as_revolute()?; + result + }, + ); - let rb1 = &self.bodies[impulse_joint.body1]; - let rb2 = &self.bodies[impulse_joint.body2]; - Some(revolute_joint.angle(rb1.rotation(), rb2.rotation())) + MoveShapeOutput { + effective_translation: result.translation.into(), + grounded: result.grounded, + is_sliding_down_slope: result.is_sliding_down_slope, + } } } diff --git a/src/plugin/context/systemparams/mod.rs b/src/plugin/context/systemparams/mod.rs index 93a630e6..e86c90a4 100644 --- a/src/plugin/context/systemparams/mod.rs +++ b/src/plugin/context/systemparams/mod.rs @@ -1,3 +1,8 @@ +//! Helper types to ease bevy_rapier API usage, grouping often used together components +//! through [`bevy::ecs::query::QueryData`] or [`bevy::ecs::system::SystemParam`] to +//! avoid writing too much boilerplate: shorter parameters for your systems, +//! and pass less parameters to bey_rapier lower level functions. + mod rapier_context_systemparam; use bevy::{ecs::query::QueryData, prelude::Entity}; @@ -5,8 +10,12 @@ pub use rapier_context_systemparam::*; use super::RapierContextEntityLink; +/// Information needed to access the rapier data from an entity managed by rapier. #[derive(QueryData)] pub struct RapierEntity { + /// This bevy [`Entity`]. pub entity: Entity, + /// Link to another bevy [`Entity`], which owns the context of this [`RapierEntity`]: + /// i.e. the relevant components from [`crate::plugin::context`]. pub rapier_context_link: &'static RapierContextEntityLink, } diff --git a/src/plugin/context/systemparams/rapier_context_systemparam.rs b/src/plugin/context/systemparams/rapier_context_systemparam.rs index 0bc5be72..35de2ed6 100644 --- a/src/plugin/context/systemparams/rapier_context_systemparam.rs +++ b/src/plugin/context/systemparams/rapier_context_systemparam.rs @@ -1,140 +1,682 @@ -use bevy::ecs::system::SystemParam; +use crate::math::{Rot, Vect}; +use bevy::ecs::{query, system::SystemParam}; use bevy::prelude::*; -use std::ops::{Deref, DerefMut}; +use rapier::prelude::Real; -use super::super::{DefaultRapierContext, RapierContext, RapierContextEntityLink}; -/// Utility [`SystemParam`] to easily access the single default [`RapierContext`] immutably. -/// -/// SAFETY: Dereferencing this struct will panic if its underlying query fails. -/// See [`RapierContextAccess`] for a safer alternative. +pub(crate) const RAPIER_CONTEXT_EXPECT_ERROR: &str = + "RapierContextEntityLink.0 refers to an entity missing components from RapierContextBundle."; + +use crate::{ + plugin::context::{ + DefaultRapierContext, RapierContextColliders, RapierContextJoints, RapierContextSimulation, + RapierQueryPipeline, RapierRigidBodySet, + }, + prelude::QueryFilter, +}; + +#[cfg(doc)] +use crate::prelude::RapierContextBundle; + +/// Utility [`SystemParam`] to easily access every required components of a [`RapierContext`] immutably. #[derive(SystemParam)] -pub struct ReadDefaultRapierContext<'w, 's, T: Component = DefaultRapierContext> { - rapier_context: Query<'w, 's, &'static RapierContext, With>, +pub struct ReadRapierContext<'w, 's, T: query::QueryFilter + 'static = With> { + /// The query used to feed components into [`RapierContext`] struct through [`ReadRapierContext::single`]. + pub rapier_context: Query< + 'w, + 's, + ( + &'static RapierContextSimulation, + &'static RapierContextColliders, + &'static RapierContextJoints, + &'static RapierQueryPipeline, + &'static RapierRigidBodySet, + ), + T, + >, } -impl<'w, 's, T: Component> ReadDefaultRapierContext<'w, 's, T> { +impl<'w, 's, T: query::QueryFilter + 'static> ReadRapierContext<'w, 's, T> { /// Use this method if you only have one [`RapierContext`]. /// /// SAFETY: This method will panic if its underlying query fails. - /// See [`RapierContextAccess`] for a safe alternative. - pub fn single(&'_ self) -> &RapierContext { - self.rapier_context.single() - } -} - -impl<'w, 's> Deref for ReadDefaultRapierContext<'w, 's> { - type Target = RapierContext; - - /// Use this method if you only have one [`RapierContext`]. /// - /// SAFETY: This method will panic if its underlying query fails. - /// See [`RapierContextAccess`] for a safe alternative. - fn deref(&self) -> &Self::Target { - self.rapier_context.single() + /// Use the underlying query [`ReadRapierContext::rapier_context`] for safer alternatives. + pub fn single(&'_ self) -> RapierContext { + let (simulation, colliders, joints, query_pipeline, rigidbody_set) = + self.rapier_context.single(); + RapierContext { + simulation, + colliders, + joints, + query_pipeline, + rigidbody_set, + } } } -/// Utility [`SystemParam`] to easily access the single default [`RapierContext`] mutably. +/// A helper struct to avoid passing too many parameters to most rapier functions. +/// This helps with reducing boilerplate, at the (small) price of maybe getting too much information from the ECS. /// -/// SAFETY: Dereferencing this struct will panic if its underlying query fails. -/// See [`RapierContextAccess`] for a safer alternative. -#[derive(SystemParam)] -pub struct WriteDefaultRapierContext<'w, 's, T: Component = DefaultRapierContext> { - rapier_context: Query<'w, 's, &'static mut RapierContext, With>, +/// Note: This is not a component, refer to [`ReadRapierContext`], [`WriteRapierContext`], or [`RapierContextBundle`] +#[derive(query::QueryData)] +pub struct RapierContext<'a> { + /// The Rapier context, containing all the state of the physics engine. + pub simulation: &'a RapierContextSimulation, + /// The set of colliders part of the simulation. + pub colliders: &'a RapierContextColliders, + /// The sets of joints part of the simulation. + pub joints: &'a RapierContextJoints, + /// The query pipeline, which performs scene queries (ray-casting, point projection, etc.) + pub query_pipeline: &'a RapierQueryPipeline, + /// The set of rigid-bodies part of the simulation. + pub rigidbody_set: &'a RapierRigidBodySet, } -impl<'w, 's, T: Component> Deref for WriteDefaultRapierContext<'w, 's, T> { - type Target = RapierContext; +/// Utility [`SystemParam`] to easily access every required components of a [`RapierContext`] mutably. +#[derive(SystemParam)] +pub struct WriteRapierContext<'w, 's, T: query::QueryFilter + 'static = With> +{ + /// The query used to feed components into [`RapierContext`] struct through [`ReadRapierContext::single`]. + pub rapier_context: Query< + 'w, + 's, + ( + &'static mut RapierContextSimulation, + &'static mut RapierContextColliders, + &'static mut RapierContextJoints, + &'static mut RapierQueryPipeline, + &'static mut RapierRigidBodySet, + ), + T, + >, +} - /// Use this method if you only have one [`RapierContext`]. +impl<'w, 's, T: query::QueryFilter + 'static> WriteRapierContext<'w, 's, T> { + /// Use this method if you only have one [`RapierContext`] corresponding to the filter (T) of [`WriteRapierContext`]. /// /// SAFETY: This method will panic if its underlying query fails. - /// See [`RapierContextAccess`] for a safe alternative. - fn deref(&self) -> &Self::Target { - self.rapier_context.single() + /// Use the underlying query [`WriteRapierContext::rapier_context`] for safer alternatives. + pub fn single(&self) -> RapierContext { + let (simulation, colliders, joints, query_pipeline, rigidbody_set) = + self.rapier_context.single(); + RapierContext { + simulation, + colliders, + joints, + query_pipeline, + rigidbody_set, + } } -} - -impl<'w, 's> DerefMut for WriteDefaultRapierContext<'w, 's> { /// Use this method if you only have one [`RapierContext`]. /// /// SAFETY: This method will panic if its underlying query fails. - /// See [`RapierContextAccess`] for a safe alternative. - fn deref_mut(&mut self) -> &mut Self::Target { - // TODO: should we cache the result ? - self.rapier_context.single_mut().into_inner() + /// Use the underlying query [`WriteRapierContext::rapier_context`] for safer alternatives. + pub fn single_mut(&mut self) -> RapierContextMut { + let (simulation, colliders, joints, query_pipeline, rigidbody_set) = + self.rapier_context.single_mut(); + RapierContextMut { + simulation, + colliders, + joints, + query_pipeline, + rigidbody_set, + } } } -/// Utility [`SystemParam`] to easily access any [`RapierContext`] immutably -#[derive(SystemParam)] -pub struct RapierContextAccess<'w, 's> { - /// Query used to retrieve a [`RapierContext`]. - /// It's helpful to iterate over every rapier contexts, - /// or get a handle over a specific context, for example through: - /// - a marker component such as [`DefaultRapierContext`] - /// - a [`RapierContextEntityLink`]. See [context](RapierContextAccess::context) - pub rapier_context: Query<'w, 's, &'static RapierContext>, +/// A helper struct to avoid passing too many parameters to most rapier functions. +/// This helps with reducing boilerplate, at the (small) price of maybe getting too much information from the ECS. +/// +/// If you need more granular control over mutability of each component, use a regular [`Query`] +pub struct RapierContextMut<'a> { + /// The Rapier context, containing all the state of the physics engine. + pub simulation: Mut<'a, RapierContextSimulation>, + /// The set of colliders part of the simulation. + pub colliders: Mut<'a, RapierContextColliders>, + /// The sets of joints part of the simulation. + pub joints: Mut<'a, RapierContextJoints>, + /// The query pipeline, which performs scene queries (ray-casting, point projection, etc.) + pub query_pipeline: Mut<'a, RapierQueryPipeline>, + /// The set of rigid-bodies part of the simulation. + pub rigidbody_set: Mut<'a, RapierRigidBodySet>, } -impl<'w, 's> RapierContextAccess<'w, 's> { - /// Retrieves the rapier context responsible for the entity owning the given [`RapierContextEntityLink`]. - /// - /// SAFETY: This method will panic if its underlying query fails. - /// See [`Self::try_context`] for a safe alternative. - pub fn context(&self, link: &RapierContextEntityLink) -> &'_ RapierContext { - self.try_context(link) - .expect("RapierContextEntityLink.0 refers to an entity without RapierContext.") +/// [`RapierRigidBodySet`] functions +mod simulation { + use crate::control::CharacterCollision; + use crate::control::MoveShapeOptions; + use crate::control::MoveShapeOutput; + use crate::plugin::context::SimulationToRenderTime; + use crate::plugin::ContactPairView; + use crate::plugin::TimestepMode; + use crate::prelude::Collider; + use crate::prelude::CollisionEvent; + use crate::prelude::ContactForceEvent; + use crate::prelude::RapierRigidBodyHandle; + use crate::prelude::TransformInterpolation; + use rapier::prelude::PhysicsHooks; + + use super::*; + + /// [`RapierContextSimulation`] functions for immutable accesses + impl<'a> RapierContext<'a> { + /// Shortcut to [`RapierContextSimulation::contact_pair`]. + pub fn contact_pair( + &self, + collider1: Entity, + collider2: Entity, + ) -> Option { + self.simulation + .contact_pair(self.colliders, self.rigidbody_set, collider1, collider2) + } + + /// Shortcut to [`RapierContextSimulation::contact_pairs_with`]. + pub fn contact_pairs_with( + &self, + collider: Entity, + ) -> impl Iterator { + self.simulation + .contact_pairs_with(self.colliders, self.rigidbody_set, collider) + } + + /// Shortcut to [`RapierContextSimulation::intersection_pair`]. + pub fn intersection_pair(&self, collider1: Entity, collider2: Entity) -> Option { + self.simulation + .intersection_pair(self.colliders, collider1, collider2) + } + + /// Shortcut to [`RapierContextSimulation::intersection_pairs_with`]. + pub fn intersection_pairs_with( + &self, + collider: Entity, + ) -> impl Iterator + '_ { + self.simulation + .intersection_pairs_with(self.colliders, collider) + } } - /// Retrieves the rapier context responsible for the entity owning the given [`RapierContextEntityLink`]. - pub fn try_context(&self, link: &RapierContextEntityLink) -> Option<&'_ RapierContext> { - self.rapier_context.get(link.0).ok() + /// [`RapierContextSimulation`] functions for mutable accesses + impl<'a> RapierContextMut<'a> { + /// Shortcut to [`RapierContextSimulation::step_simulation`]. + #[expect(clippy::too_many_arguments)] + pub fn step_simulation( + &mut self, + gravity: Vect, + timestep_mode: TimestepMode, + events: Option<( + &EventWriter, + &EventWriter, + )>, + hooks: &dyn PhysicsHooks, + time: &Time, + sim_to_render_time: &mut SimulationToRenderTime, + interpolation_query: Option< + &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, + >, + ) { + self.simulation.step_simulation( + &mut self.colliders, + &mut self.joints, + &mut self.rigidbody_set, + gravity, + timestep_mode, + events, + hooks, + time, + sim_to_render_time, + interpolation_query, + ) + } + + /// Shortcut to [`RapierContextSimulation::move_shape`]. + #[expect(clippy::too_many_arguments)] + pub fn move_shape( + &mut self, + movement: Vect, + shape: &Collider, + shape_translation: Vect, + shape_rotation: Rot, + shape_mass: Real, + options: &MoveShapeOptions, + filter: QueryFilter, + events: impl FnMut(CharacterCollision), + ) -> MoveShapeOutput { + self.simulation.move_shape( + &self.colliders, + &self.query_pipeline, + &mut self.rigidbody_set, + movement, + shape, + shape_translation, + shape_rotation, + shape_mass, + options, + filter, + events, + ) + } + + /// Shortcut to [`RapierContextSimulation::contact_pair`]. + pub fn contact_pair( + &self, + collider1: Entity, + collider2: Entity, + ) -> Option { + self.simulation + .contact_pair(&self.colliders, &self.rigidbody_set, collider1, collider2) + } + + /// Shortcut to [`RapierContextSimulation::contact_pairs_with`]. + pub fn contact_pairs_with( + &self, + collider: Entity, + ) -> impl Iterator { + self.simulation + .contact_pairs_with(&self.colliders, &self.rigidbody_set, collider) + } + + /// Shortcut to [`RapierContextSimulation::intersection_pair`]. + pub fn intersection_pair(&self, collider1: Entity, collider2: Entity) -> Option { + self.simulation + .intersection_pair(&self.colliders, collider1, collider2) + } + + /// Shortcut to [`RapierContextSimulation::intersection_pairs_with`]. + pub fn intersection_pairs_with( + &self, + collider: Entity, + ) -> impl Iterator + '_ { + self.simulation + .intersection_pairs_with(&self.colliders, collider) + } } } -impl<'w, 's> Deref for RapierContextAccess<'w, 's> { - type Target = RapierContext; +mod query_pipeline { + use rapier::{parry::query::ShapeCastOptions, prelude::QueryFilter as RapierQueryFilter}; + + use crate::prelude::{Collider, PointProjection, RayIntersection, ShapeCastHit}; - fn deref(&self) -> &Self::Target { - self.rapier_context.single() + use super::*; + + impl<'a> RapierContext<'a> { + /// Shortcut to [`RapierQueryPipeline::cast_ray`]. + pub fn cast_ray( + &self, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, Real)> { + self.query_pipeline.cast_ray( + self.colliders, + self.rigidbody_set, + ray_origin, + ray_dir, + max_toi, + solid, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::cast_ray_and_get_normal`]. + pub fn cast_ray_and_get_normal( + &self, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, RayIntersection)> { + self.query_pipeline.cast_ray_and_get_normal( + self.colliders, + self.rigidbody_set, + ray_origin, + ray_dir, + max_toi, + solid, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::intersections_with_point`]. + pub fn intersections_with_point( + &self, + point: Vect, + filter: QueryFilter, + callback: impl FnMut(Entity) -> bool, + ) { + self.query_pipeline.intersections_with_point( + self.colliders, + self.rigidbody_set, + point, + filter, + callback, + ); + } + + /// Shortcut to [`RapierQueryPipeline::intersections_with_ray`]. + pub fn intersections_with_ray( + &self, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + callback: impl FnMut(Entity, RayIntersection) -> bool, + ) { + self.query_pipeline.intersections_with_ray( + self.colliders, + self.rigidbody_set, + ray_origin, + ray_dir, + max_toi, + solid, + filter, + callback, + ) + } + + /// Shortcut to [`RapierQueryPipeline::intersections_with_shape`]. + pub fn intersections_with_shape( + &self, + shape_pos: Vect, + shape_rot: Rot, + shape: &Collider, + filter: QueryFilter, + callback: impl FnMut(Entity) -> bool, + ) { + self.query_pipeline.intersections_with_shape( + self.colliders, + self.rigidbody_set, + shape_pos, + shape_rot, + shape, + filter, + callback, + ) + } + + /// Shortcut to [`RapierQueryPipeline::colliders_with_aabb_intersecting_aabb`]. + pub fn colliders_with_aabb_intersecting_aabb( + &self, + aabb: bevy::render::primitives::Aabb, + callback: impl FnMut(Entity) -> bool, + ) { + self.query_pipeline.colliders_with_aabb_intersecting_aabb( + self.colliders, + aabb, + callback, + ) + } + + /// Shortcut to [`RapierQueryPipeline::cast_shape`]. + pub fn cast_shape( + &self, + shape_pos: Vect, + shape_rot: Rot, + shape_vel: Vect, + shape: &Collider, + options: ShapeCastOptions, + filter: QueryFilter, + ) -> Option<(Entity, ShapeCastHit)> { + self.query_pipeline.cast_shape( + self.colliders, + self.rigidbody_set, + shape_pos, + shape_rot, + shape_vel, + shape, + options, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::project_point`]. + pub fn project_point( + &self, + point: Vect, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, PointProjection)> { + self.query_pipeline.project_point( + self.colliders, + self.rigidbody_set, + point, + solid, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::with_query_filter_elts`]. + pub fn with_query_filter_elts( + &self, + filter: crate::prelude::QueryFilter, + f: impl FnOnce(RapierQueryFilter) -> T, + ) -> T { + RapierQueryPipeline::with_query_filter_elts( + &self.colliders.entity2collider, + &self.rigidbody_set.entity2body, + &self.colliders.colliders, + filter, + f, + ) + } } -} -/// Utility [`SystemParam`] to easily access any [`RapierContext`] mutably -#[derive(SystemParam)] -pub struct WriteRapierContext<'w, 's> { - /// Query used to retrieve a [`RapierContext`]. - /// It's helpful to iterate over every rapier contexts, - /// or get a handle over a specific context, for example through: - /// - a marker component such as [`DefaultRapierContext`] - /// - a [`RapierContextEntityLink`]. See [context](RapierContextAccess::context) - pub rapier_context: Query<'w, 's, &'static mut RapierContext>, -} + impl<'a> RapierContextMut<'a> { + /// Shortcut to [`RapierQueryPipeline::cast_ray`]. + pub fn cast_ray( + &self, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, Real)> { + self.query_pipeline.cast_ray( + &self.colliders, + &self.rigidbody_set, + ray_origin, + ray_dir, + max_toi, + solid, + filter, + ) + } -impl<'w, 's> WriteRapierContext<'w, 's> { - /// Retrieves the rapier context responsible for the entity owning the given [`RapierContextEntityLink`]. - /// - /// SAFETY: This method will panic if its underlying query fails. - /// See [`Self::try_context`] for a safe alternative. - pub fn context(&mut self, link: &RapierContextEntityLink) -> Mut { - self.try_context(link) - .expect("RapierContextEntityLink.0 refers to an entity without RapierContext.") + /// Shortcut to [`RapierQueryPipeline::cast_ray_and_get_normal`]. + pub fn cast_ray_and_get_normal( + &self, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, RayIntersection)> { + self.query_pipeline.cast_ray_and_get_normal( + &self.colliders, + &self.rigidbody_set, + ray_origin, + ray_dir, + max_toi, + solid, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::intersections_with_point`]. + pub fn intersections_with_point( + &self, + point: Vect, + filter: QueryFilter, + callback: impl FnMut(Entity) -> bool, + ) { + self.query_pipeline.intersections_with_point( + &self.colliders, + &self.rigidbody_set, + point, + filter, + callback, + ); + } + + /// Shortcut to [`RapierQueryPipeline::intersections_with_ray`]. + pub fn intersections_with_ray( + &self, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + callback: impl FnMut(Entity, RayIntersection) -> bool, + ) { + self.query_pipeline.intersections_with_ray( + &self.colliders, + &self.rigidbody_set, + ray_origin, + ray_dir, + max_toi, + solid, + filter, + callback, + ) + } + + /// Shortcut to [`RapierQueryPipeline::intersections_with_shape`]. + pub fn intersections_with_shape( + &self, + shape_pos: Vect, + shape_rot: Rot, + shape: &Collider, + filter: QueryFilter, + callback: impl FnMut(Entity) -> bool, + ) { + self.query_pipeline.intersections_with_shape( + &self.colliders, + &self.rigidbody_set, + shape_pos, + shape_rot, + shape, + filter, + callback, + ) + } + + /// Shortcut to [`RapierQueryPipeline::colliders_with_aabb_intersecting_aabb`]. + pub fn colliders_with_aabb_intersecting_aabb( + &self, + aabb: bevy::render::primitives::Aabb, + callback: impl FnMut(Entity) -> bool, + ) { + self.query_pipeline.colliders_with_aabb_intersecting_aabb( + &self.colliders, + aabb, + callback, + ) + } + + /// Shortcut to [`RapierQueryPipeline::cast_shape`]. + pub fn cast_shape( + &self, + shape_pos: Vect, + shape_rot: Rot, + shape_vel: Vect, + shape: &Collider, + options: ShapeCastOptions, + filter: QueryFilter, + ) -> Option<(Entity, ShapeCastHit)> { + self.query_pipeline.cast_shape( + &self.colliders, + &self.rigidbody_set, + shape_pos, + shape_rot, + shape_vel, + shape, + options, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::project_point`]. + pub fn project_point( + &self, + point: Vect, + solid: bool, + filter: QueryFilter, + ) -> Option<(Entity, PointProjection)> { + self.query_pipeline.project_point( + &self.colliders, + &self.rigidbody_set, + point, + solid, + filter, + ) + } + + /// Shortcut to [`RapierQueryPipeline::with_query_filter_elts`]. + pub fn with_query_filter_elts( + &self, + filter: crate::prelude::QueryFilter, + f: impl FnOnce(RapierQueryFilter) -> T, + ) -> T { + RapierQueryPipeline::with_query_filter_elts( + &self.colliders.entity2collider, + &self.rigidbody_set.entity2body, + &self.colliders.colliders, + filter, + f, + ) + } } +} + +mod rigidbody_set { + use std::collections::HashMap; - /// Retrieves the rapier context responsible for the entity owning the given [`RapierContextEntityLink`]. - pub fn try_context(&mut self, link: &RapierContextEntityLink) -> Option> { - self.rapier_context.get_mut(link.0).ok() + use super::*; + pub use rapier::prelude::RigidBodyHandle; + + impl<'a> RapierContext<'a> { + /// Shortcut to [`RapierRigidBodySet::entity2body`]. + pub fn entity2body(&self) -> &HashMap { + self.rigidbody_set.entity2body() + } + + /// Shortcut to [`RapierRigidBodySet::rigid_body_entity`]. + pub fn rigid_body_entity(&self, handle: RigidBodyHandle) -> Option { + self.rigidbody_set.rigid_body_entity(handle) + } + + /// Shortcut to [`RapierRigidBodySet::impulse_revolute_joint_angle`]. + pub fn impulse_revolute_joint_angle(&self, entity: Entity) -> Option { + self.rigidbody_set + .impulse_revolute_joint_angle(self.joints, entity) + } } - /// Retrieves the rapier context component on this [`Entity`]. - /// - /// Calling this method on a rapier managed entity (rigid body, collider, joints...) will fail. - /// Given entity should have a [`RapierContext`]. - pub fn try_context_from_entity( - &mut self, - rapier_context_entity: Entity, - ) -> Option> { - self.rapier_context.get_mut(rapier_context_entity).ok() + impl<'a> RapierContextMut<'a> { + /// Shortcut to [`RapierRigidBodySet::propagate_modified_body_positions_to_colliders`]. + pub fn propagate_modified_body_positions_to_colliders(&mut self) { + self.rigidbody_set + .propagate_modified_body_positions_to_colliders(&mut self.colliders) + } + + /// Shortcut to [`RapierRigidBodySet::entity2body`]. + pub fn entity2body(&self) -> &HashMap { + self.rigidbody_set.entity2body() + } + + /// Shortcut to [`RapierRigidBodySet::rigid_body_entity`]. + pub fn rigid_body_entity(&self, handle: RigidBodyHandle) -> Option { + self.rigidbody_set.rigid_body_entity(handle) + } + + /// Shortcut to [`RapierRigidBodySet::impulse_revolute_joint_angle`]. + pub fn impulse_revolute_joint_angle(&self, entity: Entity) -> Option { + self.rigidbody_set + .impulse_revolute_joint_angle(&self.joints, entity) + } } } diff --git a/src/plugin/mod.rs b/src/plugin/mod.rs index 195bcc2b..a7e3e4a8 100644 --- a/src/plugin/mod.rs +++ b/src/plugin/mod.rs @@ -1,8 +1,4 @@ -pub use self::configuration::{RapierConfiguration, SimulationToRenderTime, TimestepMode}; -pub use self::context::{ - systemparams::{RapierContextAccess, ReadDefaultRapierContext, WriteRapierContext}, - DefaultRapierContext, RapierContext, RapierContextEntityLink, -}; +pub use self::configuration::{RapierConfiguration, TimestepMode}; pub use self::plugin::{ NoUserData, PhysicsSet, RapierContextInitialization, RapierPhysicsPlugin, RapierTransformPropagateSet, @@ -13,8 +9,8 @@ pub use narrow_phase::{ContactManifoldView, ContactPairView, ContactView, Solver #[allow(clippy::too_many_arguments)] pub mod systems; -mod configuration; -mod context; +pub mod configuration; +pub mod context; mod narrow_phase; #[allow(clippy::module_inception)] mod plugin; diff --git a/src/plugin/narrow_phase.rs b/src/plugin/narrow_phase.rs index 3aea4c19..cdd0729e 100644 --- a/src/plugin/narrow_phase.rs +++ b/src/plugin/narrow_phase.rs @@ -1,22 +1,32 @@ use crate::math::{Real, Vect}; -use crate::plugin::RapierContext; +use crate::plugin::context::{RapierContextColliders, RapierContextSimulation, RapierRigidBodySet}; use bevy::prelude::*; use rapier::geometry::{Contact, ContactManifold, ContactPair, SolverContact, SolverFlags}; -impl RapierContext { +impl RapierContextSimulation { /// All the contact pairs involving the non-sensor collider attached to the given entity. /// /// The returned contact pairs identify pairs of colliders with intersecting bounding-volumes. /// To check if any geometric contact happened between the collider shapes, check /// [`ContactPairView::has_any_active_contact`]. - pub fn contact_pairs_with(&self, collider: Entity) -> impl Iterator { - self.entity2collider + pub fn contact_pairs_with<'a, 'b: 'a>( + &'a self, + context_colliders: &'b RapierContextColliders, + rigidbody_set: &'b RapierRigidBodySet, + collider: Entity, + ) -> impl Iterator { + context_colliders + .entity2collider .get(&collider) .into_iter() .flat_map(|h| { self.narrow_phase .contact_pairs_with(*h) - .map(|raw| ContactPairView { context: self, raw }) + .map(|raw| ContactPairView { + context_colliders, + rigidbody_set, + raw, + }) }) } @@ -26,19 +36,21 @@ impl RapierContext { /// The returned contact pairs identify pairs of colliders (where at least one is a sensor) with /// intersecting bounding-volumes. To check if any geometric overlap happened between the collider shapes, check /// the returned boolean. - pub fn intersection_pairs_with( - &self, + pub fn intersection_pairs_with<'a, 'b: 'a>( + &'a self, + rapier_colliders: &'b RapierContextColliders, collider: Entity, ) -> impl Iterator + '_ { - self.entity2collider + rapier_colliders + .entity2collider .get(&collider) .into_iter() .flat_map(|h| { self.narrow_phase .intersection_pairs_with(*h) .filter_map(|(h1, h2, inter)| { - let e1 = self.collider_entity(h1); - let e2 = self.collider_entity(h2); + let e1 = rapier_colliders.collider_entity(h1); + let e2 = rapier_colliders.collider_entity(h2); match (e1, e2) { (Some(e1), Some(e2)) => Some((e1, e2, inter)), _ => None, @@ -52,38 +64,64 @@ impl RapierContext { /// If this returns `None`, there is no contact between the two colliders. /// If this returns `Some`, then there may be a contact between the two colliders. Check the /// result [`ContactPairView::has_any_active_contact`] method to see if there is an actual contact. - pub fn contact_pair(&self, collider1: Entity, collider2: Entity) -> Option { - let h1 = self.entity2collider.get(&collider1)?; - let h2 = self.entity2collider.get(&collider2)?; + pub fn contact_pair<'a, 'b: 'a>( + &'a self, + context_colliders: &'b RapierContextColliders, + rigidbody_set: &'b RapierRigidBodySet, + collider1: Entity, + collider2: Entity, + ) -> Option { + let h1 = context_colliders.entity2collider.get(&collider1)?; + let h2 = context_colliders.entity2collider.get(&collider2)?; self.narrow_phase .contact_pair(*h1, *h2) - .map(|raw| ContactPairView { context: self, raw }) + .map(|raw| ContactPairView { + context_colliders, + rigidbody_set, + raw, + }) } /// The intersection pair involving two specific colliders (at least one being a sensor). /// /// If this returns `None` or `Some(false)`, then there is no intersection between the two colliders. /// If this returns `Some(true)`, then there may be an intersection between the two colliders. - pub fn intersection_pair(&self, collider1: Entity, collider2: Entity) -> Option { - let h1 = self.entity2collider.get(&collider1)?; - let h2 = self.entity2collider.get(&collider2)?; + pub fn intersection_pair( + &self, + rapier_colliders: &RapierContextColliders, + collider1: Entity, + collider2: Entity, + ) -> Option { + let h1 = rapier_colliders.entity2collider.get(&collider1)?; + let h2 = rapier_colliders.entity2collider.get(&collider2)?; self.narrow_phase.intersection_pair(*h1, *h2) } /// All the contact pairs detected during the last timestep. - pub fn contact_pairs(&self) -> impl Iterator { + pub fn contact_pairs<'a, 'b: 'a>( + &'a self, + context_colliders: &'b RapierContextColliders, + rigidbody_set: &'b RapierRigidBodySet, + ) -> impl Iterator { self.narrow_phase .contact_pairs() - .map(|raw| ContactPairView { context: self, raw }) + .map(|raw| ContactPairView { + context_colliders, + rigidbody_set, + raw, + }) } /// All the intersection pairs detected during the last timestep. - pub fn intersection_pairs(&self) -> impl Iterator + '_ { + pub fn intersection_pairs<'a, 'b: 'a>( + &'a self, + rapier_colliders: &'b RapierContextColliders, + ) -> impl Iterator + '_ { self.narrow_phase .intersection_pairs() .filter_map(|(h1, h2, inter)| { - let e1 = self.collider_entity(h1); - let e2 = self.collider_entity(h2); + let e1 = rapier_colliders.collider_entity(h1); + let e2 = rapier_colliders.collider_entity(h2); match (e1, e2) { (Some(e1), Some(e2)) => Some((e1, e2, inter)), _ => None, @@ -94,7 +132,7 @@ impl RapierContext { /// Read-only access to the properties of a contact manifold. pub struct ContactManifoldView<'a> { - context: &'a RapierContext, + rigidbody_set: &'a RapierRigidBodySet, /// The raw contact manifold from Rapier. pub raw: &'a ContactManifold, } @@ -144,7 +182,7 @@ impl<'a> ContactManifoldView<'a> { self.raw .data .rigid_body1 - .and_then(|h| self.context.rigid_body_entity(h)) + .and_then(|h| self.rigidbody_set.rigid_body_entity(h)) } /// The second rigid-body involved in this contact manifold. @@ -152,7 +190,7 @@ impl<'a> ContactManifoldView<'a> { self.raw .data .rigid_body2 - .and_then(|h| self.context.rigid_body_entity(h)) + .and_then(|h| self.rigidbody_set.rigid_body_entity(h)) } /// Flags used to control some aspects of the constraints solver for this contact manifold. @@ -301,7 +339,8 @@ impl<'a> SolverContactView<'a> { /// Read-only access to the properties of a contact pair. pub struct ContactPairView<'a> { - context: &'a RapierContext, + context_colliders: &'a RapierContextColliders, + rigidbody_set: &'a RapierRigidBodySet, /// The raw contact pair from Rapier. pub raw: &'a ContactPair, } @@ -309,12 +348,16 @@ pub struct ContactPairView<'a> { impl<'a> ContactPairView<'a> { /// The first collider involved in this contact pair. pub fn collider1(&self) -> Entity { - self.context.collider_entity(self.raw.collider1).unwrap() + self.context_colliders + .collider_entity(self.raw.collider1) + .unwrap() } /// The second collider involved in this contact pair. pub fn collider2(&self) -> Entity { - self.context.collider_entity(self.raw.collider2).unwrap() + self.context_colliders + .collider_entity(self.raw.collider2) + .unwrap() } /// The number of contact manifolds detected for this contact pair. @@ -325,7 +368,7 @@ impl<'a> ContactPairView<'a> { /// Gets the i-th contact manifold. pub fn manifold(&self, i: usize) -> Option { self.raw.manifolds.get(i).map(|raw| ContactManifoldView { - context: self.context, + rigidbody_set: self.rigidbody_set, raw, }) } @@ -333,7 +376,7 @@ impl<'a> ContactPairView<'a> { /// Iterate through all the contact manifolds of this contact pair. pub fn manifolds(&self) -> impl ExactSizeIterator { self.raw.manifolds.iter().map(|raw| ContactManifoldView { - context: self.context, + rigidbody_set: self.rigidbody_set, raw, }) } @@ -354,7 +397,7 @@ impl<'a> ContactPairView<'a> { self.raw.find_deepest_contact().map(|(manifold, contact)| { ( ContactManifoldView { - context: self.context, + rigidbody_set: self.rigidbody_set, raw: manifold, }, ContactView { raw: contact }, diff --git a/src/plugin/plugin.rs b/src/plugin/plugin.rs index 8933a7c8..e7c621cf 100644 --- a/src/plugin/plugin.rs +++ b/src/plugin/plugin.rs @@ -1,6 +1,4 @@ use crate::pipeline::{CollisionEvent, ContactForceEvent}; -use crate::plugin::configuration::SimulationToRenderTime; -use crate::plugin::{systems, RapierConfiguration, RapierContext}; use crate::prelude::*; use bevy::ecs::{ intern::Interned, @@ -11,7 +9,10 @@ use bevy::{prelude::*, transform::TransformSystem}; use rapier::dynamics::IntegrationParameters; use std::marker::PhantomData; -use super::context::DefaultRapierContext; +use super::context::{DefaultRapierContext, RapierQueryPipeline, RapierRigidBodySet}; + +#[cfg(doc)] +use crate::plugin::context::systemparams::RapierContext; /// No specific user-data is associated to the hooks. pub type NoUserData = (); @@ -108,24 +109,33 @@ where .chain() .in_set(RapierTransformPropagateSet), ( - systems::on_add_entity_with_parent, - systems::on_change_world, - systems::sync_removals, - #[cfg(all(feature = "dim3", feature = "async-collider"))] - systems::init_async_scene_colliders, - #[cfg(all(feature = "dim3", feature = "async-collider"))] - systems::init_async_colliders, - systems::init_rigid_bodies, - systems::init_colliders, - systems::init_joints, - systems::apply_scale, - systems::apply_collider_user_changes, - systems::apply_rigid_body_user_changes, - systems::apply_joint_user_changes, - systems::apply_initial_rigid_body_impulses, + ( + systems::on_add_entity_with_parent, + systems::on_change_world, + systems::sync_removals, + #[cfg(all(feature = "dim3", feature = "async-collider"))] + systems::init_async_scene_colliders, + #[cfg(all(feature = "dim3", feature = "async-collider"))] + systems::init_async_colliders, + systems::init_rigid_bodies, + systems::init_colliders, + systems::init_joints, + ) + .chain() + .in_set(PhysicsSet::SyncBackend), + ( + ( + systems::apply_collider_user_changes.in_set(RapierBevyComponentApply), + systems::apply_scale.in_set(RapierBevyComponentApply), + ) + .chain(), + systems::apply_joint_user_changes.in_set(RapierBevyComponentApply), + ), + // TODO: joints and colliders might be parallelizable. + systems::apply_initial_rigid_body_impulses.in_set(RapierBevyComponentApply), + systems::apply_rigid_body_user_changes.in_set(RapierBevyComponentApply), ) - .chain() - .in_set(PhysicsSet::SyncBackend), + .chain(), ) .chain() .into_configs(), @@ -144,6 +154,10 @@ where } } +/// A set for rapier's copying bevy_rapier's Bevy components back into rapier. +#[derive(Debug, Hash, PartialEq, Eq, Clone, SystemSet)] +pub struct RapierBevyComponentApply; + /// A set for rapier's copy of Bevy's transform propagation systems. /// /// See [`TransformSystem`](bevy::transform::TransformSystem::TransformPropagate). @@ -256,6 +270,10 @@ where self.schedule, RapierTransformPropagateSet.in_set(PhysicsSet::SyncBackend), ); + app.configure_sets( + self.schedule, + RapierBevyComponentApply.in_set(PhysicsSet::SyncBackend), + ); app.add_systems( self.schedule, @@ -286,14 +304,14 @@ where /// Designed to be passed as parameter to [`RapierPhysicsPlugin::with_custom_initialization`]. #[derive(Resource, Debug, Reflect, Clone)] pub enum RapierContextInitialization { - /// [`RapierPhysicsPlugin`] will not spawn any entity containing [`RapierContext`] automatically. + /// [`RapierPhysicsPlugin`] will not spawn any entity containing [`RapierContextBundle`] automatically. /// - /// You are responsible for creating a [`RapierContext`], + /// You are responsible for creating a [`RapierContextBundle`], /// before spawning any rapier entities (rigidbodies, colliders, joints). /// /// You might be interested in adding [`DefaultRapierContext`] to the created world. NoAutomaticRapierContext, - /// [`RapierPhysicsPlugin`] will spawn an entity containing a [`RapierContext`] + /// [`RapierPhysicsPlugin`] will spawn an entity containing a [`RapierContextBundle`] /// automatically during [`PreStartup`], with the [`DefaultRapierContext`] marker component. InitializeDefaultRapierContext { /// See [`IntegrationParameters::length_unit`] @@ -316,12 +334,19 @@ pub fn insert_default_world( RapierContextInitialization::InitializeDefaultRapierContext { length_unit } => { commands.spawn(( Name::new("Rapier Context"), - RapierContext { - integration_parameters: IntegrationParameters { - length_unit: *length_unit, - ..default() + RapierContextBundle { + colliders: RapierContextColliders::default(), + joints: RapierContextJoints::default(), + query: RapierQueryPipeline::default(), + simulation: RapierContextSimulation { + integration_parameters: IntegrationParameters { + length_unit: *length_unit, + ..default() + }, + ..RapierContextSimulation::default() }, - ..RapierContext::default() + bodies: RapierRigidBodySet::default(), + simulation_to_render_time: SimulationToRenderTime::default(), }, DefaultRapierContext, )); @@ -331,7 +356,7 @@ pub fn insert_default_world( pub fn setup_rapier_configuration( mut commands: Commands, - rapier_context: Query<(Entity, &RapierContext), Without>, + rapier_context: Query<(Entity, &RapierContextSimulation), Without>, ) { for (e, rapier_context) in rapier_context.iter() { commands.entity(e).insert(RapierConfiguration::new( @@ -341,7 +366,13 @@ pub fn setup_rapier_configuration( } pub fn setup_rapier_simulation_to_render_time( mut commands: Commands, - rapier_context: Query, Without)>, + rapier_context: Query< + Entity, + ( + With, + Without, + ), + >, ) { for e in rapier_context.iter() { commands.entity(e).insert(SimulationToRenderTime::default()); @@ -359,7 +390,7 @@ mod test { use rapier::{data::Index, dynamics::RigidBodyHandle}; use systems::tests::HeadlessRenderPlugin; - use crate::{plugin::*, prelude::*}; + use crate::{plugin::context::*, plugin::*, prelude::*}; #[cfg(feature = "dim3")] fn cuboid(hx: Real, hy: Real, hz: Real) -> Collider { @@ -436,22 +467,23 @@ mod test { let mut stepping = world.resource_mut::(); stepping.continue_frame(); app.update(); - let context = app + + let rigidbody_set = app .world_mut() - .query::<&RapierContext>() + .query::<&RapierRigidBodySet>() .get_single(&app.world()) .unwrap(); - println!("{:?}", &context.entity2body); + println!("{:?}", &rigidbody_set.entity2body); } - let context = app + let rigidbody_set = app .world_mut() - .query::<&RapierContext>() + .query::<&RapierRigidBodySet>() .get_single(&app.world()) .unwrap(); assert_eq!( - context.entity2body.iter().next().unwrap().1, + rigidbody_set.entity2body.iter().next().unwrap().1, // assert the generation is 0, that means we didn't modify it twice (due to change world detection) &RigidBodyHandle(Index::from_raw_parts(0, 0)) ); diff --git a/src/plugin/systems/character_controller.rs b/src/plugin/systems/character_controller.rs index e349db78..28bdada1 100644 --- a/src/plugin/systems/character_controller.rs +++ b/src/plugin/systems/character_controller.rs @@ -1,9 +1,13 @@ use crate::control::CharacterCollision; use crate::dynamics::RapierRigidBodyHandle; use crate::geometry::RapierColliderHandle; +use crate::plugin::context::systemparams::RAPIER_CONTEXT_EXPECT_ERROR; use crate::plugin::context::RapierContextEntityLink; +use crate::plugin::context::RapierQueryPipeline; use crate::plugin::RapierConfiguration; -use crate::plugin::WriteRapierContext; +use crate::prelude::context::RapierContextColliders; +use crate::prelude::context::RapierContextSimulation; +use crate::prelude::context::RapierRigidBodySet; use crate::prelude::KinematicCharacterController; use crate::prelude::KinematicCharacterControllerOutput; use crate::utils; @@ -17,7 +21,12 @@ use rapier::pipeline::QueryFilter; pub fn update_character_controls( mut commands: Commands, config: Query<&RapierConfiguration>, - mut context_access: WriteRapierContext, + mut context_access: Query<( + &mut RapierContextSimulation, + &RapierContextColliders, + &RapierQueryPipeline, + &mut RapierRigidBodySet, + )>, mut character_controllers: Query<( Entity, &RapierContextEntityLink, @@ -45,7 +54,13 @@ pub fn update_character_controls( let config = config .get(rapier_context_link.0) .expect("Could not get [`RapierConfiguration`]"); - let context = context_access.context(rapier_context_link).into_inner(); + let (mut context, context_colliders, query_pipeline, mut rigidbody_set) = + context_access + .get_mut(rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + + let context = &mut *context; + let rigidbody_set = &mut *rigidbody_set; let scaled_custom_shape = controller .custom_shape @@ -60,11 +75,11 @@ pub fn update_character_controls( let parent_rigid_body = body_handle.map(|h| h.0).or_else(|| { collider_handle - .and_then(|h| context.colliders.get(h.0)) + .and_then(|h| context_colliders.colliders.get(h.0)) .and_then(|c| c.parent()) }); let entity_to_move = parent_rigid_body - .and_then(|rb| context.rigid_body_entity(rb)) + .and_then(|rb| rigidbody_set.rigid_body_entity(rb)) .unwrap_or(entity); let (character_shape, character_pos) = if let Some((scaled_shape, tra, rot)) = @@ -72,14 +87,15 @@ pub fn update_character_controls( { let mut shape_pos: Isometry = (*tra, *rot).into(); - if let Some(body) = body_handle.and_then(|h| context.bodies.get(h.0)) { + if let Some(body) = body_handle.and_then(|h| rigidbody_set.bodies.get(h.0)) { shape_pos = body.position() * shape_pos } else if let Some(gtransform) = glob_transform { shape_pos = utils::transform_to_iso(>ransform.compute_transform()) * shape_pos } (&*scaled_shape.raw, shape_pos) - } else if let Some(collider) = collider_handle.and_then(|h| context.colliders.get(h.0)) + } else if let Some(collider) = + collider_handle.and_then(|h| context_colliders.colliders.get(h.0)) { (collider.shape(), *collider.position()) } else { @@ -92,7 +108,7 @@ pub fn update_character_controls( .custom_mass .or_else(|| { parent_rigid_body - .and_then(|h| context.bodies.get(h)) + .and_then(|h| rigidbody_set.bodies.get(h)) .map(|rb| rb.mass()) }) .unwrap_or(0.0); @@ -116,9 +132,9 @@ pub fn update_character_controls( let movement = raw_controller.move_shape( context.integration_parameters.dt, - &context.bodies, - &context.colliders, - &context.query_pipeline, + &rigidbody_set.bodies, + &context_colliders.colliders, + &query_pipeline.query_pipeline, character_shape, &character_pos, translation.into(), @@ -129,9 +145,9 @@ pub fn update_character_controls( if controller.apply_impulse_to_dynamic_bodies { raw_controller.solve_character_collision_impulses( context.integration_parameters.dt, - &mut context.bodies, - &context.colliders, - &context.query_pipeline, + &mut rigidbody_set.bodies, + &context_colliders.colliders, + &query_pipeline.query_pipeline, character_shape, character_mass, collisions.iter(), @@ -152,7 +168,7 @@ pub fn update_character_controls( let converted_collisions = context .character_collisions_collector .iter() - .filter_map(|c| CharacterCollision::from_raw(context, c)); + .filter_map(|c| CharacterCollision::from_raw(context_colliders, c)); if let Some(mut output) = output { output.desired_translation = controller.translation.unwrap(); diff --git a/src/plugin/systems/collider.rs b/src/plugin/systems/collider.rs index 7d094f0d..7c3d672d 100644 --- a/src/plugin/systems/collider.rs +++ b/src/plugin/systems/collider.rs @@ -1,8 +1,11 @@ use crate::dynamics::ReadMassProperties; use crate::geometry::Collider; -use crate::plugin::context::systemparams::RapierEntity; +use crate::plugin::context::systemparams::{RapierEntity, RAPIER_CONTEXT_EXPECT_ERROR}; use crate::plugin::context::RapierContextEntityLink; -use crate::plugin::{DefaultRapierContext, RapierConfiguration, RapierContext, WriteRapierContext}; +use crate::plugin::{ + context::{DefaultRapierContext, RapierContextColliders, RapierRigidBodySet}, + RapierConfiguration, +}; use crate::prelude::{ ActiveCollisionTypes, ActiveEvents, ActiveHooks, ColliderDisabled, ColliderMassProperties, ColliderScale, CollidingEntities, CollisionEvent, CollisionGroups, ContactForceEventThreshold, @@ -83,7 +86,7 @@ pub fn apply_scale( /// System responsible for applying changes the user made to a collider-related component. pub fn apply_collider_user_changes( - mut context: WriteRapierContext, + mut context: Query<(&RapierRigidBodySet, &mut RapierContextColliders)>, config: Query<&RapierConfiguration>, (changed_collider_transforms, parent_query, transform_query): ( Query< @@ -141,37 +144,40 @@ pub fn apply_collider_user_changes( mut mass_modified: EventWriter, ) { for (rapier_entity, handle, transform) in changed_collider_transforms.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if context.collider_parent(rapier_entity.entity).is_some() { + let (rigidbody_set, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if context_colliders + .collider_parent(rigidbody_set, rapier_entity.entity) + .is_some() + { let (_, collider_position) = collider_offset( rapier_entity.entity, - context, + rigidbody_set, &parent_query, &transform_query, ); - if let Some(co) = context.colliders.get_mut(handle.0) { + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_position_wrt_parent(utils::transform_to_iso(&collider_position)); } - } else if let Some(co) = context.colliders.get_mut(handle.0) { + } else if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_position(utils::transform_to_iso(&transform.compute_transform())) } } for (rapier_entity, handle, shape) in changed_shapes.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); + let (rigidbody_set, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); let config = config.get(rapier_entity.rapier_context_link.0).unwrap(); - if let Some(co) = context.colliders.get_mut(handle.0) { + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { let mut scaled_shape = shape.clone(); scaled_shape.set_scale(shape.scale, config.scaled_shape_subdivision); co.set_shape(scaled_shape.raw.clone()); if let Some(body) = co.parent() { - if let Some(body_entity) = context.rigid_body_entity(body) { + if let Some(body_entity) = rigidbody_set.rigid_body_entity(body) { mass_modified.send(body_entity.into()); } } @@ -179,111 +185,111 @@ pub fn apply_collider_user_changes( } for (rapier_entity, handle, active_events) in changed_active_events.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_active_events((*active_events).into()) } } for (rapier_entity, handle, active_hooks) in changed_active_hooks.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_active_hooks((*active_hooks).into()) } } for (rapier_entity, handle, active_collision_types) in changed_active_collision_types.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_active_collision_types((*active_collision_types).into()) } } for (rapier_entity, handle, friction) in changed_friction.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_friction(friction.coefficient); co.set_friction_combine_rule(friction.combine_rule.into()); } } for (rapier_entity, handle, restitution) in changed_restitution.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_restitution(restitution.coefficient); co.set_restitution_combine_rule(restitution.combine_rule.into()); } } for (rapier_entity, handle, contact_skin) in changed_contact_skin.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_contact_skin(contact_skin.0); } } for (rapier_entity, handle, collision_groups) in changed_collision_groups.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_collision_groups((*collision_groups).into()); } } for (rapier_entity, handle, solver_groups) in changed_solver_groups.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_solver_groups((*solver_groups).into()); } } for (rapier_entity, handle, _) in changed_sensors.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_sensor(true); } } for (rapier_entity, handle, _) in changed_disabled.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_enabled(false); } } for (rapier_entity, handle, threshold) in changed_contact_force_threshold.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (_, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { co.set_contact_force_event_threshold(threshold.0); } } for (rapier_entity, handle, mprops) in changed_collider_mass_props.iter() { - let context = context - .context(rapier_entity.rapier_context_link) - .into_inner(); - if let Some(co) = context.colliders.get_mut(handle.0) { + let (rigidbody_set, mut context_colliders) = context + .get_mut(rapier_entity.rapier_context_link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR); + if let Some(co) = context_colliders.colliders.get_mut(handle.0) { match mprops { ColliderMassProperties::Density(density) => co.set_density(*density), ColliderMassProperties::Mass(mass) => co.set_mass(*mass), @@ -293,7 +299,7 @@ pub fn apply_collider_user_changes( } if let Some(body) = co.parent() { - if let Some(body_entity) = context.rigid_body_entity(body) { + if let Some(body_entity) = rigidbody_set.rigid_body_entity(body) { mass_modified.send(body_entity.into()); } } @@ -303,12 +309,12 @@ pub fn apply_collider_user_changes( pub(crate) fn collider_offset( entity: Entity, - context: &RapierContext, + rigidbody_set: &RapierRigidBodySet, parent_query: &Query<&Parent>, transform_query: &Query<&Transform>, ) -> (Option, Transform) { let mut body_entity = entity; - let mut body_handle = context.entity2body.get(&body_entity).copied(); + let mut body_handle = rigidbody_set.entity2body.get(&body_entity).copied(); let mut child_transform = Transform::default(); while body_handle.is_none() { if let Ok(parent_entity) = parent_query.get(body_entity) { @@ -320,7 +326,7 @@ pub(crate) fn collider_offset( break; } - body_handle = context.entity2body.get(&body_entity).copied(); + body_handle = rigidbody_set.entity2body.get(&body_entity).copied(); } if body_handle.is_some() { @@ -341,7 +347,7 @@ pub(crate) fn collider_offset( pub fn init_colliders( mut commands: Commands, config: Query<&RapierConfiguration>, - mut context_access: WriteRapierContext, + mut context_access: Query<(&mut RapierRigidBodySet, &mut RapierContextColliders)>, default_context_access: Query>, colliders: Query<(ColliderComponents, Option<&GlobalTransform>), Without>, mut rigid_body_mprops: Query<&mut ReadMassProperties>, @@ -386,11 +392,14 @@ pub fn init_colliders( let config = config.get(context_entity).unwrap_or_else(|_| { panic!("Failed to retrieve `RapierConfiguration` on entity {context_entity}.") }); - let Some(context) = context_access.try_context_from_entity(context_entity) else { + + let Some(mut rigidbody_set_collider_set) = context_access.get_mut(context_entity).ok() + else { log::error!("Could not find entity {context_entity} with rapier context while initializing {entity}"); continue; }; - let context = context.into_inner(); + let context_colliders = &mut *rigidbody_set_collider_set.1; + let rigidbody_set = &mut *rigidbody_set_collider_set.0; let mut scaled_shape = shape.clone(); scaled_shape.set_scale(shape.scale, config.scaled_shape_subdivision); let mut builder = ColliderBuilder::new(scaled_shape.raw.clone()); @@ -449,20 +458,21 @@ pub fn init_colliders( } let body_entity = entity; let (body_handle, child_transform) = - collider_offset(entity, context, &parent_query, &transform_query); + collider_offset(entity, rigidbody_set, &parent_query, &transform_query); builder = builder.user_data(entity.to_bits() as u128); let handle = if let Some(body_handle) = body_handle { builder = builder.position(utils::transform_to_iso(&child_transform)); - let handle = - context - .colliders - .insert_with_parent(builder, body_handle, &mut context.bodies); + let handle = context_colliders.colliders.insert_with_parent( + builder, + body_handle, + &mut rigidbody_set.bodies, + ); if let Ok(mut mprops) = rigid_body_mprops.get_mut(body_entity) { // Inserting the collider changed the rigid-body’s mass properties. // Read them back from the engine. - if let Some(parent_body) = context.bodies.get(body_handle) { + if let Some(parent_body) = rigidbody_set.bodies.get(body_handle) { mprops.set(MassProperties::from_rapier( parent_body.mass_properties().local_mprops, )); @@ -474,11 +484,11 @@ pub fn init_colliders( builder = builder.position(utils::transform_to_iso( &global_transform.compute_transform(), )); - context.colliders.insert(builder) + context_colliders.colliders.insert(builder) }; commands.entity(entity).insert(RapierColliderHandle(handle)); - context.entity2collider.insert(entity, handle); + context_colliders.entity2collider.insert(entity, handle); } } diff --git a/src/plugin/systems/joint.rs b/src/plugin/systems/joint.rs index f162a0ef..c62b7774 100644 --- a/src/plugin/systems/joint.rs +++ b/src/plugin/systems/joint.rs @@ -2,15 +2,17 @@ use crate::dynamics::ImpulseJoint; use crate::dynamics::MultibodyJoint; use crate::dynamics::RapierImpulseJointHandle; use crate::dynamics::RapierMultibodyJointHandle; +use crate::plugin::context::systemparams::RAPIER_CONTEXT_EXPECT_ERROR; +use crate::plugin::context::DefaultRapierContext; use crate::plugin::context::RapierContextEntityLink; -use crate::plugin::DefaultRapierContext; -use crate::plugin::WriteRapierContext; +use crate::plugin::context::RapierContextJoints; +use crate::plugin::context::RapierRigidBodySet; use bevy::prelude::*; /// System responsible for creating new Rapier joints from the related `bevy_rapier` components. pub fn init_joints( mut commands: Commands, - mut context_access: WriteRapierContext, + mut context_access: Query<(&RapierRigidBodySet, &mut RapierContextJoints)>, default_context_access: Query>, impulse_joints: Query< (Entity, Option<&RapierContextEntityLink>, &ImpulseJoint), @@ -38,24 +40,26 @@ pub fn init_joints( continue; }; - let Some(context) = context_access.try_context_from_entity(context_entity) else { + let Ok(rigidbody_set_joints) = context_access.get_mut(context_entity) else { log::error!("Could not find entity {context_entity} with rapier context while initializing {entity}"); continue; }; - let context = context.into_inner(); + let rigidbody_set = rigidbody_set_joints.0; let mut target = None; let mut body_entity = entity; while target.is_none() { - target = context.entity2body.get(&body_entity).copied(); + target = rigidbody_set.entity2body.get(&body_entity).copied(); if let Ok(parent_entity) = parent_query.get(body_entity) { body_entity = parent_entity.get(); } else { break; } } + let joints = rigidbody_set_joints.1.into_inner(); - if let (Some(target), Some(source)) = (target, context.entity2body.get(&joint.parent)) { - let handle = context.impulse_joints.insert( + if let (Some(target), Some(source)) = (target, rigidbody_set.entity2body.get(&joint.parent)) + { + let handle = joints.impulse_joints.insert( *source, target, joint.data.as_ref().into_rapier(), @@ -64,7 +68,7 @@ pub fn init_joints( commands .entity(entity) .insert(RapierImpulseJointHandle(handle)); - context.entity2impulse_joint.insert(entity, handle); + joints.entity2impulse_joint.insert(entity, handle); } } @@ -84,15 +88,16 @@ pub fn init_joints( continue; }; - let Some(context) = context_access.try_context_from_entity(context_entity) else { + let Ok(context_joints) = context_access.get_mut(context_entity) else { log::error!("Could not find entity {context_entity} with rapier context while initializing {entity}"); continue; }; - let context = context.into_inner(); + let context = context_joints.0; let target = context.entity2body.get(&entity); + let joints = context_joints.1.into_inner(); if let (Some(target), Some(source)) = (target, context.entity2body.get(&joint.parent)) { - if let Some(handle) = context.multibody_joints.insert( + if let Some(handle) = joints.multibody_joints.insert( *source, *target, joint.data.as_ref().into_rapier(), @@ -101,7 +106,7 @@ pub fn init_joints( commands .entity(entity) .insert(RapierMultibodyJointHandle(handle)); - context.entity2multibody_joint.insert(entity, handle); + joints.entity2multibody_joint.insert(entity, handle); } else { error!("Failed to create multibody joint: loop detected.") } @@ -111,7 +116,7 @@ pub fn init_joints( /// System responsible for applying changes the user made to a joint component. pub fn apply_joint_user_changes( - mut context: WriteRapierContext, + mut context: Query<&mut RapierContextJoints>, changed_impulse_joints: Query< ( &RapierContextEntityLink, @@ -132,14 +137,14 @@ pub fn apply_joint_user_changes( // TODO: right now, we only support propagating changes made to the joint data. // Re-parenting the joint isn’t supported yet. for (link, handle, changed_joint) in changed_impulse_joints.iter() { - let context = context.context(link).into_inner(); + let mut context = context.get_mut(link.0).expect(RAPIER_CONTEXT_EXPECT_ERROR); if let Some(joint) = context.impulse_joints.get_mut(handle.0) { joint.data = changed_joint.data.as_ref().into_rapier(); } } for (link, handle, changed_joint) in changed_multibody_joints.iter() { - let context = context.context(link).into_inner(); + let mut context = context.get_mut(link.0).expect(RAPIER_CONTEXT_EXPECT_ERROR); // TODO: not sure this will always work properly, e.g., if the number of Dofs is changed. if let Some((mb, link_id)) = context.multibody_joints.get_mut(handle.0) { if let Some(link) = mb.link_mut(link_id) { diff --git a/src/plugin/systems/mod.rs b/src/plugin/systems/mod.rs index b1cc80f6..c21e5eda 100644 --- a/src/plugin/systems/mod.rs +++ b/src/plugin/systems/mod.rs @@ -18,17 +18,26 @@ pub use writeback::*; use crate::dynamics::{RapierRigidBodyHandle, TransformInterpolation}; use crate::pipeline::{CollisionEvent, ContactForceEvent}; -use crate::plugin::configuration::SimulationToRenderTime; -use crate::plugin::{RapierConfiguration, RapierContext, TimestepMode}; +use crate::plugin::context::SimulationToRenderTime; +use crate::plugin::{RapierConfiguration, TimestepMode}; use crate::prelude::{BevyPhysicsHooks, BevyPhysicsHooksAdapter}; use bevy::ecs::system::{StaticSystemParam, SystemParamItem}; use bevy::prelude::*; +use super::context::{ + RapierContextColliders, RapierContextJoints, RapierContextSimulation, RapierQueryPipeline, + RapierRigidBodySet, +}; + /// System responsible for advancing the physics simulation, and updating the internal state /// for scene queries. pub fn step_simulation( mut context: Query<( - &mut RapierContext, + &mut RapierContextSimulation, + &mut RapierContextColliders, + &mut RapierQueryPipeline, + &mut RapierContextJoints, + &mut RapierRigidBodySet, &RapierConfiguration, &mut SimulationToRenderTime, )>, @@ -44,11 +53,24 @@ pub fn step_simulation( { let hooks_adapter = BevyPhysicsHooksAdapter::new(hooks.into_inner()); - for (mut context, config, mut sim_to_render_time) in context.iter_mut() { + for ( + mut context, + mut context_colliders, + mut query_pipeline, + mut joints, + mut rigidbody_set, + config, + mut sim_to_render_time, + ) in context.iter_mut() + { let context = &mut *context; + let context_colliders = &mut *context_colliders; if config.physics_pipeline_active { context.step_simulation( + context_colliders, + &mut joints, + &mut rigidbody_set, config.gravity, *timestep_mode, Some((&collision_events, &contact_force_events)), @@ -58,11 +80,11 @@ pub fn step_simulation( Some(&mut interpolation_query), ); } else { - context.propagate_modified_body_positions_to_colliders(); + rigidbody_set.propagate_modified_body_positions_to_colliders(context_colliders); } if config.query_pipeline_active { - context.update_query_pipeline(); + query_pipeline.update_query_pipeline(context_colliders); } context.send_bevy_events(&mut collision_events, &mut contact_force_events); } @@ -221,10 +243,14 @@ pub mod tests { app.update(); let world = app.world_mut(); - let context = world.query::<&RapierContext>().iter(&world).next().unwrap(); + let rigidbody_set = world + .query::<&RapierRigidBodySet>() + .iter(&world) + .next() + .unwrap(); let child_transform = world.entity(child).get::().unwrap(); - let child_handle = context.entity2body[&child]; - let child_body = context.bodies.get(child_handle).unwrap(); + let child_handle = rigidbody_set.entity2body[&child]; + let child_body = rigidbody_set.bodies.get(child_handle).unwrap(); let body_transform = utils::iso_to_transform(child_body.position()); assert_eq!( GlobalTransform::from(body_transform), @@ -285,11 +311,18 @@ pub mod tests { .unwrap() .compute_transform(); let world = app.world_mut(); - let context = world.query::<&RapierContext>().iter(&world).next().unwrap(); - let parent_handle = context.entity2body[&parent]; - let parent_body = context.bodies.get(parent_handle).unwrap(); + let (rigidbody_set, context_colliders) = world + .query::<(&RapierRigidBodySet, &RapierContextColliders)>() + .iter(&world) + .next() + .unwrap(); + let parent_handle = rigidbody_set.entity2body[&parent]; + let parent_body = rigidbody_set.bodies.get(parent_handle).unwrap(); let child_collider_handle = parent_body.colliders()[0]; - let child_collider = context.colliders.get(child_collider_handle).unwrap(); + let child_collider = context_colliders + .colliders + .get(child_collider_handle) + .unwrap(); let body_transform = utils::iso_to_transform(child_collider.position()); approx::assert_relative_eq!( body_transform.translation, diff --git a/src/plugin/systems/multiple_rapier_contexts.rs b/src/plugin/systems/multiple_rapier_contexts.rs index ec3dbb3a..dbdfb1c5 100644 --- a/src/plugin/systems/multiple_rapier_contexts.rs +++ b/src/plugin/systems/multiple_rapier_contexts.rs @@ -4,7 +4,10 @@ use crate::dynamics::{ RapierImpulseJointHandle, RapierMultibodyJointHandle, RapierRigidBodyHandle, }; use crate::geometry::RapierColliderHandle; -use crate::plugin::{RapierContext, RapierContextEntityLink}; +use crate::plugin::context::RapierRigidBodySet; +use crate::plugin::context::{ + RapierContextColliders, RapierContextEntityLink, RapierContextJoints, +}; use bevy::prelude::*; /// If an entity is turned into the child of something with a physics context link, @@ -59,19 +62,23 @@ pub fn on_change_world( >, q_children: Query<&Children>, q_physics_world: Query<&RapierContextEntityLink>, - q_context: Query<&RapierContext>, + q_context: Query<( + &RapierContextColliders, + &RapierContextJoints, + &RapierRigidBodySet, + )>, mut commands: Commands, ) { for (entity, new_physics_world) in &q_changed_worlds { let context = q_context.get(new_physics_world.0); // Ensure the world actually changed before removing them from the world if !context - .map(|x| { + .map(|(colliders, joints, rigidbody_set)| { // They are already apart of this world if any of these are true - x.entity2collider.contains_key(&entity) - || x.entity2body.contains_key(&entity) - || x.entity2impulse_joint.contains_key(&entity) - || x.entity2multibody_joint.contains_key(&entity) + colliders.entity2collider.contains_key(&entity) + || rigidbody_set.entity2body.contains_key(&entity) + || joints.entity2impulse_joint.contains_key(&entity) + || joints.entity2multibody_joint.contains_key(&entity) }) .unwrap_or(false) { @@ -122,9 +129,14 @@ fn bubble_down_world_change( #[cfg(test)] mod test { + use crate::plugin::context::RapierQueryPipeline; use crate::plugin::systems::tests::HeadlessRenderPlugin; use crate::plugin::{ - NoUserData, PhysicsSet, RapierContext, RapierContextEntityLink, RapierPhysicsPlugin, + context::{ + RapierContextColliders, RapierContextEntityLink, RapierContextJoints, + RapierContextSimulation, RapierRigidBodySet, + }, + NoUserData, PhysicsSet, RapierPhysicsPlugin, }; use crate::prelude::{ActiveEvents, Collider, ContactForceEventThreshold, RigidBody, Sensor}; use bevy::prelude::*; @@ -160,7 +172,15 @@ mod test { .unwrap_or_else(|| panic!("no link to rapier context entity from {entity}.")); } // Verify link is correctly updated for children. - let new_rapier_context = world.spawn(RapierContext::default()).id(); + let new_rapier_context = world + .spawn(( + RapierContextSimulation::default(), + RapierContextColliders::default(), + RapierContextJoints::default(), + RapierQueryPipeline::default(), + RapierRigidBodySet::default(), + )) + .id(); // FIXME: We need to wait 1 frame when creating a world. // Ideally we should be able to order the systems so that we don't have to wait. app.update(); diff --git a/src/plugin/systems/remove.rs b/src/plugin/systems/remove.rs index 0342a1b9..325170b5 100644 --- a/src/plugin/systems/remove.rs +++ b/src/plugin/systems/remove.rs @@ -7,11 +7,13 @@ use crate::dynamics::RigidBody; use crate::geometry::Collider; use crate::geometry::ColliderDisabled; use crate::geometry::RapierColliderHandle; -use crate::plugin::context::systemparams::WriteRapierContext; -use crate::plugin::RapierContext; +use crate::plugin::context::{ + RapierContextColliders, RapierContextJoints, RapierContextSimulation, RapierRigidBodySet, +}; use crate::prelude::MassModifiedEvent; use crate::prelude::RigidBodyDisabled; use crate::prelude::Sensor; +use bevy::ecs::query::QueryData; use bevy::prelude::*; /// System responsible for removing from Rapier the rigid-bodies/colliders/joints which had @@ -19,7 +21,12 @@ use bevy::prelude::*; /// despawn). pub fn sync_removals( mut commands: Commands, - mut context_writer: WriteRapierContext, + mut context_writer: Query<( + &mut RapierContextSimulation, + &mut RapierContextColliders, + &mut RapierContextJoints, + &mut RapierRigidBodySet, + )>, mut removed_bodies: RemovedComponents, mut removed_colliders: RemovedComponents, mut removed_impulse_joints: RemovedComponents, @@ -42,36 +49,38 @@ pub fn sync_removals( * Rigid-bodies removal detection. */ for entity in removed_bodies.read() { - let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2body.remove(&entity) - }) else { + let Some(((mut context, mut context_colliders, mut joints, mut rigidbody_set), handle)) = + find_context(&mut context_writer, |res| res.3.entity2body.remove(&entity)) + else { continue; }; let context = &mut *context; + let joints = &mut *joints; - let _ = context.last_body_transform_set.remove(&handle); - context.bodies.remove( + let _ = rigidbody_set.last_body_transform_set.remove(&handle); + rigidbody_set.bodies.remove( handle, &mut context.islands, - &mut context.colliders, - &mut context.impulse_joints, - &mut context.multibody_joints, + &mut context_colliders.colliders, + &mut joints.impulse_joints, + &mut joints.multibody_joints, false, ); } for entity in orphan_bodies.iter() { - if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2body.remove(&entity) - }) { + if let Some(((mut context, mut context_colliders, mut joints, mut rigidbody_set), handle)) = + find_context(&mut context_writer, |res| res.3.entity2body.remove(&entity)) + { let context = &mut *context; - let _ = context.last_body_transform_set.remove(&handle); - context.bodies.remove( + let joints = &mut *joints; + let _ = rigidbody_set.last_body_transform_set.remove(&handle); + rigidbody_set.bodies.remove( handle, &mut context.islands, - &mut context.colliders, - &mut context.impulse_joints, - &mut context.multibody_joints, + &mut context_colliders.colliders, + &mut joints.impulse_joints, + &mut joints.multibody_joints, false, ); } @@ -82,34 +91,45 @@ pub fn sync_removals( * Collider removal detection. */ for entity in removed_colliders.read() { - let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2collider.remove(&entity) - }) else { + let Some(((mut context, mut context_colliders, _, mut rigidbody_set), handle)) = + find_context(&mut context_writer, |res| { + res.1.entity2collider.remove(&entity) + }) + else { continue; }; let context = &mut *context; - if let Some(parent) = context.collider_parent(entity) { + if let Some(parent) = context_colliders.collider_parent(&rigidbody_set, entity) { mass_modified.send(parent.into()); } - context - .colliders - .remove(handle, &mut context.islands, &mut context.bodies, true); + context_colliders.colliders.remove( + handle, + &mut context.islands, + &mut rigidbody_set.bodies, + true, + ); context.deleted_colliders.insert(handle, entity); } for entity in orphan_colliders.iter() { - if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2collider.remove(&entity) - }) { + if let Some(((mut context, mut context_colliders, _, mut rigidbody_set), handle)) = + find_context(&mut context_writer, |res| { + res.1.entity2collider.remove(&entity) + }) + { let context = &mut *context; - if let Some(parent) = context.collider_parent(entity) { + let context_colliders = &mut *context_colliders; + if let Some(parent) = context_colliders.collider_parent(&rigidbody_set, entity) { mass_modified.send(parent.into()); } - context - .colliders - .remove(handle, &mut context.islands, &mut context.bodies, true); + context_colliders.colliders.remove( + handle, + &mut context.islands, + &mut rigidbody_set.bodies, + true, + ); context.deleted_colliders.insert(handle, entity); } commands.entity(entity).remove::(); @@ -119,21 +139,19 @@ pub fn sync_removals( * Impulse joint removal detection. */ for entity in removed_impulse_joints.read() { - let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2impulse_joint.remove(&entity) + let Some(((_, _, mut joints, _), handle)) = find_context(&mut context_writer, |res| { + res.2.entity2impulse_joint.remove(&entity) }) else { continue; }; - let context = &mut *context; - context.impulse_joints.remove(handle, true); + joints.impulse_joints.remove(handle, true); } for entity in orphan_impulse_joints.iter() { - if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2impulse_joint.remove(&entity) + if let Some(((_, _, mut joints, _), handle)) = find_context(&mut context_writer, |res| { + res.2.entity2impulse_joint.remove(&entity) }) { - let context = &mut *context; - context.impulse_joints.remove(handle, true); + joints.impulse_joints.remove(handle, true); } commands.entity(entity).remove::(); } @@ -142,21 +160,19 @@ pub fn sync_removals( * Multibody joint removal detection. */ for entity in removed_multibody_joints.read() { - let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2multibody_joint.remove(&entity) + let Some(((_, _, mut joints, _), handle)) = find_context(&mut context_writer, |res| { + res.2.entity2multibody_joint.remove(&entity) }) else { continue; }; - let context = &mut *context; - context.multibody_joints.remove(handle, true); + joints.multibody_joints.remove(handle, true); } for entity in orphan_multibody_joints.iter() { - if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2multibody_joint.remove(&entity) + if let Some(((_, _, mut joints, _), handle)) = find_context(&mut context_writer, |res| { + res.2.entity2multibody_joint.remove(&entity) }) { - let context = &mut *context; - context.multibody_joints.remove(handle, true); + joints.multibody_joints.remove(handle, true); } commands .entity(entity) @@ -168,9 +184,9 @@ pub fn sync_removals( */ for entity in removed_sensors.read() { if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2collider.get(&entity).copied() + context.1.entity2collider.get(&entity).copied() }) { - if let Some(co) = context.colliders.get_mut(handle) { + if let Some(co) = context.1.colliders.get_mut(handle) { co.set_sensor(false); } } @@ -178,19 +194,21 @@ pub fn sync_removals( for entity in removed_colliders_disabled.read() { if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2collider.get(&entity).copied() + context.1.entity2collider.get(&entity).copied() }) { - if let Some(co) = context.colliders.get_mut(handle) { + if let Some(co) = context.1.colliders.get_mut(handle) { co.set_enabled(true); } } } for entity in removed_rigid_body_disabled.read() { - if let Some((mut context, handle)) = find_context(&mut context_writer, |context| { - context.entity2body.get(&entity).copied() - }) { - if let Some(rb) = context.bodies.get_mut(handle) { + if let Some(((_, _, _, mut rigidbody_set), handle)) = + find_context(&mut context_writer, |res| { + res.3.entity2body.get(&entity).copied() + }) + { + if let Some(rb) = rigidbody_set.bodies.get_mut(handle) { rb.set_enabled(true); } } @@ -199,12 +217,12 @@ pub fn sync_removals( // TODO: what about removing forces? } -fn find_context<'a, T>( - context_writer: &'a mut WriteRapierContext, - item_finder: impl Fn(&mut RapierContext) -> Option, -) -> Option<(Mut<'a, RapierContext>, T)> { - context_writer - .rapier_context +fn find_context<'a, TReturn, TQueryParams: QueryData>( + context_writer: &'a mut Query, + item_finder: impl Fn(&mut TQueryParams::Item<'_>) -> Option, +) -> Option<(TQueryParams::Item<'a>, TReturn)> { + let ret: Option<(TQueryParams::Item<'_>, TReturn)> = context_writer .iter_mut() - .find_map(|mut context| item_finder(&mut context).map(|handle| (context, handle))) + .find_map(|mut context| item_finder(&mut context).map(|handle| (context, handle))); + ret } diff --git a/src/plugin/systems/rigid_body.rs b/src/plugin/systems/rigid_body.rs index 67a91886..885f2f67 100644 --- a/src/plugin/systems/rigid_body.rs +++ b/src/plugin/systems/rigid_body.rs @@ -1,7 +1,10 @@ use crate::dynamics::RapierRigidBodyHandle; -use crate::plugin::context::RapierContextEntityLink; -use crate::plugin::{configuration::TimestepMode, RapierConfiguration, RapierContext}; -use crate::{dynamics::RigidBody, plugin::configuration::SimulationToRenderTime}; +use crate::plugin::context::systemparams::RAPIER_CONTEXT_EXPECT_ERROR; +use crate::plugin::context::{ + DefaultRapierContext, RapierContextColliders, RapierContextEntityLink, RapierRigidBodySet, +}; +use crate::plugin::{configuration::TimestepMode, RapierConfiguration}; +use crate::{dynamics::RigidBody, plugin::context::SimulationToRenderTime}; use crate::{prelude::*, utils}; use bevy::prelude::*; use rapier::dynamics::{RigidBodyBuilder, RigidBodyHandle, RigidBodyType}; @@ -39,7 +42,7 @@ pub type RigidBodyComponents<'a> = ( /// System responsible for applying changes the user made to a rigid-body-related component. pub fn apply_rigid_body_user_changes( - mut context: WriteRapierContext, + mut rigid_body_sets: Query<&mut RapierRigidBodySet>, config: Query<&RapierConfiguration>, changed_rb_types: Query< (&RapierRigidBodyHandle, &RapierContextEntityLink, &RigidBody), @@ -138,9 +141,12 @@ pub fn apply_rigid_body_user_changes( // Deal with sleeping first, because other changes may then wake-up the // rigid-body again. for (handle, link, sleeping) in changed_sleeping.iter() { - let context = context.context(link).into_inner(); + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { let activation = rb.activation_mut(); activation.normalized_linear_threshold = sleeping.normalized_linear_threshold; activation.angular_threshold = sleeping.angular_threshold; @@ -160,7 +166,10 @@ pub fn apply_rigid_body_user_changes( // changed to anything else, a transform change would modify the next // position instead of the current one. for (handle, link, rb_type) in changed_rb_types.iter() { - let context = context.context(link).into_inner(); + let context = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); if let Some(rb) = context.bodies.get_mut(handle.0) { rb.set_body_type((*rb_type).into(), true); } @@ -185,7 +194,10 @@ pub fn apply_rigid_body_user_changes( }; for (handle, link, global_transform, mut interpolation) in changed_transforms.iter_mut() { - let context = context.context(link).into_inner(); + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); let config = config .get(link.0) .expect("Could not get `RapierConfiguration`"); @@ -198,7 +210,7 @@ pub fn apply_rigid_body_user_changes( &handle.0, config, global_transform, - &context.last_body_transform_set, + &rigidbody_set.last_body_transform_set, )) }); @@ -210,13 +222,13 @@ pub fn apply_rigid_body_user_changes( } } // TODO: avoid to run multiple times the mutable deref ? - if let Some(rb) = context.bodies.get_mut(handle.0) { + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { transform_changed = transform_changed.or_else(|| { Some(transform_changed_fn( &handle.0, config, global_transform, - &context.last_body_transform_set, + &rigidbody_set.last_body_transform_set, )) }); @@ -226,7 +238,7 @@ pub fn apply_rigid_body_user_changes( rb.set_next_kinematic_position(utils::transform_to_iso( &global_transform.compute_transform(), )); - context + rigidbody_set .last_body_transform_set .insert(handle.0, *global_transform); } @@ -237,7 +249,7 @@ pub fn apply_rigid_body_user_changes( utils::transform_to_iso(&global_transform.compute_transform()), true, ); - context + rigidbody_set .last_body_transform_set .insert(handle.0, *global_transform); } @@ -247,8 +259,11 @@ pub fn apply_rigid_body_user_changes( } for (handle, link, velocity) in changed_velocities.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_linvel(velocity.linvel.into(), true); #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled rb.set_angvel(velocity.angvel.into(), true); @@ -256,8 +271,11 @@ pub fn apply_rigid_body_user_changes( } for (entity, link, handle, mprops) in changed_additional_mass_props.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { match mprops { AdditionalMassProperties::MassProperties(mprops) => { rb.set_additional_mass_properties(mprops.into_rapier(), true); @@ -272,22 +290,31 @@ pub fn apply_rigid_body_user_changes( } for (handle, link, additional_solver_iters) in changed_additional_solver_iterations.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_additional_solver_iterations(additional_solver_iters.0); } } for (handle, link, locked_axes) in changed_locked_axes.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_locked_axes((*locked_axes).into(), true); } } for (handle, link, forces) in changed_forces.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.reset_forces(true); rb.reset_torques(true); rb.add_force(forces.force.into(), true); @@ -297,8 +324,11 @@ pub fn apply_rigid_body_user_changes( } for (handle, link, mut impulses) in changed_impulses.iter_mut() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.apply_impulse(impulses.impulse.into(), true); #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled rb.apply_torque_impulse(impulses.torque_impulse.into(), true); @@ -307,44 +337,62 @@ pub fn apply_rigid_body_user_changes( } for (handle, link, gravity_scale) in changed_gravity_scale.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_gravity_scale(gravity_scale.0, true); } } for (handle, link, ccd) in changed_ccd.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.enable_ccd(ccd.enabled); } } for (handle, link, soft_ccd) in changed_soft_ccd.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_soft_ccd_prediction(soft_ccd.prediction); } } for (handle, link, dominance) in changed_dominance.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_dominance_group(dominance.groups); } } for (handle, link, damping) in changed_damping.iter() { - let context = context.context(link).into_inner(); - if let Some(rb) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(rb) = rigidbody_set.bodies.get_mut(handle.0) { rb.set_linear_damping(damping.linear_damping); rb.set_angular_damping(damping.angular_damping); } } for (handle, link, _) in changed_disabled.iter() { - let context = context.context(link).into_inner(); - if let Some(co) = context.bodies.get_mut(handle.0) { + let rigidbody_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); + if let Some(co) = rigidbody_set.bodies.get_mut(handle.0) { co.set_enabled(false); } } @@ -353,7 +401,7 @@ pub fn apply_rigid_body_user_changes( /// System responsible for writing the result of the last simulation step into our `bevy_rapier` /// components and the [`GlobalTransform`] component. pub fn writeback_rigid_bodies( - mut context: WriteRapierContext, + mut rigid_body_sets: Query<&mut RapierRigidBodySet>, timestep_mode: Res, config: Query<&RapierConfiguration>, sim_to_render_time: Query<&SimulationToRenderTime>, @@ -374,14 +422,17 @@ pub fn writeback_rigid_bodies( } let handle = handle.0; - let context = context.context(link).into_inner(); + let rigid_body_set = rigid_body_sets + .get_mut(link.0) + .expect(RAPIER_CONTEXT_EXPECT_ERROR) + .into_inner(); let sim_to_render_time = sim_to_render_time .get(link.0) .expect("Could not get `SimulationToRenderTime`"); // TODO: do this the other way round: iterate through Rapier’s RigidBodySet on the active bodies, // and update the components accordingly. That way, we don’t have to iterate through the entities that weren’t changed // by physics (for example because they are sleeping). - if let Some(rb) = context.bodies.get(handle) { + if let Some(rb) = rigid_body_set.bodies.get(handle) { let mut interpolated_pos = utils::iso_to_transform(rb.position()); if let TimestepMode::Interpolated { dt, .. } = *timestep_mode { @@ -452,7 +503,7 @@ pub fn writeback_rigid_bodies( // despite rounding errors. let new_global_transform = parent_global_transform.mul_transform(*transform); - context + rigid_body_set .last_body_transform_set .insert(handle, new_global_transform); } else { @@ -472,7 +523,7 @@ pub fn writeback_rigid_bodies( transform.translation = interpolated_pos.translation; } - context + rigid_body_set .last_body_transform_set .insert(handle, GlobalTransform::from(interpolated_pos)); } @@ -511,7 +562,7 @@ pub fn writeback_rigid_bodies( pub fn init_rigid_bodies( mut commands: Commands, default_context_access: Query>, - mut context: Query<(Entity, &mut RapierContext)>, + mut rigidbody_sets: Query<(Entity, &mut RapierRigidBodySet)>, rigid_bodies: Query>, ) { for ( @@ -621,18 +672,20 @@ pub fn init_rigid_bodies( continue; }; - let Ok((_, mut context)) = context.get_mut(context_entity) else { + let Ok((_, mut rigidbody_set)) = rigidbody_sets.get_mut(context_entity) else { log::error!("Could not find entity {context_entity} with rapier context while initializing {entity}"); continue; }; - let handle = context.bodies.insert(rb); + let handle = rigidbody_set.bodies.insert(rb); commands .entity(entity) .insert(RapierRigidBodyHandle(handle)); - context.entity2body.insert(entity, handle); + rigidbody_set.entity2body.insert(entity, handle); if let Some(transform) = transform { - context.last_body_transform_set.insert(handle, *transform); + rigidbody_set + .last_body_transform_set + .insert(handle, *transform); } } } @@ -643,7 +696,7 @@ pub fn init_rigid_bodies( /// mass to be available, which it was not because colliders were not created yet. As a /// result, we run this system after the collider creation. pub fn apply_initial_rigid_body_impulses( - mut context: WriteRapierContext, + mut context: Query<(&mut RapierRigidBodySet, &RapierContextColliders)>, // We can’t use RapierRigidBodyHandle yet because its creation command hasn’t been // executed yet. mut init_impulses: Query< @@ -652,15 +705,18 @@ pub fn apply_initial_rigid_body_impulses( >, ) { for (entity, link, mut impulse) in init_impulses.iter_mut() { - let context = context.context(link).into_inner(); - let bodies = &mut context.bodies; - if let Some(rb) = context + let (mut rigidbody_set, context_colliders) = + context.get_mut(link.0).expect(RAPIER_CONTEXT_EXPECT_ERROR); + let rigidbody_set = &mut *rigidbody_set; + + let bodies = &mut rigidbody_set.bodies; + if let Some(rb) = rigidbody_set .entity2body .get(&entity) .and_then(|h| bodies.get_mut(*h)) { // Make sure the mass-properties are computed. - rb.recompute_mass_properties_from_colliders(&context.colliders); + rb.recompute_mass_properties_from_colliders(&context_colliders.colliders); // Apply the impulse. rb.apply_impulse(impulse.impulse.into(), false); diff --git a/src/plugin/systems/writeback.rs b/src/plugin/systems/writeback.rs index f4eead69..42f4b345 100644 --- a/src/plugin/systems/writeback.rs +++ b/src/plugin/systems/writeback.rs @@ -1,15 +1,14 @@ use crate::dynamics::MassProperties; use crate::dynamics::ReadMassProperties; -use crate::plugin::context::RapierContextEntityLink; +use crate::plugin::context::{RapierContextEntityLink, RapierRigidBodySet}; use crate::plugin::RapierConfiguration; -use crate::plugin::RapierContextAccess; use crate::prelude::MassModifiedEvent; use bevy::prelude::*; /// System responsible for writing updated mass properties back into the [`ReadMassProperties`] component. pub fn writeback_mass_properties( link: Query<&RapierContextEntityLink>, - context: RapierContextAccess, + rigidbody_set: Query<&RapierRigidBodySet>, config: Query<&RapierConfiguration>, mut mass_props: Query<&mut ReadMassProperties>, @@ -23,10 +22,12 @@ pub fn writeback_mass_properties( .get(link.0) .expect("Could not find `RapierConfiguration`"); if config.physics_pipeline_active { - let context = context.context(link); + let Ok(rigidbody_set) = rigidbody_set.get(link.0) else { + continue; + }; - if let Some(handle) = context.entity2body.get(entity).copied() { - if let Some(rb) = context.bodies.get(handle) { + if let Some(handle) = rigidbody_set.entity2body.get(entity).copied() { + if let Some(rb) = rigidbody_set.bodies.get(handle) { if let Ok(mut mass_props) = mass_props.get_mut(**entity) { let new_mass_props = MassProperties::from_rapier(rb.mass_properties().local_mprops); diff --git a/src/render/mod.rs b/src/render/mod.rs index abdd20b4..1e6228e3 100644 --- a/src/render/mod.rs +++ b/src/render/mod.rs @@ -1,4 +1,6 @@ -use crate::plugin::RapierContext; +use crate::plugin::context::{ + RapierContextColliders, RapierContextJoints, RapierContextSimulation, RapierRigidBodySet, +}; use bevy::prelude::*; use bevy::transform::TransformSystem; use rapier::math::{Point, Real}; @@ -120,25 +122,27 @@ impl Plugin for RapierDebugRenderPlugin { } } -struct BevyLinesRenderBackend<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> { +struct BevyLinesRenderBackend<'world, 'state, 'world2, 'state2, 'a, 'c, 'd, 'v, 'p> { custom_colors: &'c Query<'world, 'state, &'a ColliderDebugColor>, default_collider_debug: ColliderDebug, override_visibility: &'v Query<'world, 'state, &'a ColliderDebug>, - context: &'b RapierContext, + context_colliders: &'d RapierContextColliders, gizmos: &'p mut Gizmos<'world2, 'state2>, } -impl<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> - BevyLinesRenderBackend<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> +impl<'world, 'state, 'world2, 'state2, 'a, 'c, 'd, 'v, 'p> + BevyLinesRenderBackend<'world, 'state, 'world2, 'state2, 'a, 'c, 'd, 'v, 'p> { fn object_color(&self, object: DebugRenderObject, default: [f32; 4]) -> [f32; 4] { let color = match object { - DebugRenderObject::Collider(h, ..) => self.context.colliders.get(h).and_then(|co| { - self.custom_colors - .get(Entity::from_bits(co.user_data as u64)) - .map(|co| co.0) - .ok() - }), + DebugRenderObject::Collider(h, ..) => { + self.context_colliders.colliders.get(h).and_then(|co| { + self.custom_colors + .get(Entity::from_bits(co.user_data as u64)) + .map(|co| co.0) + .ok() + }) + } _ => None, }; @@ -148,7 +152,7 @@ impl<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> fn drawing_enabled(&self, object: DebugRenderObject) -> bool { match object { DebugRenderObject::Collider(h, ..) => { - let Some(collider) = self.context.colliders.get(h) else { + let Some(collider) = self.context_colliders.colliders.get(h) else { return false; }; let entity = Entity::from_bits(collider.user_data as u64); @@ -166,8 +170,8 @@ impl<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> } } -impl<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> DebugRenderBackend - for BevyLinesRenderBackend<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> +impl<'world, 'state, 'world2, 'state2, 'a, 'c, 'd, 'v, 'p> DebugRenderBackend + for BevyLinesRenderBackend<'world, 'state, 'world2, 'state2, 'a, 'c, 'd, 'v, 'p> { #[cfg(feature = "dim2")] fn draw_line( @@ -211,7 +215,12 @@ impl<'world, 'state, 'world2, 'state2, 'a, 'b, 'c, 'v, 'p> DebugRenderBackend } fn debug_render_scene<'a>( - rapier_context: Query<&RapierContext>, + rapier_context: Query<( + &RapierContextSimulation, + &RapierContextColliders, + &RapierContextJoints, + &RapierRigidBodySet, + )>, mut render_context: ResMut, mut gizmos: Gizmos, custom_colors: Query<&'a ColliderDebugColor>, @@ -220,22 +229,22 @@ fn debug_render_scene<'a>( if !render_context.enabled { return; } - for rapier_context in rapier_context.iter() { + for (rapier_context, rapier_context_colliders, joints, rigidbody_set) in rapier_context.iter() { let mut backend = BevyLinesRenderBackend { custom_colors: &custom_colors, default_collider_debug: render_context.default_collider_debug, override_visibility: &override_visibility, - context: rapier_context, + context_colliders: rapier_context_colliders, gizmos: &mut gizmos, }; let unscaled_style = render_context.pipeline.style; render_context.pipeline.render( &mut backend, - &rapier_context.bodies, - &rapier_context.colliders, - &rapier_context.impulse_joints, - &rapier_context.multibody_joints, + &rigidbody_set.bodies, + &rapier_context_colliders.colliders, + &joints.impulse_joints, + &joints.multibody_joints, &rapier_context.narrow_phase, ); render_context.pipeline.style = unscaled_style;