diff --git a/src/plugin/systems.rs b/src/plugin/systems.rs deleted file mode 100644 index acd9ec04..00000000 --- a/src/plugin/systems.rs +++ /dev/null @@ -1,1762 +0,0 @@ -//! Systems responsible for interfacing our Bevy components with the Rapier physics engine. - -use crate::dynamics::{ - AdditionalMassProperties, Ccd, Damping, Dominance, ExternalForce, ExternalImpulse, - GravityScale, ImpulseJoint, LockedAxes, MassProperties, MultibodyJoint, - RapierImpulseJointHandle, RapierMultibodyJointHandle, RapierRigidBodyHandle, - ReadMassProperties, RigidBody, Sleeping, TransformInterpolation, Velocity, -}; -use crate::geometry::{ - ActiveCollisionTypes, ActiveEvents, ActiveHooks, Collider, ColliderDisabled, - ColliderMassProperties, ColliderScale, CollisionGroups, ContactForceEventThreshold, Friction, - RapierColliderHandle, Restitution, Sensor, SolverGroups, -}; -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, Vect, -}; -use crate::utils; -use bevy::ecs::system::{StaticSystemParam, SystemParamItem}; -use bevy::prelude::*; -use rapier::prelude::*; -use std::collections::HashMap; - -#[cfg(all(feature = "dim3", feature = "async-collider"))] -use { - crate::prelude::{AsyncCollider, AsyncSceneCollider}, - bevy::scene::SceneInstance, -}; - -use crate::control::CharacterCollision; -#[cfg(feature = "dim2")] -use bevy::math::Vec3Swizzles; - -/// Components that will be updated after a physics step. -pub type RigidBodyWritebackComponents<'a> = ( - Entity, - Option<&'a Parent>, - Option<&'a mut Transform>, - Option<&'a mut TransformInterpolation>, - Option<&'a mut Velocity>, - Option<&'a mut Sleeping>, -); - -/// Components related to rigid-bodies. -pub type RigidBodyComponents<'a> = ( - Entity, - &'a RigidBody, - Option<&'a GlobalTransform>, - Option<&'a Velocity>, - Option<&'a AdditionalMassProperties>, - Option<&'a ReadMassProperties>, - Option<&'a LockedAxes>, - Option<&'a ExternalForce>, - Option<&'a GravityScale>, - Option<&'a Ccd>, - Option<&'a Dominance>, - Option<&'a Sleeping>, - Option<&'a Damping>, - Option<&'a RigidBodyDisabled>, -); - -/// Components related to colliders. -pub type ColliderComponents<'a> = ( - Entity, - &'a Collider, - Option<&'a Sensor>, - Option<&'a ColliderMassProperties>, - Option<&'a ActiveEvents>, - Option<&'a ActiveHooks>, - Option<&'a ActiveCollisionTypes>, - Option<&'a Friction>, - Option<&'a Restitution>, - Option<&'a CollisionGroups>, - Option<&'a SolverGroups>, - Option<&'a ContactForceEventThreshold>, - Option<&'a ColliderDisabled>, -); - -/// System responsible for applying [`GlobalTransform::scale`] and/or [`ColliderScale`] to -/// colliders. -pub fn apply_scale( - mut context: ResMut, - config: Res, - mut changed_collider_scales: Query< - ( - &mut Collider, - &RapierColliderHandle, - &GlobalTransform, - Option<&ColliderScale>, - ), - Or<( - Changed, - Changed, - Changed, - )>, - >, -) { - // NOTE: we don’t have to apply the RapierConfiguration::physics_scale here because - // we are applying the scale to the user-facing shape here, not the ones inside - // colliders (yet). - for (mut shape, handle, transform, custom_scale) in changed_collider_scales.iter_mut() { - #[cfg(feature = "dim2")] - let effective_scale = match custom_scale { - Some(ColliderScale::Absolute(scale)) => *scale, - Some(ColliderScale::Relative(scale)) => { - *scale * transform.compute_transform().scale.xy() - } - None => transform.compute_transform().scale.xy(), - }; - #[cfg(feature = "dim3")] - let effective_scale = match custom_scale { - Some(ColliderScale::Absolute(scale)) => *scale, - Some(ColliderScale::Relative(scale)) => *scale * transform.compute_transform().scale, - None => transform.compute_transform().scale, - }; - - if shape.scale != crate::geometry::get_snapped_scale(effective_scale) { - if let Some(co) = context.colliders.get_mut(handle.0) { - if let Some(position) = co.position_wrt_parent() { - let translation: Vect = position.translation.vector.into(); - let unscaled_translation: Vect = translation / shape.scale(); - let new_translation = unscaled_translation * effective_scale; - co.set_translation_wrt_parent(new_translation.into()); - } - } - shape.set_scale(effective_scale, config.scaled_shape_subdivision); - } - } -} - -/// System responsible for applying changes the user made to a collider-related component. -pub fn apply_collider_user_changes( - config: Res, - mut context: ResMut, - changed_collider_transforms: Query< - (&RapierColliderHandle, &GlobalTransform), - (Without, Changed), - >, - - changed_shapes: Query<(&RapierColliderHandle, &Collider), Changed>, - changed_active_events: Query<(&RapierColliderHandle, &ActiveEvents), Changed>, - changed_active_hooks: Query<(&RapierColliderHandle, &ActiveHooks), Changed>, - changed_active_collision_types: Query< - (&RapierColliderHandle, &ActiveCollisionTypes), - Changed, - >, - changed_friction: Query<(&RapierColliderHandle, &Friction), Changed>, - changed_restitution: Query<(&RapierColliderHandle, &Restitution), Changed>, - changed_collision_groups: Query< - (&RapierColliderHandle, &CollisionGroups), - Changed, - >, - changed_solver_groups: Query<(&RapierColliderHandle, &SolverGroups), Changed>, - changed_sensors: Query<(&RapierColliderHandle, &Sensor), Changed>, - changed_disabled: Query<(&RapierColliderHandle, &ColliderDisabled), Changed>, - changed_contact_force_threshold: Query< - (&RapierColliderHandle, &ContactForceEventThreshold), - Changed, - >, - changed_collider_mass_props: Query< - (&RapierColliderHandle, &ColliderMassProperties), - 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 (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, 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_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_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, 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, 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, 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, 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, _) in changed_sensors.iter() { - if let Some(co) = context.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, 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, 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)) - } - } - } - } -} - -/// System responsible for applying changes the user made to a rigid-body-related component. -pub fn apply_rigid_body_user_changes( - mut context: ResMut, - config: Res, - changed_rb_types: Query<(&RapierRigidBodyHandle, &RigidBody), Changed>, - mut changed_transforms: Query< - ( - &RapierRigidBodyHandle, - &GlobalTransform, - Option<&mut TransformInterpolation>, - ), - Changed, - >, - changed_velocities: Query<(&RapierRigidBodyHandle, &Velocity), Changed>, - changed_additional_mass_props: Query< - (&RapierRigidBodyHandle, &AdditionalMassProperties), - Changed, - >, - changed_locked_axes: Query<(&RapierRigidBodyHandle, &LockedAxes), Changed>, - changed_forces: Query<(&RapierRigidBodyHandle, &ExternalForce), Changed>, - mut changed_impulses: Query< - (&RapierRigidBodyHandle, &mut ExternalImpulse), - Changed, - >, - changed_gravity_scale: Query<(&RapierRigidBodyHandle, &GravityScale), Changed>, - changed_ccd: Query<(&RapierRigidBodyHandle, &Ccd), Changed>, - changed_dominance: Query<(&RapierRigidBodyHandle, &Dominance), Changed>, - changed_sleeping: Query<(&RapierRigidBodyHandle, &Sleeping), Changed>, - changed_damping: Query<(&RapierRigidBodyHandle, &Damping), Changed>, - changed_disabled: Query< - (&RapierRigidBodyHandle, &RigidBodyDisabled), - 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(); - } - } - } - - // 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); - } - } - - // 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_fn = - |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() { - // Use an Option to avoid running the check twice. - let mut transform_changed = None; - - if let Some(interpolation) = interpolation.as_deref_mut() { - transform_changed = transform_changed.or_else(|| { - Some(transform_changed_fn( - &handle.0, - global_transform, - &context.last_body_transform_set, - )) - }); - - if transform_changed == Some(true) { - // 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) { - transform_changed = transform_changed.or_else(|| { - Some(transform_changed_fn( - &handle.0, - global_transform, - &context.last_body_transform_set, - )) - }); - - match rb.body_type() { - RigidBodyType::KinematicPositionBased => { - if transform_changed == Some(true) { - 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 transform_changed == Some(true) { - rb.set_position( - utils::transform_to_iso(&global_transform.compute_transform(), scale), - true, - ); - context - .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, 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, 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, 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, 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, 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, ccd) in changed_ccd.iter() { - if let Some(rb) = context.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, 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, _) in changed_disabled.iter() { - if let Some(co) = context.bodies.get_mut(handle.0) { - co.set_enabled(false); - } - } -} - -/// System responsible for applying changes the user made to a joint component. -pub fn apply_joint_user_changes( - mut context: ResMut, - changed_impulse_joints: Query< - (&RapierImpulseJointHandle, &ImpulseJoint), - Changed, - >, - changed_multibody_joints: Query< - (&RapierMultibodyJointHandle, &MultibodyJoint), - 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 (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); - } - } - } -} - -/// System responsible for writing the result of the last simulation step into our `bevy_rapier` -/// components and the [`GlobalTransform`] component. -pub fn writeback_rigid_bodies( - mut context: ResMut, - config: Res, - sim_to_render_time: Res, - global_transforms: Query<&GlobalTransform>, - mut writeback: Query< - RigidBodyWritebackComponents, - (With, Without), - >, -) { - let context = &mut *context; - let scale = context.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 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 - { - // 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); - - 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(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(); - } - } - } - } - } - } -} - -/// System responsible for advancing the physics simulation, and updating the internal state -/// for scene queries. -pub fn step_simulation( - mut context: ResMut, - config: Res, - hooks: StaticSystemParam, - time: Res