From d671d7ae20494781a59de9506a7bb5f126d8e7e7 Mon Sep 17 00:00:00 2001 From: AnthonyTornetta Date: Fri, 10 Feb 2023 12:48:59 -0500 Subject: [PATCH 01/92] Refactored context to have multiple worlds --- src/plugin/context.rs | 730 +++++++++++++++++++++++++++++++++++------- 1 file changed, 622 insertions(+), 108 deletions(-) diff --git a/src/plugin/context.rs b/src/plugin/context.rs index 70f6cd6bc..bcfb1f8ef 100644 --- a/src/plugin/context.rs +++ b/src/plugin/context.rs @@ -1,4 +1,5 @@ use bevy::prelude::*; +use core::fmt; use std::collections::HashMap; use std::sync::RwLock; @@ -23,10 +24,13 @@ use crate::prelude::{CollisionGroups, RapierRigidBodyHandle}; use bevy::math::Vec3Swizzles; use rapier::control::CharacterAutostep; +type WorldId = usize; + +pub const DEFAULT_WORLD_ID: WorldId = 0; + /// The Rapier context, containing all the state of the physics engine. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Resource)] -pub struct RapierContext { +pub struct RapierWorld { /// The island manager, which detects what object is sleeping /// (not moving much) to reduce computations. pub islands: IslandManager, @@ -75,73 +79,7 @@ pub struct RapierContext { pub(crate) character_collisions_collector: Vec, } -impl Default for RapierContext { - fn default() -> Self { - Self { - islands: IslandManager::new(), - broad_phase: BroadPhase::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(), - physics_scale: 1.0, - 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(), - character_collisions_collector: vec![], - } - } -} - -impl RapierContext { - /// Get the physics scale that was set for this Rapier context. - /// - /// See [`RapierPhysicsPlugin::with_physics_scale()`][crate::plugin::RapierPhysicsPlugin::with_physics_scale()]. - pub fn physics_scale(&self) -> Real { - self.physics_scale - } - - /// 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 { - self.entity2collider - .get(&entity) - .and_then(|h| self.colliders.get(*h)) - .and_then(|co| co.parent()) - .and_then(|h| self.rigid_body_entity(h)) - } - - /// Retrieve the Bevy entity the given Rapier collider (identified by its handle) is attached. - pub fn collider_entity(&self, handle: ColliderHandle) -> Option { - Self::collider_entity_with_set(&self.colliders, handle) - } - - // Mostly used to avoid borrowing self completely. - pub(crate) fn collider_entity_with_set( - colliders: &ColliderSet, - handle: ColliderHandle, - ) -> Option { - colliders - .get(handle) - .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)) - } - +impl RapierWorld { fn with_query_filter( &self, filter: QueryFilter, @@ -188,9 +126,7 @@ impl RapierContext { } } - /// Advance the simulation, based on the given timestep mode. - #[allow(clippy::too_many_arguments)] - pub fn step_simulation( + fn step_simulation( &mut self, gravity: Vect, timestep_mode: TimestepMode, @@ -315,51 +251,15 @@ impl RapierContext { } } - /// This method makes sure tha the rigid-body positions have been propagated to - /// their attached colliders, without having to perform a srimulation step. pub fn propagate_modified_body_positions_to_colliders(&mut self) { self.bodies .propagate_modified_body_positions_to_colliders(&mut self.colliders); } - /// 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.bodies, &self.colliders); } - /// The map from entities to rigid-body handles. - pub fn entity2body(&self) -> &HashMap { - &self.entity2body - } - - /// The map from entities to collider handles. - pub fn entity2collider(&self) -> &HashMap { - &self.entity2collider - } - - /// 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 - } - - /// Attempts to move shape, optionally sliding or climbing obstacles. - /// - /// # 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. #[allow(clippy::too_many_arguments)] pub fn move_shape( &mut self, @@ -460,6 +360,21 @@ impl RapierContext { } } + // Mostly used to avoid borrowing self completely. + pub(crate) fn collider_entity_with_set( + colliders: &ColliderSet, + handle: ColliderHandle, + ) -> Option { + colliders + .get(handle) + .map(|c| Entity::from_bits(c.user_data as u64)) + } + + /// Retrieve the Bevy entity the given Rapier collider (identified by its handle) is attached. + pub fn collider_entity(&self, handle: ColliderHandle) -> Option { + RapierWorld::collider_entity_with_set(&self.colliders, handle) + } + /// Find the closest intersection between a ray and a set of collider. /// /// # Parameters @@ -873,3 +788,602 @@ impl RapierContext { }); } } + +#[derive(Debug)] +pub enum WorldError { + WorldNotFound { world_id: WorldId }, +} + +impl fmt::Display for WorldError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match *self { + Self::WorldNotFound { world_id } => write!(f, "World with id {world_id} not found."), + } + } +} + +impl std::error::Error for WorldError {} + +/// The Rapier context, containing all the state of the physics engine. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Resource)] +pub struct RapierContext { + worlds: HashMap, +} + +impl Default for RapierContext { + fn default() -> Self { + // By default, there will be one world + + let mut worlds = HashMap::new(); + worlds.insert( + 0, + RapierWorld { + islands: IslandManager::new(), + broad_phase: BroadPhase::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(), + physics_scale: 1.0, + 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(), + character_collisions_collector: vec![], + }, + ); + + Self { worlds } + } +} + +impl RapierContext { + pub fn physics_scale(&self, world_id: WorldId) -> Option { + self.worlds.get(&world_id).map(|x| x.physics_scale) + } + + /// Get the physics scale that was set for this Rapier context. + /// + /// Returns None if no world was found with that id. + /// + /// See [`RapierPhysicsPlugin::with_physics_scale()`][crate::plugin::RapierPhysicsPlugin::with_physics_scale()]. + pub fn physics_scale_for_world(&self, world_id: WorldId) -> Option { + self.worlds.get(&world_id).map(|world| world.physics_scale) + } + + fn get_collider_parent_from_world( + &self, + entity: Entity, + world: &RapierWorld, + ) -> Option { + world + .entity2collider + .get(&entity) + .and_then(|h| world.colliders.get(*h)) + .and_then(|co| co.parent()) + .and_then(|h| self.rigid_body_entity(h)) + } + + /// 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 { + for (_, world) in self.worlds.iter() { + if let Some(entity) = self.get_collider_parent_from_world(entity, world) { + return Some(entity); + } + } + + None + } + + /// If the collider attached to `entity` is attached to a rigid-body, this + /// returns the `Entity` containing that rigid-body. + /// + /// If the world does not exist, this returns None + pub fn collider_parent_for_world(&self, entity: Entity, world_id: WorldId) -> Option { + if let Some(world) = self.worlds.get(&world_id) { + self.get_collider_parent_from_world(entity, world) + } else { + None + } + } + + /// Retrieve the Bevy entity the given Rapier collider (identified by its handle) is attached. + pub fn collider_entity(&self, handle: ColliderHandle) -> Option { + for (_, world) in self.worlds.iter() { + let entity = RapierWorld::collider_entity_with_set(&world.colliders, handle); + if entity.is_some() { + return entity; + } + } + + None + } + + fn get_rigid_body_entity_for_world( + &self, + handle: RigidBodyHandle, + world: &RapierWorld, + ) -> Option { + world + .bodies + .get(handle) + .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 { + for (_, world) in self.worlds.iter() { + let entity = self.get_rigid_body_entity_for_world(handle, world); + if entity.is_some() { + return entity; + } + } + + None + } + + /// Retrieve the Bevy entity the given Rapier rigid-body (identified by its handle) is attached. + /// + /// Returns None if this world does not exist + pub fn rigid_body_entity_in_world( + &self, + handle: RigidBodyHandle, + world_id: WorldId, + ) -> Option { + self.worlds + .get(&world_id) + .map(|world| self.get_rigid_body_entity_for_world(handle, world)) + .unwrap_or(None) + } + + /// Advance the simulation, based on the given timestep mode. + #[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< + Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, + >, + ) { + for (_, world) in self.worlds.iter_mut() { + world.step_simulation( + gravity, + timestep_mode, + events, + hooks, + time, + sim_to_render_time, + interpolation_query, + ); + } + } + + /// This method makes sure that the rigid-body positions have been propagated to + /// their attached colliders, without having to perform a srimulation step. + pub fn propagate_modified_body_positions_to_colliders(&mut self) { + for (_, world) in self.worlds { + world.propagate_modified_body_positions_to_colliders(); + } + } + + /// This method makes sure that the rigid-body positions have been propagated to + /// their attached colliders, without having to perform a srimulation step. + /// + /// Returns Ok if the world was found, Err(WorldError::WorldNotFound) if the world was not found. + pub fn propagate_modified_body_positions_to_colliders_for_world( + &mut self, + world_id: WorldId, + ) -> Result<(), WorldError> { + match self.worlds.get(&world_id) { + Some(world) => { + world.propagate_modified_body_positions_to_colliders(); + + Ok(()) + } + None => Err(WorldError::WorldNotFound { world_id: world_id }), + } + } + + /// 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) { + for (_, world) in self.worlds.iter_mut() { + world.update_query_pipeline(); + } + } + + /// The map from entities to rigid-body handles. + /// + /// Returns Err if the world doesn't exist, or the entity2body if it does + pub fn entity2body( + &self, + world_id: WorldId, + ) -> Result<&HashMap, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |x| { + Ok(&x.entity2body) + }) + } + + /// The map from entities to collider handles. + pub fn entity2collider( + &self, + world_id: WorldId, + ) -> Result<&HashMap, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |x| { + Ok(&x.entity2collider) + }) + } + + /// The map from entities to impulse joint handles. + pub fn entity2impulse_joint( + &self, + world_id: WorldId, + ) -> Result<&HashMap, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |x| { + Ok(&x.entity2impulse_joint) + }) + } + + /// The map from entities to multibody joint handles. + pub fn entity2multibody_joint( + &self, + world_id: WorldId, + ) -> Result<&HashMap, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |x| { + Ok(&x.entity2multibody_joint) + }) + } + + /// Attempts to move shape, optionally sliding or climbing obstacles. + /// + /// # 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. + #[allow(clippy::too_many_arguments)] + pub fn move_shape( + &mut self, + world_id: WorldId, + movement: Vect, + shape: &Collider, + shape_translation: Vect, + shape_rotation: Rot, + shape_mass: Real, + options: &MoveShapeOptions, + filter: QueryFilter, + mut events: impl FnMut(CharacterCollision), + ) -> Result { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.move_shape( + movement, + shape, + shape_translation, + shape_rotation, + shape_mass, + options, + filter, + events, + )) + }) + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// * `world_id`: the world to cast this ray in. Use DEFAULT_WORLD_ID for a single-world simulation + /// * `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, + world_id: WorldId, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.cast_ray(ray_origin, ray_dir, max_toi, solid, filter)) + }) + } + + /// Find the closest intersection between a ray and a set of collider. + /// + /// # Parameters + /// * `world_id`: the world to cast this ray in. Use DEFAULT_WORLD_ID for a single-world simulation + /// * `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_and_get_normal( + &self, + world_id: WorldId, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.cast_ray_and_get_normal(ray_origin, ray_dir, max_toi, solid, filter)) + }) + } + + /// Find the all intersections between a ray and a set of collider and passes them to a callback. + /// + /// # Parameters + /// * `world_id`: the world to cast this ray in. Use DEFAULT_WORLD_ID for a single-world simulation + /// * `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, + world_id: WorldId, + ray_origin: Vect, + ray_dir: Vect, + max_toi: Real, + solid: bool, + filter: QueryFilter, + mut callback: impl FnMut(Entity, RayIntersection) -> bool, + ) -> Result<(), WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world + .intersections_with_ray(ray_origin, ray_dir, max_toi, solid, filter, callback)) + }) + } + + /// 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, + world_id: WorldId, + shape_pos: Vect, + shape_rot: Rot, + shape: &Collider, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.intersection_with_shape(shape_pos, shape_rot, shape, 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( + &self, + world_id: WorldId, + point: Vect, + solid: bool, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.project_point(point, solid, filter)) + }) + } + + /// 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, + world_id: WorldId, + point: Vect, + filter: QueryFilter, + mut callback: impl FnMut(Entity) -> bool, + ) -> Result<(), WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.intersections_with_point(point, filter, callback)) + }) + } + + /// 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, + world_id: WorldId, + point: Vect, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.project_point_and_get_feature(point, filter)) + }) + } + + /// Finds all entities of all the colliders with an Aabb intersecting the given Aabb. + pub fn colliders_with_aabb_intersecting_aabb( + &self, + world_id: WorldId, + aabb: Aabb, + mut callback: impl FnMut(Entity) -> bool, + ) -> Result<(), WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.colliders_with_aabb_intersecting_aabb(aabb, callback)) + }) + } + + /// 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 `TOI`, witness and normal 1 refer to the world + /// collider, and are in world space. + /// + /// # Parameters + /// * `shape_pos` - The initial position 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 `shapeVel.norm() * maxToi`. + /// * `filter`: set of rules used to determine which collider is taken into account by this scene query. + #[allow(clippy::too_many_arguments)] + pub fn cast_shape( + &self, + world_id: WorldId, + shape_pos: Vect, + shape_rot: Rot, + shape_vel: Vect, + shape: &Collider, + max_toi: Real, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.cast_shape(shape_pos, shape_rot, shape_vel, shape, max_toi, filter)) + }) + } + + /* 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 `TOI`, 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, + world_id: WorldId, + shape_motion: &NonlinearRigidMotion, + shape: &Collider, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, + filter: QueryFilter, + ) -> Result, WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.nonlinear_cast_shape(shape_motion, shape, start_time, end_time, stop_at_penetration, filter)) + }) + } + */ + + /// 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, + world_id: WorldId, + shape_pos: Vect, + shape_rot: Rot, + shape: &Collider, + filter: QueryFilter, + mut callback: impl FnMut(Entity) -> bool, + ) -> Result<(), WorldError> { + self.worlds + .get(&world_id) + .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + Ok(world.intersections_with_shape(shape_pos, shape_rot, shape, filter, callback)) + }) + } +} From cdfafa40c4cd04e6d5bb3b8fa96328df6ebfaa8f Mon Sep 17 00:00:00 2001 From: AnthonyTornetta Date: Sat, 11 Feb 2023 14:40:24 -0500 Subject: [PATCH 02/92] Supporting multiple worlds --- src/control/character_controller.rs | 8 +- src/plugin/context.rs | 113 +- src/plugin/mod.rs | 4 +- src/plugin/narrow_phase.rs | 9 +- src/plugin/plugin.rs | 6 +- src/plugin/systems.rs | 1687 ++++++++++++++------------- src/render/mod.rs | 51 +- 7 files changed, 967 insertions(+), 911 deletions(-) diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index 1786cf638..7e5bd0563 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -1,8 +1,8 @@ use crate::geometry::{Collider, CollisionGroups, Toi}; use crate::math::{Real, Rot, Vect}; +use crate::plugin::context::RapierWorld; use bevy::prelude::*; -use crate::plugin::RapierContext; pub use rapier::control::CharacterAutostep; pub use rapier::control::CharacterLength; use rapier::prelude::{ColliderSet, QueryFilterFlags}; @@ -26,10 +26,10 @@ pub struct CharacterCollision { impl CharacterCollision { pub(crate) fn from_raw( - ctxt: &RapierContext, + world: &RapierWorld, c: &rapier::control::CharacterCollision, ) -> Option { - Self::from_raw_with_set(ctxt.physics_scale, &ctxt.colliders, c) + Self::from_raw_with_set(world.physics_scale, &world.colliders, c) } pub(crate) fn from_raw_with_set( @@ -37,7 +37,7 @@ impl CharacterCollision { colliders: &ColliderSet, c: &rapier::control::CharacterCollision, ) -> Option { - RapierContext::collider_entity_with_set(colliders, c.handle).map(|entity| { + RapierWorld::collider_entity_with_set(colliders, c.handle).map(|entity| { CharacterCollision { entity, character_translation: (c.character_pos.translation.vector * physics_scale).into(), diff --git a/src/plugin/context.rs b/src/plugin/context.rs index bcfb1f8ef..582b6cd25 100644 --- a/src/plugin/context.rs +++ b/src/plugin/context.rs @@ -126,6 +126,12 @@ impl RapierWorld { } } + pub fn rigid_body_entity(&self, handle: RigidBodyHandle) -> Option { + self.bodies + .get(handle) + .map(|c| Entity::from_bits(c.user_data as u64)) + } + fn step_simulation( &mut self, gravity: Vect, @@ -134,7 +140,7 @@ impl RapierWorld { hooks: &dyn PhysicsHooks, time: &Time, sim_to_render_time: &mut SimulationToRenderTime, - mut interpolation_query: Option< + interpolation_query: &mut Option< Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, >, ) { @@ -270,7 +276,7 @@ impl RapierWorld { shape_mass: Real, options: &MoveShapeOptions, filter: QueryFilter, - mut events: impl FnMut(CharacterCollision), + events: &mut impl FnMut(CharacterCollision), ) -> MoveShapeOutput { let physics_scale = self.physics_scale; let mut scaled_shape = shape.clone(); @@ -789,6 +795,33 @@ impl RapierWorld { } } +impl Default for RapierWorld { + fn default() -> Self { + Self { + islands: IslandManager::new(), + broad_phase: BroadPhase::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(), + physics_scale: 1.0, + 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(), + character_collisions_collector: vec![], + } + } +} + #[derive(Debug)] pub enum WorldError { WorldNotFound { world_id: WorldId }, @@ -808,45 +841,31 @@ impl std::error::Error for WorldError {} #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Resource)] pub struct RapierContext { - worlds: HashMap, + pub worlds: HashMap, } +impl RapierContext {} + impl Default for RapierContext { fn default() -> Self { - // By default, there will be one world + Self::new(RapierWorld::default()) + } +} +impl RapierContext { + pub fn new(world: RapierWorld) -> Self { let mut worlds = HashMap::new(); - worlds.insert( - 0, - RapierWorld { - islands: IslandManager::new(), - broad_phase: BroadPhase::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(), - physics_scale: 1.0, - 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(), - character_collisions_collector: vec![], - }, - ); + worlds.insert(DEFAULT_WORLD_ID, world); Self { worlds } } -} -impl RapierContext { + pub fn get_world(&self, world_id: WorldId) -> Result<&RapierWorld, WorldError> { + self.worlds + .get(&world_id) + .ok_or(WorldError::WorldNotFound { world_id }) + } + pub fn physics_scale(&self, world_id: WorldId) -> Option { self.worlds.get(&world_id).map(|x| x.physics_scale) } @@ -909,21 +928,10 @@ impl RapierContext { None } - fn get_rigid_body_entity_for_world( - &self, - handle: RigidBodyHandle, - world: &RapierWorld, - ) -> Option { - world - .bodies - .get(handle) - .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 { for (_, world) in self.worlds.iter() { - let entity = self.get_rigid_body_entity_for_world(handle, world); + let entity = world.rigid_body_entity(handle); if entity.is_some() { return entity; } @@ -942,7 +950,7 @@ impl RapierContext { ) -> Option { self.worlds .get(&world_id) - .map(|world| self.get_rigid_body_entity_for_world(handle, world)) + .map(|world| world.rigid_body_entity(handle)) .unwrap_or(None) } @@ -968,7 +976,7 @@ impl RapierContext { hooks, time, sim_to_render_time, - interpolation_query, + &mut interpolation_query, ); } } @@ -976,7 +984,7 @@ impl RapierContext { /// This method makes sure that the rigid-body positions have been propagated to /// their attached colliders, without having to perform a srimulation step. pub fn propagate_modified_body_positions_to_colliders(&mut self) { - for (_, world) in self.worlds { + for (_, world) in self.worlds.iter_mut() { world.propagate_modified_body_positions_to_colliders(); } } @@ -989,7 +997,7 @@ impl RapierContext { &mut self, world_id: WorldId, ) -> Result<(), WorldError> { - match self.worlds.get(&world_id) { + match self.worlds.get_mut(&world_id) { Some(world) => { world.propagate_modified_body_positions_to_colliders(); @@ -1080,11 +1088,11 @@ impl RapierContext { shape_mass: Real, options: &MoveShapeOptions, filter: QueryFilter, - mut events: impl FnMut(CharacterCollision), + events: &mut impl FnMut(CharacterCollision), ) -> Result { - self.worlds - .get(&world_id) - .map_or(Err(WorldError::WorldNotFound { world_id }), |world| { + self.worlds.get_mut(&world_id).map_or( + Err(WorldError::WorldNotFound { world_id }), + |world| { Ok(world.move_shape( movement, shape, @@ -1095,7 +1103,8 @@ impl RapierContext { filter, events, )) - }) + }, + ) } /// Find the closest intersection between a ray and a set of collider. diff --git a/src/plugin/mod.rs b/src/plugin/mod.rs index d832673b0..3ad5de854 100644 --- a/src/plugin/mod.rs +++ b/src/plugin/mod.rs @@ -7,7 +7,7 @@ pub use self::plugin::{NoUserData, PhysicsStages, RapierPhysicsPlugin}; pub mod systems; mod configuration; -mod context; +pub(crate) mod context; mod narrow_phase; #[allow(clippy::module_inception)] -mod plugin; +pub(crate) mod plugin; diff --git a/src/plugin/narrow_phase.rs b/src/plugin/narrow_phase.rs index 001ef60e9..17e1a04c8 100644 --- a/src/plugin/narrow_phase.rs +++ b/src/plugin/narrow_phase.rs @@ -1,9 +1,10 @@ use crate::math::{Real, Vect}; -use crate::plugin::RapierContext; use bevy::prelude::*; use rapier::geometry::{Contact, ContactManifold, ContactPair, SolverContact, SolverFlags}; -impl RapierContext { +use super::context::RapierWorld; + +impl RapierWorld { /// All the contacts involving the non-sensor collider attached to the given entity. pub fn contacts_with(&self, collider: Entity) -> impl Iterator { self.entity2collider @@ -86,7 +87,7 @@ impl RapierContext { /// Read-only access to the properties of a contact manifold. pub struct ContactManifoldView<'a> { - context: &'a RapierContext, + context: &'a RapierWorld, /// The raw contact manifold from Rapier. pub raw: &'a ContactManifold, } @@ -291,7 +292,7 @@ impl<'a> SolverContactView<'a> { /// Read-only access to the properties of a contact pair. pub struct ContactPairView<'a> { - context: &'a RapierContext, + context: &'a RapierWorld, /// The raw contact pair from Rapier. pub raw: &'a ContactPair, } diff --git a/src/plugin/plugin.rs b/src/plugin/plugin.rs index ffd761dd5..2b767fc82 100644 --- a/src/plugin/plugin.rs +++ b/src/plugin/plugin.rs @@ -7,6 +7,8 @@ use bevy::ecs::system::SystemParamItem; use bevy::prelude::*; use std::marker::PhantomData; +use super::context::RapierWorld; + /// No specific user-data is associated to the hooks. pub type NoUserData = (); @@ -203,10 +205,10 @@ where } app.insert_resource(SimulationToRenderTime::default()) - .insert_resource(RapierContext { + .insert_resource(RapierContext::new(RapierWorld { physics_scale: self.physics_scale, ..Default::default() - }) + })) .insert_resource(Events::::default()) .insert_resource(Events::::default()); diff --git a/src/plugin/systems.rs b/src/plugin/systems.rs index c11ba7e72..68a022320 100644 --- a/src/plugin/systems.rs +++ b/src/plugin/systems.rs @@ -150,96 +150,98 @@ pub fn apply_collider_user_changes( Changed, >, ) { - let scale = context.physics_scale; - - for (handle, transform) in changed_collider_transforms.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - if co.parent().is_none() { - co.set_position(utils::transform_to_iso( - &transform.compute_transform(), - scale, - )) + for (_, world) in context.worlds.iter_mut() { + let scale = world.physics_scale; + + for (handle, transform) in changed_collider_transforms.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + if co.parent().is_none() { + co.set_position(utils::transform_to_iso( + &transform.compute_transform(), + scale, + )) + } } } - } - for (handle, shape) in changed_shapes.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - let mut scaled_shape = shape.clone(); - scaled_shape.set_scale(shape.scale / scale, config.scaled_shape_subdivision); - co.set_shape(scaled_shape.raw.clone()) + for (handle, shape) in changed_shapes.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + let mut scaled_shape = shape.clone(); + scaled_shape.set_scale(shape.scale / scale, config.scaled_shape_subdivision); + co.set_shape(scaled_shape.raw.clone()) + } } - } - for (handle, active_events) in changed_active_events.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_active_events((*active_events).into()) + for (handle, active_events) in changed_active_events.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_active_events((*active_events).into()) + } } - } - for (handle, active_hooks) in changed_active_hooks.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_active_hooks((*active_hooks).into()) + for (handle, active_hooks) in changed_active_hooks.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_active_hooks((*active_hooks).into()) + } } - } - for (handle, active_collision_types) in changed_active_collision_types.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_active_collision_types((*active_collision_types).into()) + for (handle, active_collision_types) in changed_active_collision_types.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_active_collision_types((*active_collision_types).into()) + } } - } - for (handle, friction) in changed_friction.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_friction(friction.coefficient); - co.set_friction_combine_rule(friction.combine_rule.into()); + for (handle, friction) in changed_friction.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_friction(friction.coefficient); + co.set_friction_combine_rule(friction.combine_rule.into()); + } } - } - for (handle, restitution) in changed_restitution.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_restitution(restitution.coefficient); - co.set_restitution_combine_rule(restitution.combine_rule.into()); + for (handle, restitution) in changed_restitution.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_restitution(restitution.coefficient); + co.set_restitution_combine_rule(restitution.combine_rule.into()); + } } - } - for (handle, collision_groups) in changed_collision_groups.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_collision_groups((*collision_groups).into()); + for (handle, collision_groups) in changed_collision_groups.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_collision_groups((*collision_groups).into()); + } } - } - for (handle, solver_groups) in changed_solver_groups.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_solver_groups((*solver_groups).into()); + for (handle, solver_groups) in changed_solver_groups.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_solver_groups((*solver_groups).into()); + } } - } - for (handle, _) in changed_sensors.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_sensor(true); + for (handle, _) in changed_sensors.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_sensor(true); + } } - } - for (handle, _) in changed_disabled.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_enabled(false); + for (handle, _) in changed_disabled.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_enabled(false); + } } - } - for (handle, threshold) in changed_contact_force_threshold.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - co.set_contact_force_event_threshold(threshold.0); + for (handle, threshold) in changed_contact_force_threshold.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_contact_force_event_threshold(threshold.0); + } } - } - for (handle, mprops) in changed_collider_mass_props.iter() { - if let Some(co) = context.colliders.get_mut(handle.0) { - match mprops { - ColliderMassProperties::Density(density) => co.set_density(*density), - ColliderMassProperties::Mass(mass) => co.set_mass(*mass), - ColliderMassProperties::MassProperties(mprops) => { - co.set_mass_properties(mprops.into_rapier(scale)) + for (handle, mprops) in changed_collider_mass_props.iter() { + if let Some(co) = world.colliders.get_mut(handle.0) { + match mprops { + ColliderMassProperties::Density(density) => co.set_density(*density), + ColliderMassProperties::Mass(mass) => co.set_mass(*mass), + ColliderMassProperties::MassProperties(mprops) => { + co.set_mass_properties(mprops.into_rapier(scale)) + } } } } @@ -280,172 +282,177 @@ pub fn apply_rigid_body_user_changes( Changed, >, ) { - let context = &mut *context; - let scale = context.physics_scale; - - // Deal with sleeping first, because other changes may then wake-up the - // rigid-body again. - for (handle, sleeping) in changed_sleeping.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - let activation = rb.activation_mut(); - activation.linear_threshold = sleeping.linear_threshold; - activation.angular_threshold = sleeping.angular_threshold; - - if !sleeping.sleeping && activation.sleeping { - rb.wake_up(true); - } else if sleeping.sleeping && !activation.sleeping { - rb.sleep(); + for (_, world) in context.worlds.iter_mut() { + let context = &mut *context; + let scale = world.physics_scale; + + // Deal with sleeping first, because other changes may then wake-up the + // rigid-body again. + for (handle, sleeping) in changed_sleeping.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + let activation = rb.activation_mut(); + activation.linear_threshold = sleeping.linear_threshold; + activation.angular_threshold = sleeping.angular_threshold; + + if !sleeping.sleeping && activation.sleeping { + rb.wake_up(true); + } else if sleeping.sleeping && !activation.sleeping { + rb.sleep(); + } } } - } - // NOTE: we must change the rigid-body type before updating the - // transform or velocity. Otherwise, if the rigid-body was fixed - // and changed to anything else, the velocity change wouldn’t have any effect. - // Similarly, if the rigid-body was kinematic position-based before and - // changed to anything else, a transform change would modify the next - // position instead of the current one. - for (handle, rb_type) in changed_rb_types.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.set_body_type((*rb_type).into(), true); + // NOTE: we must change the rigid-body type before updating the + // transform or velocity. Otherwise, if the rigid-body was fixed + // and changed to anything else, the velocity change wouldn’t have any effect. + // Similarly, if the rigid-body was kinematic position-based before and + // changed to anything else, a transform change would modify the next + // position instead of the current one. + for (handle, rb_type) in changed_rb_types.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.set_body_type((*rb_type).into(), true); + } } - } - // Manually checks if the transform changed. - // This is needed for detecting if the user actually changed the rigid-body - // transform, or if it was just the change we made in our `writeback_rigid_bodies` - // system. - let transform_changed = - |handle: &RigidBodyHandle, - transform: &GlobalTransform, - last_transform_set: &HashMap| { - if config.force_update_from_transform_changes { - true - } else if let Some(prev) = last_transform_set.get(handle) { - *prev != *transform - } else { - true - } - }; + // Manually checks if the transform changed. + // This is needed for detecting if the user actually changed the rigid-body + // transform, or if it was just the change we made in our `writeback_rigid_bodies` + // system. + let transform_changed = + |handle: &RigidBodyHandle, + transform: &GlobalTransform, + last_transform_set: &HashMap| { + if config.force_update_from_transform_changes { + true + } else if let Some(prev) = last_transform_set.get(handle) { + *prev != *transform + } else { + true + } + }; - for (handle, global_transform, mut interpolation) in changed_transforms.iter_mut() { - if let Some(interpolation) = interpolation.as_deref_mut() { - // Reset the interpolation so we don’t overwrite - // the user’s input. - interpolation.start = None; - interpolation.end = None; - } + for (handle, global_transform, mut interpolation) in changed_transforms.iter_mut() { + if let Some(interpolation) = interpolation.as_deref_mut() { + // Reset the interpolation so we don’t overwrite + // the user’s input. + interpolation.start = None; + interpolation.end = None; + } - if let Some(rb) = context.bodies.get_mut(handle.0) { - match rb.body_type() { - RigidBodyType::KinematicPositionBased => { - if transform_changed( - &handle.0, - global_transform, - &context.last_body_transform_set, - ) { - rb.set_next_kinematic_position(utils::transform_to_iso( - &global_transform.compute_transform(), - scale, - )); - context - .last_body_transform_set - .insert(handle.0, *global_transform); + if let Some(rb) = world.bodies.get_mut(handle.0) { + match rb.body_type() { + RigidBodyType::KinematicPositionBased => { + if transform_changed( + &handle.0, + global_transform, + &world.last_body_transform_set, + ) { + rb.set_next_kinematic_position(utils::transform_to_iso( + &global_transform.compute_transform(), + scale, + )); + world + .last_body_transform_set + .insert(handle.0, *global_transform); + } } - } - _ => { - if transform_changed( - &handle.0, - global_transform, - &context.last_body_transform_set, - ) { - rb.set_position( - utils::transform_to_iso(&global_transform.compute_transform(), scale), - true, - ); - context - .last_body_transform_set - .insert(handle.0, *global_transform); + _ => { + if transform_changed( + &handle.0, + global_transform, + &world.last_body_transform_set, + ) { + rb.set_position( + utils::transform_to_iso( + &global_transform.compute_transform(), + scale, + ), + true, + ); + world + .last_body_transform_set + .insert(handle.0, *global_transform); + } } } } } - } - for (handle, velocity) in changed_velocities.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.set_linvel((velocity.linvel / scale).into(), true); - #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled - rb.set_angvel(velocity.angvel.into(), true); + for (handle, velocity) in changed_velocities.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.set_linvel((velocity.linvel / scale).into(), true); + #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled + rb.set_angvel(velocity.angvel.into(), true); + } } - } - for (handle, mprops) in changed_additional_mass_props.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - match mprops { - AdditionalMassProperties::MassProperties(mprops) => { - rb.set_additional_mass_properties(mprops.into_rapier(scale), true); - } - AdditionalMassProperties::Mass(mass) => { - rb.set_additional_mass(*mass, true); + for (handle, mprops) in changed_additional_mass_props.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + match mprops { + AdditionalMassProperties::MassProperties(mprops) => { + rb.set_additional_mass_properties(mprops.into_rapier(scale), true); + } + AdditionalMassProperties::Mass(mass) => { + rb.set_additional_mass(*mass, true); + } } } } - } - for (handle, locked_axes) in changed_locked_axes.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.set_locked_axes((*locked_axes).into(), true); + for (handle, locked_axes) in changed_locked_axes.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.set_locked_axes((*locked_axes).into(), true); + } } - } - for (handle, forces) in changed_forces.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.reset_forces(true); - rb.reset_torques(true); - rb.add_force((forces.force / scale).into(), true); - #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled - rb.add_torque(forces.torque.into(), true); + for (handle, forces) in changed_forces.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.reset_forces(true); + rb.reset_torques(true); + rb.add_force((forces.force / scale).into(), true); + #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled + rb.add_torque(forces.torque.into(), true); + } } - } - for (handle, mut impulses) in changed_impulses.iter_mut() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.apply_impulse((impulses.impulse / scale).into(), true); - #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled - rb.apply_torque_impulse(impulses.torque_impulse.into(), true); - impulses.reset(); + for (handle, mut impulses) in changed_impulses.iter_mut() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.apply_impulse((impulses.impulse / scale).into(), true); + #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled + rb.apply_torque_impulse(impulses.torque_impulse.into(), true); + impulses.reset(); + } } - } - for (handle, gravity_scale) in changed_gravity_scale.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.set_gravity_scale(gravity_scale.0, true); + for (handle, gravity_scale) in changed_gravity_scale.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.set_gravity_scale(gravity_scale.0, true); + } } - } - for (handle, ccd) in changed_ccd.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.enable_ccd(ccd.enabled); + for (handle, ccd) in changed_ccd.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.enable_ccd(ccd.enabled); + } } - } - for (handle, dominance) in changed_dominance.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.set_dominance_group(dominance.groups); + for (handle, dominance) in changed_dominance.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.set_dominance_group(dominance.groups); + } } - } - for (handle, damping) in changed_damping.iter() { - if let Some(rb) = context.bodies.get_mut(handle.0) { - rb.set_linear_damping(damping.linear_damping); - rb.set_angular_damping(damping.angular_damping); + for (handle, damping) in changed_damping.iter() { + if let Some(rb) = world.bodies.get_mut(handle.0) { + rb.set_linear_damping(damping.linear_damping); + rb.set_angular_damping(damping.angular_damping); + } } - } - for (handle, _) in changed_disabled.iter() { - if let Some(co) = context.bodies.get_mut(handle.0) { - co.set_enabled(false); + for (handle, _) in changed_disabled.iter() { + if let Some(co) = world.bodies.get_mut(handle.0) { + co.set_enabled(false); + } } } } @@ -462,21 +469,23 @@ pub fn apply_joint_user_changes( Changed, >, ) { - let scale = context.physics_scale; - - // TODO: right now, we only support propagating changes made to the joint data. - // Re-parenting the joint isn’t supported yet. - for (handle, changed_joint) in changed_impulse_joints.iter() { - if let Some(joint) = context.impulse_joints.get_mut(handle.0) { - joint.data = changed_joint.data.into_rapier(scale); + for (_, world) in context.worlds.iter_mut() { + let scale = world.physics_scale; + + // TODO: right now, we only support propagating changes made to the joint data. + // Re-parenting the joint isn’t supported yet. + for (handle, changed_joint) in changed_impulse_joints.iter() { + if let Some(joint) = world.impulse_joints.get_mut(handle.0) { + joint.data = changed_joint.data.into_rapier(scale); + } } - } - for (handle, changed_joint) in changed_multibody_joints.iter() { - // 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) { - link.joint.data = changed_joint.data.into_rapier(scale); + for (handle, changed_joint) in changed_multibody_joints.iter() { + // TODO: not sure this will always work properly, e.g., if the number of Dofs is changed. + if let Some((mb, link_id)) = world.multibody_joints.get_mut(handle.0) { + if let Some(link) = mb.link_mut(link_id) { + link.joint.data = changed_joint.data.into_rapier(scale); + } } } } @@ -491,133 +500,137 @@ pub fn writeback_rigid_bodies( global_transforms: Query<&GlobalTransform>, mut writeback: Query, ) { - let context = &mut *context; - let scale = context.physics_scale; + for (_, world) in context.worlds.iter_mut() { + let context = &mut *context; + let scale = world.physics_scale; - if config.physics_pipeline_active { - for (entity, parent, transform, mut interpolation, mut velocity, mut sleeping) in - writeback.iter_mut() - { - // 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(handle) = context.entity2body.get(&entity).copied() { - if let Some(rb) = context.bodies.get(handle) { - let mut interpolated_pos = utils::iso_to_transform(rb.position(), scale); - - if let TimestepMode::Interpolated { dt, .. } = config.timestep_mode { - if let Some(interpolation) = interpolation.as_deref_mut() { - if interpolation.end.is_none() { - interpolation.end = Some(*rb.position()); - } - - if let Some(interpolated) = - interpolation.lerp_slerp((dt + sim_to_render_time.diff) / dt) - { - interpolated_pos = utils::iso_to_transform(&interpolated, scale); + if config.physics_pipeline_active { + for (entity, parent, transform, mut interpolation, mut velocity, mut sleeping) in + writeback.iter_mut() + { + // 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(handle) = world.entity2body.get(&entity).copied() { + if let Some(rb) = world.bodies.get(handle) { + let mut interpolated_pos = utils::iso_to_transform(rb.position(), scale); + + if let TimestepMode::Interpolated { dt, .. } = config.timestep_mode { + if let Some(interpolation) = interpolation.as_deref_mut() { + if interpolation.end.is_none() { + interpolation.end = Some(*rb.position()); + } + + if let Some(interpolated) = + interpolation.lerp_slerp((dt + sim_to_render_time.diff) / dt) + { + interpolated_pos = + utils::iso_to_transform(&interpolated, scale); + } } } - } - - if let Some(mut transform) = transform { - // NOTE: we query the parent’s global transform here, which is a bit - // unfortunate (performance-wise). An alternative would be to - // deduce the parent’s global transform from the current entity’s - // global transform. However, this makes it nearly impossible - // (because of rounding errors) to predict the exact next value this - // entity’s global transform will get after the next transform - // propagation, which breaks our transform modification detection - // that we do to detect if the user’s transform has to be written - // into the rigid-body. - if let Some(parent_global_transform) = - parent.and_then(|p| global_transforms.get(**p).ok()) - { - // We need to compute the new local transform such that: - // curr_parent_global_transform * new_transform = interpolated_pos - // new_transform = curr_parent_global_transform.inverse() * interpolated_pos - let (_, inverse_parent_rotation, inverse_parent_translation) = - parent_global_transform - .affine() - .inverse() - .to_scale_rotation_translation(); - let new_rotation = inverse_parent_rotation * interpolated_pos.rotation; - - #[allow(unused_mut)] // mut is needed in 2D but not in 3D. - let mut new_translation = inverse_parent_rotation - * interpolated_pos.translation - + inverse_parent_translation; - - // In 2D, preserve the transform `z` component that may have been set by the user - #[cfg(feature = "dim2")] - { - new_translation.z = transform.translation.z; - } - if transform.rotation != new_rotation - || transform.translation != new_translation + if let Some(mut transform) = transform { + // NOTE: we query the parent’s global transform here, which is a bit + // unfortunate (performance-wise). An alternative would be to + // deduce the parent’s global transform from the current entity’s + // global transform. However, this makes it nearly impossible + // (because of rounding errors) to predict the exact next value this + // entity’s global transform will get after the next transform + // propagation, which breaks our transform modification detection + // that we do to detect if the user’s transform has to be written + // into the rigid-body. + if let Some(parent_global_transform) = + parent.and_then(|p| global_transforms.get(**p).ok()) { - // NOTE: we write the new value only if there was an - // actual change, in order to not trigger bevy’s - // change tracking when the values didn’t change. - transform.rotation = new_rotation; - transform.translation = new_translation; + // We need to compute the new local transform such that: + // curr_parent_global_transform * new_transform = interpolated_pos + // new_transform = curr_parent_global_transform.inverse() * interpolated_pos + let (_, inverse_parent_rotation, inverse_parent_translation) = + parent_global_transform + .affine() + .inverse() + .to_scale_rotation_translation(); + let new_rotation = + inverse_parent_rotation * interpolated_pos.rotation; + + #[allow(unused_mut)] // mut is needed in 2D but not in 3D. + let mut new_translation = inverse_parent_rotation + * interpolated_pos.translation + + inverse_parent_translation; + + // In 2D, preserve the transform `z` component that may have been set by the user + #[cfg(feature = "dim2")] + { + new_translation.z = transform.translation.z; + } + + if transform.rotation != new_rotation + || transform.translation != new_translation + { + // NOTE: we write the new value only if there was an + // actual change, in order to not trigger bevy’s + // change tracking when the values didn’t change. + transform.rotation = new_rotation; + transform.translation = new_translation; + } + + // NOTE: we need to compute the result of the next transform propagation + // to make sure that our change detection for transforms is exact + // despite rounding errors. + let new_global_transform = + parent_global_transform.mul_transform(*transform); + + world + .last_body_transform_set + .insert(handle, new_global_transform); + } else { + // In 2D, preserve the transform `z` component that may have been set by the user + #[cfg(feature = "dim2")] + { + interpolated_pos.translation.z = transform.translation.z; + } + + if transform.rotation != interpolated_pos.rotation + || transform.translation != interpolated_pos.translation + { + // NOTE: we write the new value only if there was an + // actual change, in order to not trigger bevy’s + // change tracking when the values didn’t change. + transform.rotation = interpolated_pos.rotation; + transform.translation = interpolated_pos.translation; + } + + world + .last_body_transform_set + .insert(handle, GlobalTransform::from(interpolated_pos)); } - - // NOTE: we need to compute the result of the next transform propagation - // to make sure that our change detection for transforms is exact - // despite rounding errors. - let new_global_transform = - parent_global_transform.mul_transform(*transform); - - context - .last_body_transform_set - .insert(handle, new_global_transform); - } else { - // In 2D, preserve the transform `z` component that may have been set by the user - #[cfg(feature = "dim2")] - { - interpolated_pos.translation.z = transform.translation.z; - } - - if transform.rotation != interpolated_pos.rotation - || transform.translation != interpolated_pos.translation - { - // NOTE: we write the new value only if there was an - // actual change, in order to not trigger bevy’s - // change tracking when the values didn’t change. - transform.rotation = interpolated_pos.rotation; - transform.translation = interpolated_pos.translation; - } - - context - .last_body_transform_set - .insert(handle, GlobalTransform::from(interpolated_pos)); } - } - if let Some(velocity) = &mut velocity { - let new_vel = Velocity { - linvel: (rb.linvel() * scale).into(), - #[cfg(feature = "dim3")] - angvel: (*rb.angvel()).into(), - #[cfg(feature = "dim2")] - angvel: rb.angvel(), - }; - - // NOTE: we write the new value only if there was an - // actual change, in order to not trigger bevy’s - // change tracking when the values didn’t change. - if **velocity != new_vel { - **velocity = new_vel; + if let Some(velocity) = &mut velocity { + let new_vel = Velocity { + linvel: (rb.linvel() * scale).into(), + #[cfg(feature = "dim3")] + angvel: (*rb.angvel()).into(), + #[cfg(feature = "dim2")] + angvel: rb.angvel(), + }; + + // NOTE: we write the new value only if there was an + // actual change, in order to not trigger bevy’s + // change tracking when the values didn’t change. + if **velocity != new_vel { + **velocity = new_vel; + } } - } - if let Some(sleeping) = &mut sleeping { - // NOTE: we write the new value only if there was an - // actual change, in order to not trigger bevy’s - // change tracking when the values didn’t change. - if sleeping.sleeping != rb.is_sleeping() { - sleeping.sleeping = rb.is_sleeping(); + if let Some(sleeping) = &mut sleeping { + // NOTE: we write the new value only if there was an + // actual change, in order to not trigger bevy’s + // change tracking when the values didn’t change. + if sleeping.sleeping != rb.is_sleeping() { + sleeping.sleeping = rb.is_sleeping(); + } } } } @@ -640,26 +653,28 @@ pub fn step_simulation( PhysicsHooks: 'static + BevyPhysicsHooks, for<'w, 's> SystemParamItem<'w, 's, PhysicsHooks>: BevyPhysicsHooks, { - let context = &mut *context; - let hooks_adapter = BevyPhysicsHooksAdapter::::new(hooks); - - if config.physics_pipeline_active { - context.step_simulation( - config.gravity, - config.timestep_mode, - Some((collision_events, contact_force_events)), - &hooks_adapter, - &time, - &mut sim_to_render_time, - Some(interpolation_query), - ); - context.deleted_colliders.clear(); - } else { - context.propagate_modified_body_positions_to_colliders(); - } + for (_, world) in context.worlds.iter_mut() { + let context = &mut *context; + let hooks_adapter = BevyPhysicsHooksAdapter::::new(hooks); + + if config.physics_pipeline_active { + context.step_simulation( + config.gravity, + config.timestep_mode, + Some((collision_events, contact_force_events)), + &hooks_adapter, + &time, + &mut sim_to_render_time, + Some(interpolation_query), + ); + world.deleted_colliders.clear(); + } else { + context.propagate_modified_body_positions_to_colliders(); + } - if config.query_pipeline_active { - context.update_query_pipeline(); + if config.query_pipeline_active { + context.update_query_pipeline(); + } } } @@ -744,127 +759,130 @@ pub fn init_colliders( mut rigid_body_mprops: Query<&mut ReadMassProperties>, parent_query: Query<(&Parent, Option<&Transform>)>, ) { - let context = &mut *context; - let physics_scale = context.physics_scale; - - for ( - ( - entity, - shape, - sensor, - mprops, - active_events, - active_hooks, - active_collision_types, - friction, - restitution, - collision_groups, - solver_groups, - contact_force_event_threshold, - disabled, - ), - global_transform, - ) in colliders.iter() - { - let mut scaled_shape = shape.clone(); - scaled_shape.set_scale(shape.scale / physics_scale, config.scaled_shape_subdivision); - let mut builder = ColliderBuilder::new(scaled_shape.raw.clone()); - - builder = builder.sensor(sensor.is_some()); - builder = builder.enabled(disabled.is_none()); - - if let Some(mprops) = mprops { - builder = match mprops { - ColliderMassProperties::Density(density) => builder.density(*density), - ColliderMassProperties::Mass(mass) => builder.mass(*mass), - ColliderMassProperties::MassProperties(mprops) => { - builder.mass_properties(mprops.into_rapier(physics_scale)) - } - }; - } - - if let Some(active_events) = active_events { - builder = builder.active_events((*active_events).into()); - } + for (_, world) in context.worlds.iter_mut() { + let context = &mut *context; + let physics_scale = world.physics_scale; + + for ( + ( + entity, + shape, + sensor, + mprops, + active_events, + active_hooks, + active_collision_types, + friction, + restitution, + collision_groups, + solver_groups, + contact_force_event_threshold, + disabled, + ), + global_transform, + ) in colliders.iter() + { + let mut scaled_shape = shape.clone(); + scaled_shape.set_scale(shape.scale / physics_scale, config.scaled_shape_subdivision); + let mut builder = ColliderBuilder::new(scaled_shape.raw.clone()); + + builder = builder.sensor(sensor.is_some()); + builder = builder.enabled(disabled.is_none()); + + if let Some(mprops) = mprops { + builder = match mprops { + ColliderMassProperties::Density(density) => builder.density(*density), + ColliderMassProperties::Mass(mass) => builder.mass(*mass), + ColliderMassProperties::MassProperties(mprops) => { + builder.mass_properties(mprops.into_rapier(physics_scale)) + } + }; + } - if let Some(active_hooks) = active_hooks { - builder = builder.active_hooks((*active_hooks).into()); - } + if let Some(active_events) = active_events { + builder = builder.active_events((*active_events).into()); + } - if let Some(active_collision_types) = active_collision_types { - builder = builder.active_collision_types((*active_collision_types).into()); - } + if let Some(active_hooks) = active_hooks { + builder = builder.active_hooks((*active_hooks).into()); + } - if let Some(friction) = friction { - builder = builder - .friction(friction.coefficient) - .friction_combine_rule(friction.combine_rule.into()); - } + if let Some(active_collision_types) = active_collision_types { + builder = builder.active_collision_types((*active_collision_types).into()); + } - if let Some(restitution) = restitution { - builder = builder - .restitution(restitution.coefficient) - .restitution_combine_rule(restitution.combine_rule.into()); - } + if let Some(friction) = friction { + builder = builder + .friction(friction.coefficient) + .friction_combine_rule(friction.combine_rule.into()); + } - if let Some(collision_groups) = collision_groups { - builder = builder.collision_groups((*collision_groups).into()); - } + if let Some(restitution) = restitution { + builder = builder + .restitution(restitution.coefficient) + .restitution_combine_rule(restitution.combine_rule.into()); + } - if let Some(solver_groups) = solver_groups { - builder = builder.solver_groups((*solver_groups).into()); - } + if let Some(collision_groups) = collision_groups { + builder = builder.collision_groups((*collision_groups).into()); + } - if let Some(threshold) = contact_force_event_threshold { - builder = builder.contact_force_event_threshold(threshold.0); - } + if let Some(solver_groups) = solver_groups { + builder = builder.solver_groups((*solver_groups).into()); + } - let mut body_entity = entity; - let mut body_handle = context.entity2body.get(&body_entity).copied(); - let mut child_transform = Transform::default(); - while body_handle.is_none() { - if let Ok((parent_entity, transform)) = parent_query.get(body_entity) { - if let Some(transform) = transform { - child_transform = *transform * child_transform; - } - body_entity = parent_entity.get(); - } else { - break; + if let Some(threshold) = contact_force_event_threshold { + builder = builder.contact_force_event_threshold(threshold.0); } - body_handle = context.entity2body.get(&body_entity).copied(); - } + let mut body_entity = entity; + let mut body_handle = world.entity2body.get(&body_entity).copied(); + let mut child_transform = Transform::default(); + while body_handle.is_none() { + if let Ok((parent_entity, transform)) = parent_query.get(body_entity) { + if let Some(transform) = transform { + child_transform = *transform * child_transform; + } + body_entity = parent_entity.get(); + } else { + break; + } - builder = builder.user_data(entity.to_bits() as u128); + body_handle = world.entity2body.get(&body_entity).copied(); + } - let handle = if let Some(body_handle) = body_handle { - builder = builder.position(utils::transform_to_iso(&child_transform, physics_scale)); - let handle = - context - .colliders - .insert_with_parent(builder, body_handle, &mut context.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) { - mprops.0 = MassProperties::from_rapier( - parent_body.mass_properties().local_mprops, - physics_scale, - ); + 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, physics_scale)); + let handle = + world + .colliders + .insert_with_parent(builder, body_handle, &mut world.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) = world.bodies.get(body_handle) { + mprops.0 = MassProperties::from_rapier( + parent_body.mass_properties().local_mprops, + physics_scale, + ); + } } - } - handle - } else { - let global_transform = global_transform.cloned().unwrap_or_default(); - builder = builder.position(utils::transform_to_iso( - &global_transform.compute_transform(), - physics_scale, - )); - context.colliders.insert(builder) - }; + handle + } else { + let global_transform = global_transform.cloned().unwrap_or_default(); + builder = builder.position(utils::transform_to_iso( + &global_transform.compute_transform(), + physics_scale, + )); + world.colliders.insert(builder) + }; - commands.entity(entity).insert(RapierColliderHandle(handle)); - context.entity2collider.insert(entity, handle); + commands.entity(entity).insert(RapierColliderHandle(handle)); + world.entity2collider.insert(entity, handle); + } } } @@ -874,105 +892,107 @@ pub fn init_rigid_bodies( mut context: ResMut, rigid_bodies: Query>, ) { - let physics_scale = context.physics_scale; - - for ( - entity, - rb, - transform, - vel, - additional_mass_props, - _mass_props, - locked_axes, - force, - gravity_scale, - ccd, - dominance, - sleep, - damping, - disabled, - ) in rigid_bodies.iter() - { - let mut builder = RigidBodyBuilder::new((*rb).into()); - builder = builder.enabled(disabled.is_none()); - - if let Some(transform) = transform { - builder = builder.position(utils::transform_to_iso( - &transform.compute_transform(), - physics_scale, - )); - } + for (_, world) in context.worlds.iter_mut() { + let physics_scale = world.physics_scale; - #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled - if let Some(vel) = vel { - builder = builder - .linvel((vel.linvel / physics_scale).into()) - .angvel(vel.angvel.into()); - } + for ( + entity, + rb, + transform, + vel, + additional_mass_props, + _mass_props, + locked_axes, + force, + gravity_scale, + ccd, + dominance, + sleep, + damping, + disabled, + ) in rigid_bodies.iter() + { + let mut builder = RigidBodyBuilder::new((*rb).into()); + builder = builder.enabled(disabled.is_none()); - if let Some(locked_axes) = locked_axes { - builder = builder.locked_axes((*locked_axes).into()) - } + if let Some(transform) = transform { + builder = builder.position(utils::transform_to_iso( + &transform.compute_transform(), + physics_scale, + )); + } - if let Some(gravity_scale) = gravity_scale { - builder = builder.gravity_scale(gravity_scale.0); - } + #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled + if let Some(vel) = vel { + builder = builder + .linvel((vel.linvel / physics_scale).into()) + .angvel(vel.angvel.into()); + } - if let Some(ccd) = ccd { - builder = builder.ccd_enabled(ccd.enabled) - } + if let Some(locked_axes) = locked_axes { + builder = builder.locked_axes((*locked_axes).into()) + } - if let Some(dominance) = dominance { - builder = builder.dominance_group(dominance.groups) - } + if let Some(gravity_scale) = gravity_scale { + builder = builder.gravity_scale(gravity_scale.0); + } - if let Some(sleep) = sleep { - builder = builder.sleeping(sleep.sleeping); - } + if let Some(ccd) = ccd { + builder = builder.ccd_enabled(ccd.enabled) + } - if let Some(damping) = damping { - builder = builder - .linear_damping(damping.linear_damping) - .angular_damping(damping.angular_damping); - } + if let Some(dominance) = dominance { + builder = builder.dominance_group(dominance.groups) + } - if let Some(mprops) = additional_mass_props { - builder = match mprops { - AdditionalMassProperties::MassProperties(mprops) => { - builder.additional_mass_properties(mprops.into_rapier(physics_scale)) - } - AdditionalMassProperties::Mass(mass) => builder.additional_mass(*mass), - }; - } + if let Some(sleep) = sleep { + builder = builder.sleeping(sleep.sleeping); + } - builder = builder.user_data(entity.to_bits() as u128); + if let Some(damping) = damping { + builder = builder + .linear_damping(damping.linear_damping) + .angular_damping(damping.angular_damping); + } - let mut rb = builder.build(); + if let Some(mprops) = additional_mass_props { + builder = match mprops { + AdditionalMassProperties::MassProperties(mprops) => { + builder.additional_mass_properties(mprops.into_rapier(physics_scale)) + } + AdditionalMassProperties::Mass(mass) => builder.additional_mass(*mass), + }; + } - #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled - if let Some(force) = force { - rb.add_force((force.force / physics_scale).into(), false); - rb.add_torque(force.torque.into(), false); - } + builder = builder.user_data(entity.to_bits() as u128); - // NOTE: we can’t apply impulses yet at this point because - // the rigid-body’s mass isn’t up-to-date yet (its - // attached colliders, if any, haven’t been created yet). + let mut rb = builder.build(); - if let Some(sleep) = sleep { - let activation = rb.activation_mut(); - activation.linear_threshold = sleep.linear_threshold; - activation.angular_threshold = sleep.angular_threshold; - } + #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled + if let Some(force) = force { + rb.add_force((force.force / physics_scale).into(), false); + rb.add_torque(force.torque.into(), false); + } + + // NOTE: we can’t apply impulses yet at this point because + // the rigid-body’s mass isn’t up-to-date yet (its + // attached colliders, if any, haven’t been created yet). + + if let Some(sleep) = sleep { + let activation = rb.activation_mut(); + activation.linear_threshold = sleep.linear_threshold; + activation.angular_threshold = sleep.angular_threshold; + } - let handle = context.bodies.insert(rb); - commands - .entity(entity) - .insert(RapierRigidBodyHandle(handle)); - context.entity2body.insert(entity, handle); + let handle = world.bodies.insert(rb); + commands + .entity(entity) + .insert(RapierRigidBodyHandle(handle)); + world.entity2body.insert(entity, handle); - if let Some(transform) = transform { - context.last_body_transform_set.insert(handle, *transform); + if let Some(transform) = transform { + world.last_body_transform_set.insert(handle, *transform); + } } } } @@ -988,25 +1008,27 @@ pub fn apply_initial_rigid_body_impulses( // executed yet. mut init_impulses: Query<(Entity, &mut ExternalImpulse), Without>, ) { - let context = &mut *context; - let scale = context.physics_scale; - - for (entity, mut impulse) in init_impulses.iter_mut() { - let bodies = &mut context.bodies; - if let Some(rb) = context - .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); - // Apply the impulse. - rb.apply_impulse((impulse.impulse / scale).into(), false); + for (_, world) in context.worlds.iter_mut() { + let context = &mut *context; + let scale = world.physics_scale; + + for (entity, mut impulse) in init_impulses.iter_mut() { + let bodies = &mut world.bodies; + if let Some(rb) = world + .entity2body + .get(&entity) + .and_then(|h| bodies.get_mut(*h)) + { + // Make sure the mass-properties are computed. + rb.recompute_mass_properties_from_colliders(&world.colliders); + // Apply the impulse. + rb.apply_impulse((impulse.impulse / scale).into(), false); - #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled - rb.apply_torque_impulse(impulse.torque_impulse.into(), false); + #[allow(clippy::useless_conversion)] // Need to convert if dim3 enabled + rb.apply_torque_impulse(impulse.torque_impulse.into(), false); - impulse.reset(); + impulse.reset(); + } } } } @@ -1019,49 +1041,53 @@ pub fn init_joints( multibody_joints: Query<(Entity, &MultibodyJoint), Without>, parent_query: Query<&Parent>, ) { - let context = &mut *context; - let scale = context.physics_scale; - - for (entity, joint) in impulse_joints.iter() { - let mut target = None; - let mut body_entity = entity; - while target.is_none() { - target = context.entity2body.get(&body_entity).copied(); - if let Ok(parent_entity) = parent_query.get(body_entity) { - body_entity = parent_entity.get(); - } else { - break; + for (_, world) in context.worlds.iter_mut() { + let context = &mut *context; + let scale = world.physics_scale; + + for (entity, joint) in impulse_joints.iter() { + let mut target = None; + let mut body_entity = entity; + while target.is_none() { + target = world.entity2body.get(&body_entity).copied(); + if let Ok(parent_entity) = parent_query.get(body_entity) { + body_entity = parent_entity.get(); + } else { + break; + } } - } - if let (Some(target), Some(source)) = (target, context.entity2body.get(&joint.parent)) { - let handle = - context - .impulse_joints - .insert(*source, target, joint.data.into_rapier(scale), true); - commands - .entity(entity) - .insert(RapierImpulseJointHandle(handle)); - context.entity2impulse_joint.insert(entity, handle); + if let (Some(target), Some(source)) = (target, world.entity2body.get(&joint.parent)) { + let handle = world.impulse_joints.insert( + *source, + target, + joint.data.into_rapier(scale), + true, + ); + commands + .entity(entity) + .insert(RapierImpulseJointHandle(handle)); + world.entity2impulse_joint.insert(entity, handle); + } } - } - for (entity, joint) in multibody_joints.iter() { - let target = context.entity2body.get(&entity); + for (entity, joint) in multibody_joints.iter() { + let target = world.entity2body.get(&entity); - if let (Some(target), Some(source)) = (target, context.entity2body.get(&joint.parent)) { - if let Some(handle) = context.multibody_joints.insert( - *source, - *target, - joint.data.into_rapier(scale), - true, - ) { - commands - .entity(entity) - .insert(RapierMultibodyJointHandle(handle)); - context.entity2multibody_joint.insert(entity, handle); - } else { - error!("Failed to create multibody joint: loop detected.") + if let (Some(target), Some(source)) = (target, world.entity2body.get(&joint.parent)) { + if let Some(handle) = world.multibody_joints.insert( + *source, + *target, + joint.data.into_rapier(scale), + true, + ) { + commands + .entity(entity) + .insert(RapierMultibodyJointHandle(handle)); + world.entity2multibody_joint.insert(entity, handle); + } else { + error!("Failed to create multibody joint: loop detected.") + } } } } @@ -1089,125 +1115,127 @@ pub fn sync_removals( removed_rigid_body_disabled: RemovedComponents, removed_colliders_disabled: RemovedComponents, ) { - /* - * Rigid-bodies removal detection. - */ - let context = &mut *context; - for entity in removed_bodies.iter() { - if let Some(handle) = context.entity2body.remove(&entity) { - let _ = context.last_body_transform_set.remove(&handle); - context.bodies.remove( - handle, - &mut context.islands, - &mut context.colliders, - &mut context.impulse_joints, - &mut context.multibody_joints, - false, - ); + for (_, world) in context.worlds.iter_mut() { + /* + * Rigid-bodies removal detection. + */ + let context = &mut *context; + for entity in removed_bodies.iter() { + if let Some(handle) = world.entity2body.remove(&entity) { + let _ = world.last_body_transform_set.remove(&handle); + world.bodies.remove( + handle, + &mut world.islands, + &mut world.colliders, + &mut world.impulse_joints, + &mut world.multibody_joints, + false, + ); + } } - } - let context = &mut *context; - for entity in orphan_bodies.iter() { - if let Some(handle) = context.entity2body.remove(&entity) { - let _ = context.last_body_transform_set.remove(&handle); - context.bodies.remove( - handle, - &mut context.islands, - &mut context.colliders, - &mut context.impulse_joints, - &mut context.multibody_joints, - false, - ); + let context = &mut *context; + for entity in orphan_bodies.iter() { + if let Some(handle) = world.entity2body.remove(&entity) { + let _ = world.last_body_transform_set.remove(&handle); + world.bodies.remove( + handle, + &mut world.islands, + &mut world.colliders, + &mut world.impulse_joints, + &mut world.multibody_joints, + false, + ); + } + commands.entity(entity).remove::(); } - commands.entity(entity).remove::(); - } - /* - * Collider removal detection. - */ - for entity in removed_colliders.iter() { - if let Some(handle) = context.entity2collider.remove(&entity) { - context - .colliders - .remove(handle, &mut context.islands, &mut context.bodies, true); - context.deleted_colliders.insert(handle, entity); + /* + * Collider removal detection. + */ + for entity in removed_colliders.iter() { + if let Some(handle) = world.entity2collider.remove(&entity) { + world + .colliders + .remove(handle, &mut world.islands, &mut world.bodies, true); + world.deleted_colliders.insert(handle, entity); + } } - } - for entity in orphan_colliders.iter() { - if let Some(handle) = context.entity2collider.remove(&entity) { - context - .colliders - .remove(handle, &mut context.islands, &mut context.bodies, true); - context.deleted_colliders.insert(handle, entity); + for entity in orphan_colliders.iter() { + if let Some(handle) = world.entity2collider.remove(&entity) { + world + .colliders + .remove(handle, &mut world.islands, &mut world.bodies, true); + world.deleted_colliders.insert(handle, entity); + } + commands.entity(entity).remove::(); } - commands.entity(entity).remove::(); - } - /* - * Impulse joint removal detection. - */ - for entity in removed_impulse_joints.iter() { - if let Some(handle) = context.entity2impulse_joint.remove(&entity) { - context.impulse_joints.remove(handle, true); + /* + * Impulse joint removal detection. + */ + for entity in removed_impulse_joints.iter() { + if let Some(handle) = world.entity2impulse_joint.remove(&entity) { + world.impulse_joints.remove(handle, true); + } } - } - for entity in orphan_impulse_joints.iter() { - if let Some(handle) = context.entity2impulse_joint.remove(&entity) { - context.impulse_joints.remove(handle, true); + for entity in orphan_impulse_joints.iter() { + if let Some(handle) = world.entity2impulse_joint.remove(&entity) { + world.impulse_joints.remove(handle, true); + } + commands.entity(entity).remove::(); } - commands.entity(entity).remove::(); - } - /* - * Multibody joint removal detection. - */ - for entity in removed_multibody_joints.iter() { - if let Some(handle) = context.entity2multibody_joint.remove(&entity) { - context.multibody_joints.remove(handle, true); + /* + * Multibody joint removal detection. + */ + for entity in removed_multibody_joints.iter() { + if let Some(handle) = world.entity2multibody_joint.remove(&entity) { + world.multibody_joints.remove(handle, true); + } } - } - for entity in orphan_multibody_joints.iter() { - if let Some(handle) = context.entity2multibody_joint.remove(&entity) { - context.multibody_joints.remove(handle, true); + for entity in orphan_multibody_joints.iter() { + if let Some(handle) = world.entity2multibody_joint.remove(&entity) { + world.multibody_joints.remove(handle, true); + } + commands + .entity(entity) + .remove::(); } - commands - .entity(entity) - .remove::(); - } - /* - * Marker components removal detection. - */ - for entity in removed_sensors.iter() { - if let Some(handle) = context.entity2collider.get(&entity) { - if let Some(co) = context.colliders.get_mut(*handle) { - co.set_sensor(false); + /* + * Marker components removal detection. + */ + for entity in removed_sensors.iter() { + if let Some(handle) = world.entity2collider.get(&entity) { + if let Some(co) = world.colliders.get_mut(*handle) { + co.set_sensor(false); + } } } - } - for entity in removed_colliders_disabled.iter() { - if let Some(handle) = context.entity2collider.get(&entity) { - if let Some(co) = context.colliders.get_mut(*handle) { - co.set_enabled(true); + for entity in removed_colliders_disabled.iter() { + if let Some(handle) = world.entity2collider.get(&entity) { + if let Some(co) = world.colliders.get_mut(*handle) { + co.set_enabled(true); + } } } - } - for entity in removed_rigid_body_disabled.iter() { - if let Some(handle) = context.entity2body.get(&entity) { - if let Some(rb) = context.bodies.get_mut(*handle) { - rb.set_enabled(true); + for entity in removed_rigid_body_disabled.iter() { + if let Some(handle) = world.entity2body.get(&entity) { + if let Some(rb) = world.bodies.get_mut(*handle) { + rb.set_enabled(true); + } } } - } - // TODO: update mass props after collider removal. - // TODO: what about removing forces? + // TODO: update mass props after collider removal. + // TODO: what about removing forces? + } } /// Adds entity to [`CollidingEntities`] on starting collision and removes from it when the @@ -1254,147 +1282,150 @@ pub fn update_character_controls( )>, mut transforms: Query<&mut Transform>, ) { - let physics_scale = context.physics_scale; - let context = &mut *context; - for (entity, mut controller, output, collider_handle, body_handle, glob_transform) in - character_controllers.iter_mut() - { - if let (Some(raw_controller), Some(translation)) = - (controller.to_raw(physics_scale), controller.translation) + for (_, world) in context.worlds.iter_mut() { + let physics_scale = world.physics_scale; + let context = &mut *context; + for (entity, mut controller, output, collider_handle, body_handle, glob_transform) in + character_controllers.iter_mut() { - let scaled_custom_shape = - controller - .custom_shape - .as_ref() - .map(|(custom_shape, tra, rot)| { - // TODO: avoid the systematic scale somehow? - let mut scaled_shape = custom_shape.clone(); - scaled_shape.set_scale( - custom_shape.scale / physics_scale, - config.scaled_shape_subdivision, - ); - - (scaled_shape, *tra / physics_scale, *rot) - }); - - let parent_rigid_body = body_handle.map(|h| h.0).or_else(|| { - collider_handle - .and_then(|h| context.colliders.get(h.0)) - .and_then(|c| c.parent()) - }); - let entity_to_move = parent_rigid_body - .and_then(|rb| context.rigid_body_entity(rb)) - .unwrap_or(entity); - - let (character_shape, character_pos) = if let Some((scaled_shape, tra, rot)) = - &scaled_custom_shape + if let (Some(raw_controller), Some(translation)) = + (controller.to_raw(physics_scale), controller.translation) { - let mut shape_pos: Isometry = (*tra, *rot).into(); - - if let Some(body) = body_handle.and_then(|h| context.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(), physics_scale) - * shape_pos - } - - (&*scaled_shape.raw, shape_pos) - } else if let Some(collider) = collider_handle.and_then(|h| context.colliders.get(h.0)) - { - (collider.shape(), *collider.position()) - } else { - continue; - }; + let scaled_custom_shape = + controller + .custom_shape + .as_ref() + .map(|(custom_shape, tra, rot)| { + // TODO: avoid the systematic scale somehow? + let mut scaled_shape = custom_shape.clone(); + scaled_shape.set_scale( + custom_shape.scale / physics_scale, + config.scaled_shape_subdivision, + ); + + (scaled_shape, *tra / physics_scale, *rot) + }); + + let parent_rigid_body = body_handle.map(|h| h.0).or_else(|| { + collider_handle + .and_then(|h| world.colliders.get(h.0)) + .and_then(|c| c.parent()) + }); + let entity_to_move = parent_rigid_body + .and_then(|rb| context.rigid_body_entity(rb)) + .unwrap_or(entity); + + let (character_shape, character_pos) = if let Some((scaled_shape, tra, rot)) = + &scaled_custom_shape + { + let mut shape_pos: Isometry = (*tra, *rot).into(); + + if let Some(body) = body_handle.and_then(|h| world.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(), physics_scale) + * shape_pos + } - let exclude_collider = collider_handle.map(|h| h.0); - - let character_mass = controller - .custom_mass - .or_else(|| { - parent_rigid_body - .and_then(|h| context.bodies.get(h)) - .map(|rb| rb.mass()) - }) - .unwrap_or(0.0); - - let mut filter = QueryFilter { - flags: controller.filter_flags, - groups: controller.filter_groups.map(|g| g.into()), - exclude_collider: None, - exclude_rigid_body: None, - predicate: None, - }; + (&*scaled_shape.raw, shape_pos) + } else if let Some(collider) = + collider_handle.and_then(|h| world.colliders.get(h.0)) + { + (collider.shape(), *collider.position()) + } else { + continue; + }; - if let Some(parent) = parent_rigid_body { - filter = filter.exclude_rigid_body(parent); - } else if let Some(excl_co) = exclude_collider { - filter = filter.exclude_collider(excl_co) - }; + let exclude_collider = collider_handle.map(|h| h.0); + + let character_mass = controller + .custom_mass + .or_else(|| { + parent_rigid_body + .and_then(|h| world.bodies.get(h)) + .map(|rb| rb.mass()) + }) + .unwrap_or(0.0); + + let mut filter = QueryFilter { + flags: controller.filter_flags, + groups: controller.filter_groups.map(|g| g.into()), + exclude_collider: None, + exclude_rigid_body: None, + predicate: None, + }; - let collisions = &mut context.character_collisions_collector; - collisions.clear(); - - let movement = raw_controller.move_shape( - context.integration_parameters.dt, - &context.bodies, - &context.colliders, - &context.query_pipeline, - character_shape, - &character_pos, - (translation / physics_scale).into(), - filter, - |c| collisions.push(c), - ); + if let Some(parent) = parent_rigid_body { + filter = filter.exclude_rigid_body(parent); + } else if let Some(excl_co) = exclude_collider { + filter = filter.exclude_collider(excl_co) + }; - if controller.apply_impulse_to_dynamic_bodies { - for collision in &*collisions { - raw_controller.solve_character_collision_impulses( - context.integration_parameters.dt, - &mut context.bodies, - &context.colliders, - &context.query_pipeline, - character_shape, - character_mass, - collision, - filter, - ) + let collisions = &mut world.character_collisions_collector; + collisions.clear(); + + let movement = raw_controller.move_shape( + world.integration_parameters.dt, + &world.bodies, + &world.colliders, + &world.query_pipeline, + character_shape, + &character_pos, + (translation / physics_scale).into(), + filter, + |c| collisions.push(c), + ); + + if controller.apply_impulse_to_dynamic_bodies { + for collision in &*collisions { + raw_controller.solve_character_collision_impulses( + world.integration_parameters.dt, + &mut world.bodies, + &world.colliders, + &world.query_pipeline, + character_shape, + character_mass, + collision, + filter, + ) + } } - } - if let Ok(mut transform) = transforms.get_mut(entity_to_move) { - // TODO: take the parent’s GlobalTransform rotation into account? - transform.translation.x += movement.translation.x * physics_scale; - transform.translation.y += movement.translation.y * physics_scale; - #[cfg(feature = "dim3")] - { - transform.translation.z += movement.translation.z * physics_scale; + if let Ok(mut transform) = transforms.get_mut(entity_to_move) { + // TODO: take the parent’s GlobalTransform rotation into account? + transform.translation.x += movement.translation.x * physics_scale; + transform.translation.y += movement.translation.y * physics_scale; + #[cfg(feature = "dim3")] + { + transform.translation.z += movement.translation.z * physics_scale; + } } - } - let converted_collisions = context - .character_collisions_collector - .iter() - .filter_map(|c| CharacterCollision::from_raw(context, c)); + let converted_collisions = world + .character_collisions_collector + .iter() + .filter_map(|c| CharacterCollision::from_raw(world, c)); + + if let Some(mut output) = output { + output.desired_translation = controller.translation.unwrap(); // Already takes the physics_scale into account. + output.effective_translation = (movement.translation * physics_scale).into(); + output.grounded = movement.grounded; + output.collisions.clear(); + output.collisions.extend(converted_collisions); + } else { + commands + .entity(entity) + .insert(KinematicCharacterControllerOutput { + desired_translation: controller.translation.unwrap(), // Already takes the physics_scale into account. + effective_translation: (movement.translation * physics_scale).into(), + grounded: movement.grounded, + collisions: converted_collisions.collect(), + }); + } - if let Some(mut output) = output { - output.desired_translation = controller.translation.unwrap(); // Already takes the physics_scale into account. - output.effective_translation = (movement.translation * physics_scale).into(); - output.grounded = movement.grounded; - output.collisions.clear(); - output.collisions.extend(converted_collisions); - } else { - commands - .entity(entity) - .insert(KinematicCharacterControllerOutput { - desired_translation: controller.translation.unwrap(), // Already takes the physics_scale into account. - effective_translation: (movement.translation * physics_scale).into(), - grounded: movement.grounded, - collisions: converted_collisions.collect(), - }); + controller.translation = None; } - - controller.translation = None; } } } @@ -1415,7 +1446,7 @@ mod tests { use std::f32::consts::PI; use super::*; - use crate::plugin::{NoUserData, RapierPhysicsPlugin}; + use crate::plugin::{context::DEFAULT_WORLD_ID, NoUserData, RapierPhysicsPlugin}; #[test] fn colliding_entities_updates() { @@ -1617,10 +1648,14 @@ mod tests { let child_transform = app.world.entity(child).get::().unwrap(); let context = app.world.resource::(); - let child_handle = context.entity2body[&child]; - let child_body = context.bodies.get(child_handle).unwrap(); + let world = context + .get_world(DEFAULT_WORLD_ID) + .expect("The default world should exist."); + + let child_handle = world.entity2body[&child]; + let child_body = world.bodies.get(child_handle).unwrap(); let body_transform = - utils::iso_to_transform(child_body.position(), context.physics_scale); + utils::iso_to_transform(child_body.position(), world.physics_scale); assert_eq!( GlobalTransform::from(body_transform), *child_transform, @@ -1678,12 +1713,16 @@ mod tests { .unwrap() .compute_transform(); let context = app.world.resource::(); - let parent_handle = context.entity2body[&parent]; - let parent_body = context.bodies.get(parent_handle).unwrap(); + let world = context + .get_world(DEFAULT_WORLD_ID) + .expect("The default world should exist."); + + let parent_handle = world.entity2body[&parent]; + let parent_body = world.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 = world.colliders.get(child_collider_handle).unwrap(); let body_transform = - utils::iso_to_transform(child_collider.position(), context.physics_scale); + utils::iso_to_transform(child_collider.position(), world.physics_scale); approx::assert_relative_eq!( body_transform.translation, child_transform.translation, diff --git a/src/render/mod.rs b/src/render/mod.rs index 41ce7dab0..d11a9ee1d 100644 --- a/src/render/mod.rs +++ b/src/render/mod.rs @@ -1,3 +1,4 @@ +use crate::plugin::context::RapierWorld; use crate::plugin::RapierContext; use crate::render::lines::DebugLinesConfig; use bevy::prelude::*; @@ -114,13 +115,14 @@ struct BevyLinesRenderBackend<'world, 'state, 'a, 'b, 'c> { physics_scale: f32, custom_colors: Query<'world, 'state, &'a ColliderDebugColor>, context: &'b RapierContext, + world: &'b RapierWorld, lines: &'c mut DebugLines, } impl<'world, 'state, 'a, 'b, 'c> BevyLinesRenderBackend<'world, 'state, 'a, 'b, 'c> { 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| { + DebugRenderObject::Collider(h, ..) => self.world.colliders.get(h).and_then(|co| { self.custom_colors .get(Entity::from_bits(co.user_data as u64)) .map(|co| co.0) @@ -180,27 +182,30 @@ fn debug_render_scene( mut lines: ResMut, custom_colors: Query<&ColliderDebugColor>, ) { - if !render_context.enabled { - return; - } + for (_, world) in rapier_context.worlds { + if !render_context.enabled { + return; + } - *lines_config.always_on_top.write().unwrap() = render_context.always_on_top; - let mut backend = BevyLinesRenderBackend { - physics_scale: rapier_context.physics_scale, - custom_colors, - context: &rapier_context, - lines: &mut lines, - }; - - let unscaled_style = render_context.pipeline.style; - render_context.pipeline.style.rigid_body_axes_length /= rapier_context.physics_scale; - render_context.pipeline.render( - &mut backend, - &rapier_context.bodies, - &rapier_context.colliders, - &rapier_context.impulse_joints, - &rapier_context.multibody_joints, - &rapier_context.narrow_phase, - ); - render_context.pipeline.style = unscaled_style; + *lines_config.always_on_top.write().unwrap() = render_context.always_on_top; + let mut backend = BevyLinesRenderBackend { + physics_scale: world.physics_scale, + world: &world, + custom_colors, + context: &rapier_context, + lines: &mut lines, + }; + + let unscaled_style = render_context.pipeline.style; + render_context.pipeline.style.rigid_body_axes_length /= world.physics_scale; + render_context.pipeline.render( + &mut backend, + &world.bodies, + &world.colliders, + &world.impulse_joints, + &world.multibody_joints, + &world.narrow_phase, + ); + render_context.pipeline.style = unscaled_style; + } } From 2010f9428875f39d518c7b8b3a494234d72fe45a Mon Sep 17 00:00:00 2001 From: AnthonyTornetta Date: Mon, 13 Feb 2023 13:23:17 -0500 Subject: [PATCH 03/92] Adding example + reworking to be faster. Disabled custom events for now --- bevy_rapier3d/examples/multi_world3.rs | 66 +++++ bevy_rapier3d/examples/ray_casting3.rs | 19 +- bevy_rapier3d/examples/static_trimesh3.rs | 6 +- src/dynamics/rigid_body.rs | 10 +- src/lib.rs | 2 + src/pipeline/events.rs | 10 +- src/plugin/context.rs | 45 +++- src/plugin/plugin.rs | 3 +- src/plugin/systems.rs | 307 +++++++++++++++------- src/render/mod.rs | 14 +- 10 files changed, 345 insertions(+), 137 deletions(-) create mode 100644 bevy_rapier3d/examples/multi_world3.rs diff --git a/bevy_rapier3d/examples/multi_world3.rs b/bevy_rapier3d/examples/multi_world3.rs new file mode 100644 index 000000000..6083cfe08 --- /dev/null +++ b/bevy_rapier3d/examples/multi_world3.rs @@ -0,0 +1,66 @@ +use bevy::prelude::*; +use bevy_rapier3d::prelude::*; + +fn main() { + App::new() + .insert_resource(ClearColor(Color::rgb( + 0xF9 as f32 / 255.0, + 0xF9 as f32 / 255.0, + 0xFF as f32 / 255.0, + ))) + .add_plugins(DefaultPlugins) + .add_plugin(RapierPhysicsPlugin::::default()) + .add_plugin(RapierDebugRenderPlugin::default()) + .add_startup_system(setup_graphics) + .add_startup_system(setup_physics) + .run(); +} + +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() + }); +} + +const N_WORLDS: usize = 2; + +pub fn setup_physics(mut commands: Commands) { + for i in 0..N_WORLDS { + let color = [Color::hsl(220.0, 1.0, 0.3), Color::hsl(180.0, 1.0, 0.3)][i % 2]; + + /* + * Ground + */ + let ground_size = 20.1; + let ground_height = 0.1; + + commands.spawn(( + TransformBundle::from(Transform::from_xyz( + 0.0, + (i as f32) * -2.0 - ground_height, + 0.0, + )), + Collider::cuboid(ground_size, ground_height, ground_size), + ColliderDebugColor(color), + )); + + /* + * Create the cube + */ + + commands + .spawn(TransformBundle::from(Transform::from_rotation( + Quat::from_rotation_x(0.2), + ))) + .with_children(|child| { + child.spawn(( + TransformBundle::from(Transform::from_xyz(0.0, ((1 + i) * 5) as f32, 0.0)), + RigidBody::Dynamic, + Collider::cuboid(0.5, 0.5, 0.5), + ColliderDebugColor(color), + )); + }); + } +} diff --git a/bevy_rapier3d/examples/ray_casting3.rs b/bevy_rapier3d/examples/ray_casting3.rs index 5490bc72d..57107cd2a 100644 --- a/bevy_rapier3d/examples/ray_casting3.rs +++ b/bevy_rapier3d/examples/ray_casting3.rs @@ -1,5 +1,5 @@ use bevy::prelude::*; -use bevy_rapier3d::prelude::*; +use bevy_rapier3d::prelude::{DEFAULT_WORLD_ID, *}; fn main() { App::new() @@ -83,13 +83,16 @@ fn cast_ray( ray_from_mouse_position(windows.get_primary().unwrap(), camera, camera_transform); // Then cast the ray. - let hit = rapier_context.cast_ray( - ray_pos, - ray_dir, - f32::MAX, - true, - QueryFilter::only_dynamic(), - ); + let hit = rapier_context + .cast_ray( + DEFAULT_WORLD_ID, + ray_pos, + ray_dir, + f32::MAX, + true, + QueryFilter::only_dynamic(), + ) + .expect("Default world should exist."); if let Some((entity, _toi)) = hit { // Color in blue the entity we just hit. diff --git a/bevy_rapier3d/examples/static_trimesh3.rs b/bevy_rapier3d/examples/static_trimesh3.rs index 4b926bfdb..0cd8e217c 100644 --- a/bevy_rapier3d/examples/static_trimesh3.rs +++ b/bevy_rapier3d/examples/static_trimesh3.rs @@ -128,7 +128,11 @@ fn ball_spawner( // NOTE: The timing here only works properly with `time_dependent_number_of_timesteps` // disabled, as it is for examples. - ball_state.seconds_until_next_spawn -= rapier_context.integration_parameters.dt; + ball_state.seconds_until_next_spawn -= rapier_context + .get_world(DEFAULT_WORLD_ID) + .expect("Default world should exist.") + .integration_parameters + .dt; if ball_state.seconds_until_next_spawn > 0.0 { return; } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9472ebaad..f4f63f35f 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1,4 +1,4 @@ -use crate::math::Vect; +use crate::{math::Vect, plugin::context::WorldId}; use bevy::{prelude::*, reflect::FromReflect}; use rapier::prelude::{ Isometry, LockedAxes as RapierLockedAxes, RigidBodyActivation, RigidBodyHandle, RigidBodyType, @@ -400,6 +400,14 @@ impl Default for GravityScale { } } +/// Denotes which world this body is a part of. If omitted, the default world is assumed. +#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, Component, Reflect, FromReflect)] +#[reflect(Component, PartialEq)] +pub struct BodyWorld { + /// The world which this body is in + pub world_id: WorldId, +} + /// Information used for Continuous-Collision-Detection. #[derive(Copy, Clone, Debug, Default, PartialEq, Eq, Component, Reflect, FromReflect)] #[reflect(Component, PartialEq)] diff --git a/src/lib.rs b/src/lib.rs index 5f8c594b0..2361e139c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,4 +76,6 @@ pub mod prelude { pub use crate::plugin::*; #[cfg(any(feature = "debug-render-3d", feature = "debug-render-2d"))] pub use crate::render::*; + + pub use crate::prelude::context::DEFAULT_WORLD_ID; } diff --git a/src/pipeline/events.rs b/src/pipeline/events.rs index 61678579e..c1ea034a9 100644 --- a/src/pipeline/events.rs +++ b/src/pipeline/events.rs @@ -45,15 +45,15 @@ pub struct ContactForceEvent { // investigated how to reproduce this exactly to open an // issue). /// A set of queues collecting events emitted by the physics engine. -pub(crate) struct EventQueue<'a> { +pub(crate) struct EventQueue<'a, 'b> { // Used ot retrieve the entity of colliders that have been removed from the simulation // since the last physics step. pub deleted_colliders: &'a HashMap, - pub collision_events: RwLock>, - pub contact_force_events: RwLock>, + pub collision_events: RwLock<&'b mut EventWriter<'b, 'b, CollisionEvent>>, + pub contact_force_events: RwLock<&'b mut EventWriter<'b, 'b, ContactForceEvent>>, } -impl<'a> EventQueue<'a> { +impl<'a, 'b> EventQueue<'a, 'b> { fn collider2entity(&self, colliders: &ColliderSet, handle: ColliderHandle) -> Entity { colliders .get(handle) @@ -63,7 +63,7 @@ impl<'a> EventQueue<'a> { } } -impl<'a> EventHandler for EventQueue<'a> { +impl<'a, 'b> EventHandler for EventQueue<'a, 'b> { fn handle_collision_event( &self, _bodies: &RigidBodySet, diff --git a/src/plugin/context.rs b/src/plugin/context.rs index 582b6cd25..49d302d2a 100644 --- a/src/plugin/context.rs +++ b/src/plugin/context.rs @@ -24,8 +24,9 @@ use crate::prelude::{CollisionGroups, RapierRigidBodyHandle}; use bevy::math::Vec3Swizzles; use rapier::control::CharacterAutostep; -type WorldId = usize; +pub type WorldId = usize; +/// This world id is the default world that is created. This world cannot be removed. pub const DEFAULT_WORLD_ID: WorldId = 0; /// The Rapier context, containing all the state of the physics engine. @@ -132,28 +133,40 @@ impl RapierWorld { .map(|c| Entity::from_bits(c.user_data as u64)) } - fn step_simulation( + pub fn step_simulation<'a>( &mut self, gravity: Vect, timestep_mode: TimestepMode, - events: Option<(EventWriter, EventWriter)>, + // events: &'a mut Option<( + // &'a mut EventWriter, + // &'a mut EventWriter, + // )>, hooks: &dyn PhysicsHooks, time: &Time, sim_to_render_time: &mut SimulationToRenderTime, interpolation_query: &mut Option< - Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, + &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, >, ) { - let event_queue = events.map(|(ce, fe)| EventQueue { - deleted_colliders: &self.deleted_colliders, - collision_events: RwLock::new(ce), - contact_force_events: RwLock::new(fe), - }); + // let event_queue = match events { + // Some((ce, fe)) => Some(EventQueue { + // deleted_colliders: &self.deleted_colliders, + // collision_events: RwLock::new(*ce), + // contact_force_events: RwLock::new(*fe), + // }), + // None => None, + // }; + + //events.map(|(ce, fe)| EventQueue { + // deleted_colliders: &self.deleted_colliders, + // collision_events: RwLock::new(ce), + // contact_force_events: RwLock::new(fe), + // }); let events = self .event_handler .as_deref() - .or_else(|| event_queue.as_ref().map(|q| q as &dyn EventHandler)) + // .or_else(|| event_queue.as_ref().map(|q| q as &dyn EventHandler)) .unwrap_or(&() as &dyn EventHandler); match timestep_mode { @@ -866,6 +879,12 @@ impl RapierContext { .ok_or(WorldError::WorldNotFound { world_id }) } + pub fn get_world_mut(&mut self, world_id: WorldId) -> Result<&mut RapierWorld, WorldError> { + self.worlds + .get_mut(&world_id) + .ok_or(WorldError::WorldNotFound { world_id }) + } + pub fn physics_scale(&self, world_id: WorldId) -> Option { self.worlds.get(&world_id).map(|x| x.physics_scale) } @@ -957,7 +976,7 @@ impl RapierContext { /// Advance the simulation, based on the given timestep mode. #[allow(clippy::too_many_arguments)] pub fn step_simulation( - &mut self, + mut self, gravity: Vect, timestep_mode: TimestepMode, events: Option<(EventWriter, EventWriter)>, @@ -965,14 +984,14 @@ impl RapierContext { time: &Time, sim_to_render_time: &mut SimulationToRenderTime, mut interpolation_query: Option< - Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, + &mut Query<(&RapierRigidBodyHandle, &mut TransformInterpolation)>, >, ) { for (_, world) in self.worlds.iter_mut() { world.step_simulation( gravity, timestep_mode, - events, + // &mut events.as_mut().map(|(ce, fe)| (ce, fe)), hooks, time, sim_to_render_time, diff --git a/src/plugin/plugin.rs b/src/plugin/plugin.rs index 2b767fc82..4d9bea250 100644 --- a/src/plugin/plugin.rs +++ b/src/plugin/plugin.rs @@ -196,7 +196,8 @@ where .register_type::() .register_type::() .register_type::() - .register_type::(); + .register_type::() + .register_type::(); // Insert all of our required resources. Don’t overwrite // the `RapierConfiguration` if it already exists. diff --git a/src/plugin/systems.rs b/src/plugin/systems.rs index 68a022320..b2cfbfd34 100644 --- a/src/plugin/systems.rs +++ b/src/plugin/systems.rs @@ -15,8 +15,8 @@ use crate::pipeline::{CollisionEvent, ContactForceEvent}; use crate::plugin::configuration::{SimulationToRenderTime, TimestepMode}; use crate::plugin::{RapierConfiguration, RapierContext}; use crate::prelude::{ - BevyPhysicsHooks, BevyPhysicsHooksAdapter, CollidingEntities, KinematicCharacterController, - KinematicCharacterControllerOutput, RigidBodyDisabled, + BevyPhysicsHooks, BevyPhysicsHooksAdapter, BodyWorld, CollidingEntities, + KinematicCharacterController, KinematicCharacterControllerOutput, RigidBodyDisabled, }; use crate::utils; use bevy::ecs::system::SystemParamItem; @@ -34,6 +34,8 @@ use crate::control::CharacterCollision; #[cfg(feature = "dim2")] use bevy::math::Vec3Swizzles; +use super::context::{RapierWorld, DEFAULT_WORLD_ID}; + /// Components that will be updated after a physics step. pub type RigidBodyWritebackComponents<'a> = ( Entity, @@ -122,126 +124,236 @@ pub fn apply_collider_user_changes( config: Res, mut context: ResMut, changed_collider_transforms: Query< - (&RapierColliderHandle, &GlobalTransform), + (&RapierColliderHandle, &GlobalTransform, Option<&BodyWorld>), (Without, Changed), >, - changed_shapes: Query<(&RapierColliderHandle, &Collider), Changed>, - changed_active_events: Query<(&RapierColliderHandle, &ActiveEvents), Changed>, - changed_active_hooks: Query<(&RapierColliderHandle, &ActiveHooks), Changed>, + changed_shapes: Query< + (&RapierColliderHandle, &Collider, Option<&BodyWorld>), + Changed, + >, + changed_active_events: Query< + (&RapierColliderHandle, &ActiveEvents, Option<&BodyWorld>), + Changed, + >, + changed_active_hooks: Query< + (&RapierColliderHandle, &ActiveHooks, Option<&BodyWorld>), + Changed, + >, changed_active_collision_types: Query< - (&RapierColliderHandle, &ActiveCollisionTypes), + ( + &RapierColliderHandle, + &ActiveCollisionTypes, + Option<&BodyWorld>, + ), Changed, >, - changed_friction: Query<(&RapierColliderHandle, &Friction), Changed>, - changed_restitution: Query<(&RapierColliderHandle, &Restitution), Changed>, + changed_friction: Query< + (&RapierColliderHandle, &Friction, Option<&BodyWorld>), + Changed, + >, + changed_restitution: Query< + (&RapierColliderHandle, &Restitution, Option<&BodyWorld>), + Changed, + >, changed_collision_groups: Query< - (&RapierColliderHandle, &CollisionGroups), + (&RapierColliderHandle, &CollisionGroups, Option<&BodyWorld>), Changed, >, - changed_solver_groups: Query<(&RapierColliderHandle, &SolverGroups), Changed>, - changed_sensors: Query<(&RapierColliderHandle, &Sensor), Changed>, - changed_disabled: Query<(&RapierColliderHandle, &ColliderDisabled), Changed>, + changed_solver_groups: Query< + (&RapierColliderHandle, &SolverGroups, Option<&BodyWorld>), + Changed, + >, + changed_sensors: Query<(&RapierColliderHandle, &Sensor, Option<&BodyWorld>), Changed>, + changed_disabled: Query< + (&RapierColliderHandle, &ColliderDisabled, Option<&BodyWorld>), + Changed, + >, changed_contact_force_threshold: Query< - (&RapierColliderHandle, &ContactForceEventThreshold), + ( + &RapierColliderHandle, + &ContactForceEventThreshold, + Option<&BodyWorld>, + ), Changed, >, changed_collider_mass_props: Query< - (&RapierColliderHandle, &ColliderMassProperties), + ( + &RapierColliderHandle, + &ColliderMassProperties, + Option<&BodyWorld>, + ), Changed, >, ) { - for (_, world) in context.worlds.iter_mut() { - let scale = world.physics_scale; + for (handle, transform, world_within) in changed_collider_transforms.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); - for (handle, transform) in changed_collider_transforms.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - if co.parent().is_none() { - co.set_position(utils::transform_to_iso( - &transform.compute_transform(), - scale, - )) - } + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + if co.parent().is_none() { + co.set_position(utils::transform_to_iso( + &transform.compute_transform(), + world.physics_scale, + )) } } + } - for (handle, shape) in changed_shapes.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - let mut scaled_shape = shape.clone(); - scaled_shape.set_scale(shape.scale / scale, config.scaled_shape_subdivision); - co.set_shape(scaled_shape.raw.clone()) - } + for (handle, shape, world_within) in changed_shapes.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + let mut scaled_shape = shape.clone(); + scaled_shape.set_scale( + shape.scale / world.physics_scale, + config.scaled_shape_subdivision, + ); + co.set_shape(scaled_shape.raw.clone()) } + } - for (handle, active_events) in changed_active_events.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_active_events((*active_events).into()) - } + for (handle, active_events, world_within) in changed_active_events.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_active_events((*active_events).into()) } + } - for (handle, active_hooks) in changed_active_hooks.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_active_hooks((*active_hooks).into()) - } + for (handle, active_hooks, world_within) in changed_active_hooks.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_active_hooks((*active_hooks).into()) } + } - for (handle, active_collision_types) in changed_active_collision_types.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_active_collision_types((*active_collision_types).into()) - } + for (handle, active_collision_types, world_within) in changed_active_collision_types.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_active_collision_types((*active_collision_types).into()) } + } - for (handle, friction) in changed_friction.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_friction(friction.coefficient); - co.set_friction_combine_rule(friction.combine_rule.into()); - } + for (handle, friction, world_within) in changed_friction.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_friction(friction.coefficient); + co.set_friction_combine_rule(friction.combine_rule.into()); } + } - for (handle, restitution) in changed_restitution.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_restitution(restitution.coefficient); - co.set_restitution_combine_rule(restitution.combine_rule.into()); - } + for (handle, restitution, world_within) in changed_restitution.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_restitution(restitution.coefficient); + co.set_restitution_combine_rule(restitution.combine_rule.into()); } + } - for (handle, collision_groups) in changed_collision_groups.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_collision_groups((*collision_groups).into()); - } + for (handle, collision_groups, world_within) in changed_collision_groups.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_collision_groups((*collision_groups).into()); } + } - for (handle, solver_groups) in changed_solver_groups.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_solver_groups((*solver_groups).into()); - } + for (handle, solver_groups, world_within) in changed_solver_groups.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_solver_groups((*solver_groups).into()); } + } - for (handle, _) in changed_sensors.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_sensor(true); - } + for (handle, _, world_within) in changed_sensors.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_sensor(true); } + } - for (handle, _) in changed_disabled.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_enabled(false); - } + for (handle, _, world_within) in changed_disabled.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_enabled(false); } + } - for (handle, threshold) in changed_contact_force_threshold.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - co.set_contact_force_event_threshold(threshold.0); - } + for (handle, threshold, world_within) in changed_contact_force_threshold.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + co.set_contact_force_event_threshold(threshold.0); } + } - for (handle, mprops) in changed_collider_mass_props.iter() { - if let Some(co) = world.colliders.get_mut(handle.0) { - match mprops { - ColliderMassProperties::Density(density) => co.set_density(*density), - ColliderMassProperties::Mass(mass) => co.set_mass(*mass), - ColliderMassProperties::MassProperties(mprops) => { - co.set_mass_properties(mprops.into_rapier(scale)) - } + for (handle, mprops, world_within) in changed_collider_mass_props.iter() { + let world_id = world_within.map(|x| x.world_id).unwrap_or(DEFAULT_WORLD_ID); + + let world = context + .get_world_mut(world_id) + .expect("World {world_id} does not exist"); + + if let Some(co) = world.colliders.get_mut(handle.0) { + match mprops { + ColliderMassProperties::Density(density) => co.set_density(*density), + ColliderMassProperties::Mass(mass) => co.set_mass(*mass), + ColliderMassProperties::MassProperties(mprops) => { + co.set_mass_properties(mprops.into_rapier(world.physics_scale)) } } } @@ -283,7 +395,6 @@ pub fn apply_rigid_body_user_changes( >, ) { for (_, world) in context.worlds.iter_mut() { - let context = &mut *context; let scale = world.physics_scale; // Deal with sleeping first, because other changes may then wake-up the @@ -501,7 +612,6 @@ pub fn writeback_rigid_bodies( mut writeback: Query, ) { for (_, world) in context.worlds.iter_mut() { - let context = &mut *context; let scale = world.physics_scale; if config.physics_pipeline_active { @@ -646,34 +756,34 @@ pub fn step_simulation( config: Res, hooks: SystemParamItem, (time, mut sim_to_render_time): (Res