diff --git a/src/collision/collider/backend.rs b/src/collision/collider/backend.rs index 7e958298..1b8f38e3 100644 --- a/src/collision/collider/backend.rs +++ b/src/collision/collider/backend.rs @@ -11,6 +11,7 @@ use bevy::{ ecs::{intern::Interned, schedule::ScheduleLabel, system::SystemId}, prelude::*, }; +use mass_properties::OnChangeColliderMassProperties; /// A plugin for handling generic collider backend logic. /// @@ -181,10 +182,9 @@ impl Plugin for ColliderBackendPlugin { // Get the needed collider components. // TODO: Is there an efficient way to do this with QueryState? - let (Some(parent), Some(collider_mass_properties), Some(collider_transform)) = ( + let (Some(parent), Some(collider_mass_properties)) = ( entity_ref.get::().copied(), entity_ref.get::().copied(), - entity_ref.get::().copied(), ) else { return; }; @@ -195,76 +195,65 @@ impl Plugin for ColliderBackendPlugin { let system_id = *system_id; // Handle collider removal with the collider data passed as input. - world.commands().run_system_with_input( - system_id, - (parent, collider_mass_properties, collider_transform), - ); + world + .commands() + .run_system_with_input(system_id, (entity, parent, collider_mass_properties)); }); - // When the `Sensor` component is added to a collider, - // remove the collider's contribution on the rigid body's mass properties. + // When the `Sensor` component is added to a collider, trigger an event. + // The `MassPropertyPlugin` responds to the event and updates the body's mass properties accordingly. app.observe( |trigger: Trigger, - query: Query<( - &ColliderParent, - &ColliderMassProperties, - &PreviousColliderTransform, - )>, - mut body_query: Query| { - if let Ok(( - collider_parent, - collider_mass_properties, - previous_collider_transform, - )) = query.get(trigger.entity()) - { + mut commands: Commands, + query: Query<&ColliderMassProperties>| { + if let Ok(collider_mass_properties) = query.get(trigger.entity()) { // If the collider mass properties are zero, there is nothing to subtract. if *collider_mass_properties == ColliderMassProperties::ZERO { return; } - if let Ok(mut mass_properties) = body_query.get_mut(collider_parent.0) { - // Subtract previous collider mass props from the body's own mass props. - mass_properties -= - collider_mass_properties.transformed_by(previous_collider_transform); - } + // Trigger an event to update the body's mass propertiess. + commands.trigger_targets( + OnChangeColliderMassProperties { + previous: Some(*collider_mass_properties), + current: None, + }, + trigger.entity(), + ); } }, ); - // When the `Sensor` component is removed from a collider, - // add the collider's mass properties to the rigid body's mass properties. + // When the `Sensor` component is removed from a collider, update its mass properties and trigger an event. + // The `MassPropertyPlugin` responds to the event and updates the body's mass properties accordingly. app.observe( |trigger: Trigger, + mut commands: Commands, mut collider_query: Query<( Ref, - &ColliderParent, &ColliderDensity, - &mut ColliderMassProperties, - &ColliderTransform, - )>, - mut body_query: Query| { - if let Ok(( - collider, - collider_parent, - density, - mut collider_mass_properties, - collider_transform, - )) = collider_query.get_mut(trigger.entity()) + &mut ColliderMassProperties + )>| { + if let Ok((collider, density, mut collider_mass_properties)) = + collider_query.get_mut(trigger.entity()) { - if let Ok(mut mass_properties) = body_query.get_mut(collider_parent.0) { - // Update collider mass props. - *collider_mass_properties = - collider.mass_properties(density.max(Scalar::EPSILON)); - - // If the collider mass properties are zero, there is nothing to add. - if *collider_mass_properties == ColliderMassProperties::ZERO { - return; - } - - // Add new collider mass props to the body's mass props. - mass_properties += - collider_mass_properties.transformed_by(collider_transform); + // Update collider mass props. + *collider_mass_properties = + collider.mass_properties(density.max(Scalar::EPSILON)); + + // If the collider mass properties are zero, there is nothing to add. + if *collider_mass_properties == ColliderMassProperties::ZERO { + return; } + + // Trigger an event to update the body's mass properties. + commands.trigger_targets( + OnChangeColliderMassProperties { + previous: None, + current: Some(*collider_mass_properties), + }, + trigger.entity(), + ); } }, ); @@ -278,10 +267,11 @@ impl Plugin for ColliderBackendPlugin { ( update_collider_scale::, update_collider_mass_properties::, + update_previous_collider_transforms, ) .chain() .in_set(PrepareSet::Finalize) - .before(crate::prepare::warn_missing_mass), + .before(dynamics::rigid_body::mass_properties::warn_missing_mass), ), ); @@ -689,28 +679,30 @@ pub fn update_collider_scale( /// A resource that stores the system ID for the system that reacts to collider removals. #[derive(Resource)] -struct ColliderRemovalSystem(SystemId<(ColliderParent, ColliderMassProperties, ColliderTransform)>); +struct ColliderRemovalSystem(SystemId<(Entity, ColliderParent, ColliderMassProperties)>); /// Updates the mass properties of bodies and wakes bodies up when an attached collider is removed. /// -/// Takes the removed collider's parent, mass properties, and transform as input. +/// Takes the removed collider's entity, parent, mass properties, and transform as input. fn collider_removed( - In((parent, collider_mass_props, collider_transform)): In<( + In((collider_entity, parent, collider_mass_props)): In<( + Entity, ColliderParent, ColliderMassProperties, - ColliderTransform, )>, mut commands: Commands, - mut mass_prop_query: Query<(MassPropertiesQuery, &mut TimeSleeping)>, + mut mass_prop_query: Query<&mut TimeSleeping>, ) { let parent = parent.get(); - if let Ok((mut mass_properties, mut time_sleeping)) = mass_prop_query.get_mut(parent) { + if let Ok(mut time_sleeping) = mass_prop_query.get_mut(parent) { // Subtract the mass properties of the collider from the mass properties of the rigid body. - mass_properties -= ColliderMassProperties { - center_of_mass: collider_transform.transform_point(collider_mass_props.center_of_mass), - - ..collider_mass_props - }; + commands.trigger_targets( + OnChangeColliderMassProperties { + previous: Some(collider_mass_props), + current: None, + }, + collider_entity, + ); // Wake up the rigid body since removing the collider could also remove active contacts. commands.entity(parent).remove::(); @@ -721,12 +713,10 @@ fn collider_removed( /// Updates the mass properties of [`Collider`]s and [collider parents](ColliderParent). #[allow(clippy::type_complexity)] pub(crate) fn update_collider_mass_properties( - mut mass_props: Query<(Entity, MassPropertiesQuery)>, - mut colliders: Query< + mut commands: Commands, + mut query: Query< ( - &ColliderTransform, - &mut PreviousColliderTransform, - &ColliderParent, + Entity, Ref, &ColliderDensity, &mut ColliderMassProperties, @@ -742,31 +732,26 @@ pub(crate) fn update_collider_mass_properties( ), >, ) { - for ( - collider_transform, - mut previous_collider_transform, - collider_parent, - collider, - density, - mut collider_mass_properties, - ) in &mut colliders - { - if let Ok((_, mut mass_properties)) = mass_props.get_mut(collider_parent.0) { - // Subtract previous collider mass props from the body's own mass props. - // If the collider is new, it doesn't have previous mass props, so we shouldn't subtract anything. - if !collider.is_added() { - mass_properties -= - collider_mass_properties.transformed_by(&previous_collider_transform); - } + for (entity, collider, density, mut collider_mass_properties) in &mut query { + // If the collider is new, it doesn't have previous mass properties. + let previous = (!collider.is_added()).then_some(*collider_mass_properties); - previous_collider_transform.0 = *collider_transform; + // Update the collider's mass properties. + *collider_mass_properties = collider.mass_properties(density.max(Scalar::EPSILON)); - // Update collider mass props. - *collider_mass_properties = collider.mass_properties(density.max(Scalar::EPSILON)); + let current = Some(*collider_mass_properties); - // Add new collider mass props to the body's mass props. - mass_properties += collider_mass_properties.transformed_by(collider_transform); - } + // The `MassPropertyPlugin` will respond to this event and update the body's mass properties accordingly. + commands.trigger_targets(OnChangeColliderMassProperties { previous, current }, entity); + } +} + +/// Updates the [`PreviousColliderTransform`] component for colliders. +pub(crate) fn update_previous_collider_transforms( + mut query: Query<(&ColliderTransform, &mut PreviousColliderTransform)>, +) { + for (collider_transform, mut previous_collider_transform) in &mut query { + previous_collider_transform.0 = *collider_transform; } } @@ -785,6 +770,7 @@ mod tests { app.add_plugins(( PreparePlugin::new(FixedPostUpdate), + MassPropertyPlugin::new(FixedPostUpdate), ColliderBackendPlugin::::new(FixedPostUpdate), ColliderHierarchyPlugin::new(FixedPostUpdate), HierarchyPlugin, diff --git a/src/dynamics/integrator/mod.rs b/src/dynamics/integrator/mod.rs index e4c19912..4d55016d 100644 --- a/src/dynamics/integrator/mod.rs +++ b/src/dynamics/integrator/mod.rs @@ -62,7 +62,7 @@ impl Plugin for IntegratorPlugin { #[cfg(feature = "3d")] app.add_systems( self.schedule.intern(), - crate::prepare::update_global_angular_inertia::<()> + dynamics::rigid_body::mass_properties::update_global_angular_inertia::<()> .in_set(IntegrationSet::Position) .after(integrate_positions), ); diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 4c82a687..9f8fe0d8 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -72,10 +72,18 @@ pub mod prelude { pub use super::{ ccd::{CcdPlugin, SpeculativeMargin, SweepMode, SweptCcd}, integrator::{Gravity, IntegratorPlugin}, - rigid_body::*, + rigid_body::{ + mass_properties::{ + AngularInertia, AngularInertiaError, CenterOfMass, ColliderDensity, + ColliderMassProperties, Mass, MassError, MassPropertiesBundle, MassPropertiesQuery, + MassPropertyPlugin, + }, + *, + }, sleeping::{DeactivationTime, SleepingPlugin, SleepingThreshold}, solver::{joints::*, PhysicsLengthUnit, SolverPlugin, SolverSet}, }; + pub(crate) use crate::dynamics::rigid_body::mass_properties::GlobalAngularInertia; } // For intra-doc links diff --git a/src/dynamics/rigid_body/mass_properties/components.rs b/src/dynamics/rigid_body/mass_properties/components.rs new file mode 100644 index 00000000..fb5fe0c4 --- /dev/null +++ b/src/dynamics/rigid_body/mass_properties/components.rs @@ -0,0 +1,1179 @@ +use crate::prelude::*; +use bevy::prelude::*; +use derive_more::From; + +/// An error returned for an invalid mass. +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum MassError { + /// The mass is negative. + Negative, + /// The mass is NaN. + NaN, +} + +/// The mass of a dynamic [rigid body]. +/// +/// Note that zero mass is treated as a special case, and is used to represent infinite mass. +/// +/// [rigid body]: RigidBody +/// +/// ## Representation +/// +/// Internally, the mass is actually stored as the inverse mass `1.0 / mass`. +/// This is because most physics calculations operate on the inverse mass, and storing it directly +/// allows for fewer divisions and guards against division by zero. +/// +/// When using [`Mass`], you shouldn't need to worry about this internal representation. +/// The provided constructors and getters abstract away the implementation details. +/// +/// In terms of performance, the main thing to keep in mind is that [`inverse`](Self::inverse) is a no-op +/// and [`value`](Self::value) contains a division. When dividing by the mass, it's better to use +/// `foo * mass.inverse()` than `foo / mass.value()`. +#[derive(Reflect, Clone, Copy, Component, Debug, Default, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, Default, PartialEq)] +pub struct Mass { + /// The inverse mass. + /// + /// This is stored as an inverse because most physics calculations + /// operate on the inverse mass, and storing it directly allows for + /// fewer divisions and guards against division by zero. + inverse: Scalar, +} + +impl Mass { + /// Infinite mass. + pub const INFINITY: Self = Self { inverse: 0.0 }; + + /// Creates a new [`Mass`] from the given mass. + /// + /// # Panics + /// + /// Panics if the mass is negative when `debug_assertions` are enabled. + #[inline] + pub fn new(mass: Scalar) -> Self { + Self::from_inverse(mass.recip_or_zero()) + } + + /// Tries to create a new [`Mass`] from the given mass. + /// + /// # Errors + /// + /// Returns [`Err(MassError)`](MassError) if the mass is negative or NaN. + #[inline] + pub fn try_new(mass: Scalar) -> Result { + if mass.is_nan() { + Err(MassError::NaN) + } else if mass < 0.0 { + Err(MassError::Negative) + } else { + Ok(Self::from_inverse(mass.recip_or_zero())) + } + } + + /// Creates a new [`Mass`] from the given inverse mass. + /// + /// # Panics + /// + /// Panics if the inverse mass is negative when `debug_assertions` are enabled. + #[inline] + pub fn from_inverse(inverse_mass: Scalar) -> Self { + debug_assert!( + inverse_mass >= 0.0 && !inverse_mass.is_nan(), + "mass must be positive or zero" + ); + + Self { + inverse: inverse_mass, + } + } + + /// Tries to create a new [`Mass`] from the given inverse mass. + /// + /// # Errors + /// + /// Returns [`Err(MassError)`](MassError) if the inverse mass is negative or NaN. + pub fn try_from_inverse(inverse_mass: Scalar) -> Result { + if inverse_mass.is_nan() { + Err(MassError::NaN) + } else if inverse_mass < 0.0 { + Err(MassError::Negative) + } else { + Ok(Self { + inverse: inverse_mass, + }) + } + } + + /// Returns the mass. If it is infinite, returns zero. + /// + /// Note that this involves a division because [`Mass`] internally stores the inverse mass. + /// If dividing by the mass, consider using `foo * mass.inverse()` instead of `foo / mass.value()`. + #[inline] + pub fn value(self) -> Scalar { + self.inverse.recip_or_zero() + } + + /// Returns the inverse mass. + /// + /// This is a no-op because [`Mass`] internally stores the inverse mass. + #[inline] + pub fn inverse(self) -> Scalar { + self.inverse + } + + /// Sets the mass. + #[inline] + pub fn set(&mut self, mass: impl Into) { + *self = mass.into(); + } + + /// Returns `true` if the mass is neither infinite nor NaN. + #[inline] + pub fn is_finite(self) -> bool { + !self.is_infinite() && !self.is_nan() + } + + /// Returns `true` if the mass is positive infinity or negative infinity. + #[inline] + pub fn is_infinite(self) -> bool { + self == Self::INFINITY + } + + /// Returns `true` if the mass is NaN. + #[inline] + pub fn is_nan(self) -> bool { + self.inverse.is_nan() + } +} + +impl From for Mass { + fn from(mass: Scalar) -> Self { + Self::new(mass) + } +} + +// TODO: Add errors for asymmetric and non-positive definite matrices in 3D. +/// An error returned for an invalid angular inertia. +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum AngularInertiaError { + /// The angular inertia is negative. + Negative, + /// The angular inertia is NaN. + NaN, +} + +/// The moment of inertia of a dynamic [rigid body]. This represents the torque needed for a desired angular acceleration. +/// +/// Note that zero angular inertia is treated as a special case, and is used to represent infinite angular inertia. +/// +/// [rigid body]: RigidBody +/// +/// ## Representation +/// +/// Internally, the angular inertia is actually stored as the inverse angular inertia `1.0 / angular_inertia`. +/// This is because most physics calculations operate on the inverse angular inertia, and storing it directly +/// allows for fewer divisions and guards against division by zero. +/// +/// When using [`AngularInertia`], you shouldn't need to worry about this internal representation. +/// The provided constructors and getters abstract away the implementation details. +/// +/// In terms of performance, the main thing to keep in mind is that [`inverse`](Self::inverse) is a no-op +/// and [`value`](Self::value) contains a division. When dividing by the angular inertia, it's better to use +/// `foo * angular_inertia.inverse()` than `foo / angular_inertia.value()`. +#[cfg(feature = "2d")] +#[derive(Reflect, Clone, Copy, Component, Debug, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, Default, PartialEq)] +pub struct AngularInertia { + /// The inverse angular inertia. + /// + /// This is stored as an inverse to minimize the number of divisions + /// and to guard against division by zero. Most physics calculations + /// use the inverse angular inertia. + inverse: Scalar, +} + +#[cfg(feature = "2d")] +impl AngularInertia { + /// Infinite angular inertia. + pub const INFINITY: Self = Self { inverse: 0.0 }; + + /// Creates a new [`AngularInertia`] from the given angular inertia. + /// + /// # Panics + /// + /// Panics if the angular inertia is negative when `debug_assertions` are enabled. + #[inline] + pub fn new(angular_inertia: Scalar) -> Self { + Self::from_inverse(angular_inertia.recip_or_zero()) + } + + /// Tries to create a new [`AngularInertia`] from the given angular inertia. + /// + /// # Errors + /// + /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if the angular inertia is negative or NaN. + #[inline] + pub fn try_new(angular_inertia: Scalar) -> Result { + if angular_inertia.is_nan() { + Err(AngularInertiaError::NaN) + } else if angular_inertia < 0.0 { + Err(AngularInertiaError::Negative) + } else { + Ok(Self::from_inverse(angular_inertia.recip_or_zero())) + } + } + + /// Creates a new [`AngularInertia`] from the given inverse angular inertia. + /// + /// # Panics + /// + /// Panics if the inverse angular inertia is negative when `debug_assertions` are enabled. + #[inline] + pub fn from_inverse(inverse_angular_inertia: Scalar) -> Self { + debug_assert!( + inverse_angular_inertia >= 0.0 && !inverse_angular_inertia.is_nan(), + "angular inertia must be positive or zero" + ); + + Self { + inverse: inverse_angular_inertia, + } + } + + /// Tries to create a new [`AngularInertia`] from the given inverse angular inertia. + /// + /// # Errors + /// + /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if the inverse angular inertia is negative or NaN. + #[inline] + pub fn try_from_inverse(inverse_angular_inertia: Scalar) -> Result { + if inverse_angular_inertia.is_nan() { + Err(AngularInertiaError::NaN) + } else if inverse_angular_inertia < 0.0 { + Err(AngularInertiaError::Negative) + } else { + Ok(Self { + inverse: inverse_angular_inertia, + }) + } + } + + /// Returns the angular inertia. If it is infinite, returns zero. + /// + /// Note that this involves a division because [`AngularInertia`] internally stores the inverse angular inertia. + /// If dividing by the angular inertia, consider using `foo * angular_inertia.inverse()` instead of `foo / angular_inertia.value()`. + #[inline] + pub fn value(self) -> Scalar { + self.inverse.recip_or_zero() + } + + /// Returns the inverse angular inertia. + /// + /// This is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. + #[inline] + pub fn inverse(self) -> Scalar { + self.inverse + } + + /// Returns a mutable reference to the inverse of the angular inertia. + /// + /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. + #[inline] + pub fn inverse_mut(&mut self) -> &mut Scalar { + &mut self.inverse + } + + /// Sets the angular inertia. + #[inline] + pub fn set(&mut self, angular_inertia: impl Into) { + *self = angular_inertia.into(); + } + + /// Computes the angular inertia shifted by the given offset, taking into account the given mass. + #[inline] + pub fn shifted(&self, mass: Scalar, offset: Vector) -> Scalar { + shifted_angular_inertia(self.value(), mass, offset) + } + + /// Computes the angular inertia shifted by the given offset, taking into account the given mass. + #[inline] + pub fn shifted_inverse(&self, mass: Scalar, offset: Vector) -> Scalar { + shifted_angular_inertia(self.value(), mass, offset).recip_or_zero() + } + + /// Returns `true` if the angular inertia is neither infinite nor NaN. + #[inline] + pub fn is_finite(self) -> bool { + !self.is_infinite() && !self.is_nan() + } + + /// Returns `true` if the angular inertia is positive infinity or negative infinity. + #[inline] + pub fn is_infinite(self) -> bool { + self == Self::INFINITY + } + + /// Returns `true` if the angular inertia is NaN. + #[inline] + pub fn is_nan(self) -> bool { + self.inverse.is_nan() + } +} + +#[cfg(feature = "2d")] +impl From for AngularInertia { + fn from(angular_inertia: Scalar) -> Self { + Self::new(angular_inertia) + } +} + +/// The local moment of inertia of a dynamic [rigid body] as a 3x3 tensor matrix. +/// This represents the torque needed for a desired angular acceleration about the XYZ axes. +/// +/// This is computed in local space, so the object's orientation is not taken into account. +/// The world-space version is stored in [`GlobalAngularInertia`], which is automatically recomputed +/// whenever the local angular inertia or rotation is changed. +/// +/// To manually compute the world-space version that takes the body's rotation into account, +/// use the associated `rotated` method. Note that this operation is quite expensive, so use it sparingly. +/// +/// The angular inertia tensor should be symmetric and positive definite. +/// +/// Note that zero angular inertia is treated as a special case, and is used to represent infinite angular inertia. +/// +/// [rigid body]: RigidBody +/// +/// ## Representation +/// +/// Internally, the angular inertia is actually stored as the inverse angular inertia tensor `angular_inertia_matrix.inverse()`. +/// This is because most physics calculations operate on the inverse angular inertia, and storing it directly +/// allows for fewer inversions and guards against division by zero. +/// +/// When using [`AngularInertia`], you shouldn't need to worry about this internal representation. +/// The provided constructors and getters abstract away the implementation details. +/// +/// In terms of performance, the main thing to keep in mind is that [`inverse`](Self::inverse) is a no-op +/// and [`value`](Self::value) contains an inversion. When multiplying by the inverse angular inertia, it's better to use +/// `angular_inertia.inverse() * foo` than `angular_inertia.value().inverse() * foo`. +#[cfg(feature = "3d")] +#[derive(Reflect, Clone, Copy, Component, Debug, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, PartialEq)] +pub struct AngularInertia { + // TODO: The matrix should be symmetric and positive definite. + // We could add a custom `SymmetricMat3` type to enforce symmetricity and reduce memory usage. + inverse: Matrix, +} + +impl Default for AngularInertia { + fn default() -> Self { + Self::INFINITY + } +} + +// TODO: Add helpers for getting the principal angular inertia and local inertial frame. This requires an eigensolver. +// `bevy_heavy` has this functionality. +#[cfg(feature = "3d")] +impl AngularInertia { + /// Infinite angular inertia. + pub const INFINITY: Self = Self { + inverse: Matrix::ZERO, + }; + + /// Creates a new [`AngularInertia`] from the given principal angular inertia. + /// + /// The principal angular inertia represents the torque needed for a desired angular acceleration + /// about the local coordinate axes. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + /// + /// To specify the orientation of the local inertial frame, consider using [`AngularInertia::new_with_local_frame`]. + /// + /// # Panics + /// + /// Panics if any component of the principal angular inertia is negative when `debug_assertions` are enabled. + #[inline] + #[doc(alias = "from_principal_angular_inertia")] + pub fn new(principal_angular_inertia: Vector) -> Self { + debug_assert!( + principal_angular_inertia.cmpge(Vector::ZERO).all() + && !principal_angular_inertia.is_nan(), + "principal angular inertia must be positive or zero for all axes" + ); + + Self::from_inverse_tensor(Matrix::from_diagonal( + principal_angular_inertia.recip_or_zero(), + )) + } + + /// Tries to create a new [`AngularInertia`] from the given principal angular inertia. + /// + /// The principal angular inertia represents the torque needed for a desired angular acceleration + /// about the local coordinate axes. To specify the orientation of the local inertial frame, + /// consider using [`AngularInertia::try_new_with_local_frame`]. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + /// + /// # Errors + /// + /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if any component of the principal angular inertia is negative or NaN. + #[inline] + pub fn try_new(principal_angular_inertia: Vector) -> Result { + if principal_angular_inertia.is_nan() { + Err(AngularInertiaError::NaN) + } else if !principal_angular_inertia.cmpge(Vector::ZERO).all() { + Err(AngularInertiaError::Negative) + } else { + Ok(Self::from_inverse_tensor(Matrix::from_diagonal( + principal_angular_inertia.recip_or_zero(), + ))) + } + } + + /// Creates a new [`AngularInertia`] from the given principal angular inertia + /// and the orientation of the local inertial frame. + /// + /// The principal angular inertia represents the torque needed for a desired angular acceleration + /// about the local coordinate axes defined by the given `orientation`. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + /// + /// # Panics + /// + /// Panics if any component of the principal angular inertia is negative when `debug_assertions` are enabled. + #[inline] + #[doc(alias = "from_principal_angular_inertia_with_local_frame")] + pub fn new_with_local_frame( + principal_angular_inertia: Vector, + orientation: Quaternion, + ) -> Self { + debug_assert!( + principal_angular_inertia.cmpge(Vector::ZERO).all() + && !principal_angular_inertia.is_nan(), + "principal angular inertia must be positive or zero for all axes" + ); + + Self::from_inverse_tensor( + Matrix::from_quat(orientation) + * Matrix::from_diagonal(principal_angular_inertia.recip_or_zero()) + * Matrix::from_quat(orientation.inverse()), + ) + } + + /// Tries to create a new [`AngularInertia`] from the given principal angular inertia + /// and the orientation of the local inertial frame. + /// + /// The principal angular inertia represents the torque needed for a desired angular acceleration + /// about the local coordinate axes defined by the given `orientation`. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + /// + /// # Errors + /// + /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if any component of the principal angular inertia is negative or NaN. + #[inline] + pub fn try_new_with_local_frame( + principal_angular_inertia: Vector, + orientation: Quaternion, + ) -> Result { + if principal_angular_inertia.is_nan() { + Err(AngularInertiaError::NaN) + } else if !principal_angular_inertia.cmpge(Vector::ZERO).all() { + Err(AngularInertiaError::Negative) + } else { + Ok(Self::from_inverse_tensor( + Matrix::from_quat(orientation) + * Matrix::from_diagonal(principal_angular_inertia.recip_or_zero()) + * Matrix::from_quat(orientation.inverse()), + )) + } + } + + /// Creates a new [`AngularInertia`] from the given angular inertia tensor. + /// + /// The tensor should be symmetric and positive definite. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + #[inline] + #[doc(alias = "from_mat3")] + pub fn from_tensor(tensor: Matrix) -> Self { + Self::from_inverse_tensor(tensor.inverse_or_zero()) + } + + /// Creates a new [`AngularInertia`] from the given angular inertia tensor. + /// + /// The tensor should be symmetric and positive definite. + #[inline] + #[doc(alias = "from_inverse_mat3")] + pub fn from_inverse_tensor(inverse_tensor: Matrix) -> Self { + Self { + inverse: inverse_tensor, + } + } + + /// Returns the angular inertia tensor. If it is infinite, returns zero. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + /// If multiplying by the inverse angular inertia, consider using `angular_inertia.inverse() * foo` + /// instead of `angular_inertia.value().inverse() * foo`. + /// + /// Equivalent to [`AngularInertia::tensor`]. + #[inline] + pub(crate) fn value(self) -> Matrix { + self.tensor() + } + + /// Returns the inverse of the angular inertia tensor. + /// + /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. + /// + /// Equivalent to [`AngularInertia::inverse_tensor`]. + #[inline] + pub(crate) fn inverse(self) -> Matrix { + self.inverse_tensor() + } + + /// Returns a mutable reference to the inverse of the angular inertia tensor. + /// + /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. + #[inline] + pub(crate) fn inverse_mut(&mut self) -> &mut Matrix { + self.inverse_tensor_mut() + } + + /// Returns the angular inertia tensor. + /// + /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. + /// If multiplying by the inverse angular inertia, consider using `angular_inertia.inverse() * foo` + /// instead of `angular_inertia.value().inverse() * foo`. + #[inline] + #[doc(alias = "as_mat3")] + pub fn tensor(self) -> Matrix { + self.inverse.inverse_or_zero() + } + + /// Returns the inverse of the angular inertia tensor. + /// + /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. + #[inline] + #[doc(alias = "as_inverse_mat3")] + pub fn inverse_tensor(self) -> Matrix { + self.inverse + } + + /// Returns a mutable reference to the inverse of the angular inertia tensor. + /// + /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. + #[inline] + #[doc(alias = "as_inverse_mat3_mut")] + pub fn inverse_tensor_mut(&mut self) -> &mut Matrix { + &mut self.inverse + } + + /// Sets the angular inertia tensor. + #[inline] + pub fn set(&mut self, angular_inertia: impl Into) { + *self = angular_inertia.into(); + } + + /// Computes the angular inertia tensor with the given rotation. + /// + /// This can be used to transform local angular inertia to world space. + #[inline] + pub fn rotated(self, rotation: Quaternion) -> Self { + let rot_mat3 = Matrix::from_quat(rotation); + Self::from_inverse_tensor((rot_mat3 * self.inverse) * rot_mat3.transpose()) + } + + /// Computes the angular inertia tensor shifted by the given offset, taking into account the given mass. + #[inline] + pub fn shifted_tensor(&self, mass: Scalar, offset: Vector) -> Matrix3 { + shifted_angular_inertia(self.tensor(), mass, offset) + } + + /// Computes the inverse angular inertia tensor shifted by the given offset, taking into account the given mass. + #[inline] + pub fn shifted_inverse_tensor(&self, mass: Scalar, offset: Vector) -> Matrix3 { + shifted_angular_inertia(self.tensor(), mass, offset).inverse_or_zero() + } + + /// Returns `true` if the angular inertia is neither infinite nor NaN. + #[inline] + pub fn is_finite(self) -> bool { + !self.is_infinite() && !self.is_nan() + } + + /// Returns `true` if the angular inertia is positive infinity or negative infinity. + #[inline] + pub fn is_infinite(self) -> bool { + self == Self::INFINITY + } + + /// Returns `true` if the angular inertia is NaN. + #[inline] + pub fn is_nan(self) -> bool { + self.inverse.is_nan() + } +} + +#[cfg(feature = "3d")] +impl From for AngularInertia { + fn from(tensor: Matrix) -> Self { + Self::from_tensor(tensor) + } +} + +#[cfg(feature = "2d")] +impl core::ops::Mul for AngularInertia { + type Output = Scalar; + + #[inline] + fn mul(self, rhs: Scalar) -> Scalar { + self.value() * rhs + } +} + +impl core::ops::Mul for AngularInertia { + type Output = Vector; + + #[inline] + fn mul(self, rhs: Vector) -> Vector { + self.value() * rhs + } +} + +// In 2D, the global angular inertia is the same as the local angular inertia. +#[cfg(feature = "2d")] +pub(crate) type GlobalAngularInertia = AngularInertia; + +/// The world-space moment of inertia of a dynamic [rigid body] as a 3x3 tensor matrix. +/// This represents the torque needed for a desired angular acceleration about the XYZ axes. +/// +/// This component is automatically updated whenever the local [`AngularInertia`] or rotation is changed. +/// To manually update it, use the associated [`update`](Self::update) method. +/// +/// [rigid body]: RigidBody +#[cfg(feature = "3d")] +#[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, PartialEq, From)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, PartialEq)] +pub struct GlobalAngularInertia(AngularInertia); + +#[cfg(feature = "3d")] +impl GlobalAngularInertia { + /// Creates a new [`GlobalAngularInertia`] from the given local angular inertia and rotation. + pub fn new( + local_angular_inertia: impl Into, + rotation: impl Into, + ) -> Self { + let local_angular_inertia: AngularInertia = local_angular_inertia.into(); + Self(local_angular_inertia.rotated(rotation.into())) + } + + /// Updates the global angular inertia with the given local angular inertia and rotation. + pub fn update( + &mut self, + local_angular_inertia: impl Into, + rotation: impl Into, + ) { + *self = Self::new(local_angular_inertia, rotation); + } +} + +#[cfg(feature = "3d")] +impl From for AngularInertia { + fn from(inertia: GlobalAngularInertia) -> Self { + inertia.0 + } +} + +#[cfg(feature = "3d")] +impl From for GlobalAngularInertia { + fn from(tensor: Matrix) -> Self { + Self(AngularInertia::from_tensor(tensor)) + } +} + +/// The local center of mass of a dynamic [rigid body]. +/// +/// [rigid body]: RigidBody +#[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, DerefMut, PartialEq, From)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, Default, PartialEq)] +pub struct CenterOfMass(pub Vector); + +impl CenterOfMass { + /// A center of mass set at the local origin. + pub const ZERO: Self = Self(Vector::ZERO); +} + +/// A bundle containing mass properties. +/// +/// ## Example +/// +/// The easiest way to create a new bundle is to use the [`new_computed`](Self::new_computed) method +/// that computes the mass properties based on a given [`Collider`] and density. +/// +/// ``` +#[cfg_attr(feature = "2d", doc = "use avian2d::prelude::*;")] +#[cfg_attr(feature = "3d", doc = "use avian3d::prelude::*;")] +/// use bevy::prelude::*; +/// +/// fn setup(mut commands: Commands) { +/// commands.spawn(( +/// RigidBody::Dynamic, +#[cfg_attr( + feature = "2d", + doc = " MassPropertiesBundle::new_computed(&Collider::circle(0.5), 1.0)," +)] +#[cfg_attr( + feature = "3d", + doc = " MassPropertiesBundle::new_computed(&Collider::sphere(0.5), 1.0)," +)] +/// )); +/// } +/// ``` +#[allow(missing_docs)] +#[derive(Bundle, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +pub struct MassPropertiesBundle { + pub mass: Mass, + pub angular_inertia: AngularInertia, + pub center_of_mass: CenterOfMass, +} + +impl MassPropertiesBundle { + /// Computes the mass properties for a [`Collider`] based on its shape and a given density. + #[cfg(all( + feature = "default-collider", + any(feature = "parry-f32", feature = "parry-f64") + ))] + pub fn new_computed(collider: &Collider, density: Scalar) -> Self { + let ColliderMassProperties { + mass, + angular_inertia, + center_of_mass, + .. + } = collider.mass_properties(density); + + Self { + mass: Mass::new(mass), + #[cfg(feature = "2d")] + angular_inertia: AngularInertia::new(angular_inertia), + #[cfg(feature = "3d")] + angular_inertia: AngularInertia::from_tensor(angular_inertia), + center_of_mass: CenterOfMass(center_of_mass), + } + } +} + +/// The density of a [`Collider`], 1.0 by default. This is used for computing +/// the [`ColliderMassProperties`] for each collider. +/// +/// ## Example +/// +/// ``` +#[cfg_attr(feature = "2d", doc = "use avian2d::prelude::*;")] +#[cfg_attr(feature = "3d", doc = "use avian3d::prelude::*;")] +/// use bevy::prelude::*; +/// +/// // Spawn a body with a collider that has a density of 2.5 +/// fn setup(mut commands: Commands) { +/// commands.spawn(( +/// RigidBody::Dynamic, +#[cfg_attr(feature = "2d", doc = " Collider::circle(0.5),")] +#[cfg_attr(feature = "3d", doc = " Collider::sphere(0.5),")] +/// ColliderDensity(2.5), +/// )); +/// } +/// ``` +#[derive(Reflect, Clone, Copy, Component, Debug, Deref, DerefMut, PartialEq, PartialOrd)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, PartialEq)] +pub struct ColliderDensity(pub Scalar); + +impl ColliderDensity { + /// The density of the [`Collider`] is zero. It has no mass. + pub const ZERO: Self = Self(0.0); +} + +impl Default for ColliderDensity { + fn default() -> Self { + Self(1.0) + } +} + +impl From for ColliderDensity { + fn from(density: Scalar) -> Self { + Self(density) + } +} + +/// An automatically added component that contains the read-only mass properties of a [`Collider`]. +/// The density used for computing the mass properties can be configured using the [`ColliderDensity`] +/// component. +/// +/// These mass properties will be added to the [rigid body's](RigidBody) actual [`Mass`], +/// [`AngularInertia`] and [`CenterOfMass`] components. +/// +/// ## Example +/// +/// ```no_run +#[cfg_attr(feature = "2d", doc = "use avian2d::prelude::*;")] +#[cfg_attr(feature = "3d", doc = "use avian3d::prelude::*;")] +/// use bevy::prelude::*; +/// +/// fn main() { +/// App::new() +/// .add_plugins((DefaultPlugins, PhysicsPlugins::default())) +/// .add_systems(Startup, setup) +/// .add_systems(Update, print_collider_masses) +/// .run(); +/// } +/// +/// fn setup(mut commands: Commands) { +#[cfg_attr(feature = "2d", doc = " commands.spawn(Collider::circle(0.5));")] +#[cfg_attr(feature = "3d", doc = " commands.spawn(Collider::sphere(0.5));")] +/// } +/// +/// fn print_collider_masses(query: Query<&ColliderMassProperties>) { +/// for mass_props in &query { +/// println!("{}", mass_props.mass); +/// } +/// } +/// ``` +#[derive(Reflect, Clone, Copy, Component, Debug, PartialEq)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, Component, PartialEq)] +pub struct ColliderMassProperties { + /// Mass given by the collider. + pub mass: Scalar, + + /// Angular inertia given by the collider. + #[cfg(feature = "2d")] + pub angular_inertia: Scalar, + + /// Angular inertia tensor given by the collider. + #[cfg(feature = "3d")] + pub angular_inertia: Matrix, + + /// Local center of mass given by the collider. + pub center_of_mass: Vector, +} + +impl ColliderMassProperties { + /// The collider has no mass. + pub const ZERO: Self = Self { + mass: 0.0, + #[cfg(feature = "2d")] + angular_inertia: 0.0, + #[cfg(feature = "3d")] + angular_inertia: Matrix::ZERO, + center_of_mass: Vector::ZERO, + }; + + /// The collider has infinite mass. + pub const INFINITY: Self = Self { + mass: Scalar::INFINITY, + #[cfg(feature = "2d")] + angular_inertia: Scalar::INFINITY, + #[cfg(feature = "3d")] + angular_inertia: Matrix::from_diagonal(Vector::INFINITY), + center_of_mass: Vector::ZERO, + }; + + /// Computes mass properties from a given collider and density. + /// + /// Because [`ColliderMassProperties`] is read-only, adding this as a component manually + /// has no effect. The mass properties will be recomputed using the [`ColliderDensity`]. + pub fn new(collider: &C, density: Scalar) -> Self { + collider.mass_properties(density) + } + + /// Transforms the center of mass by the given [`ColliderTransform`]. + #[inline] + pub fn transformed_by(mut self, transform: &ColliderTransform) -> Self { + self.center_of_mass = transform.transform_point(self.center_of_mass); + self + } + + /// Computes the angular inertia shifted by the given offset, taking into account mass. + #[cfg(feature = "2d")] + #[inline] + pub fn shifted_angular_inertia(&self, offset: Vector) -> Scalar { + shifted_angular_inertia(self.angular_inertia, self.mass, offset) + } + + /// Computes the angular inertia shifted by the given offset, taking into account mass. + #[cfg(feature = "3d")] + #[inline] + pub fn shifted_angular_inertia(&self, offset: Vector) -> Matrix { + shifted_angular_inertia(self.angular_inertia, self.mass, offset) + } + + /// Computes the inverse angular inertia shifted by the given offset, taking into account mass. + #[cfg(feature = "2d")] + #[inline] + pub fn shifted_inverse_angular_inertia(&self, offset: Vector) -> Scalar { + shifted_angular_inertia(self.angular_inertia, self.mass, offset).recip_or_zero() + } + + /// Computes the inverse angular inertia shifted by the given offset, taking into account mass. + #[cfg(feature = "3d")] + #[inline] + pub fn shifted_inverse_angular_inertia(&self, offset: Vector) -> Matrix { + shifted_angular_inertia(self.angular_inertia, self.mass, offset).inverse_or_zero() + } +} + +impl Default for ColliderMassProperties { + fn default() -> Self { + Self::ZERO + } +} + +#[cfg(feature = "2d")] +#[inline] +pub(crate) fn shifted_angular_inertia( + angular_inertia: Scalar, + mass: Scalar, + offset: Vector, +) -> Scalar { + if mass > 0.0 && mass.is_finite() && offset != Vector::ZERO { + angular_inertia + offset.length_squared() * mass + } else { + angular_inertia + } +} + +#[cfg(feature = "3d")] +#[inline] +pub(crate) fn shifted_angular_inertia(tensor: Matrix, mass: Scalar, offset: Vector) -> Matrix { + if mass > 0.0 && mass.is_finite() && offset != Vector::ZERO { + let diagonal_element = offset.length_squared(); + let diagonal_mat = Matrix3::from_diagonal(Vector::splat(diagonal_element)); + let offset_outer_product = + Matrix3::from_cols(offset * offset.x, offset * offset.y, offset * offset.z); + tensor + (diagonal_mat + offset_outer_product) * mass + } else { + tensor + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[cfg(feature = "3d")] + use approx::assert_relative_eq; + + #[test] + fn mass_creation() { + let mass = Mass::new(10.0); + assert_eq!(mass, Mass::from_inverse(0.1)); + assert_eq!(mass.value(), 10.0); + assert_eq!(mass.inverse(), 0.1); + } + + #[test] + fn zero_mass() { + // Zero mass should be equivalent to infinite mass. + let mass = Mass::new(0.0); + assert_eq!(mass, Mass::new(Scalar::INFINITY)); + assert_eq!(mass, Mass::from_inverse(0.0)); + assert_eq!(mass.value(), 0.0); + assert_eq!(mass.inverse(), 0.0); + assert!(mass.is_infinite()); + assert!(!mass.is_finite()); + assert!(!mass.is_nan()); + } + + #[test] + fn infinite_mass() { + let mass = Mass::INFINITY; + assert_eq!(mass, Mass::new(Scalar::INFINITY)); + assert_eq!(mass, Mass::from_inverse(0.0)); + assert_eq!(mass.value(), 0.0); + assert_eq!(mass.inverse(), 0.0); + assert!(mass.is_infinite()); + assert!(!mass.is_finite()); + assert!(!mass.is_nan()); + } + + #[test] + #[should_panic] + fn negative_mass_panics() { + Mass::new(-1.0); + } + + #[test] + fn negative_mass_error() { + assert_eq!( + Mass::try_new(-1.0), + Err(MassError::Negative), + "negative mass should return an error" + ); + } + + #[test] + fn nan_mass_error() { + assert_eq!( + Mass::try_new(Scalar::NAN), + Err(MassError::NaN), + "NaN mass should return an error" + ); + } + + #[test] + #[cfg(feature = "2d")] + fn angular_inertia_creation() { + let angular_inertia = AngularInertia::new(10.0); + assert_eq!(angular_inertia, AngularInertia::from_inverse(0.1)); + assert_eq!(angular_inertia.value(), 10.0); + assert_eq!(angular_inertia.inverse(), 0.1); + } + + #[test] + #[cfg(feature = "2d")] + fn zero_angular_inertia() { + // Zero angular inertia should be equivalent to infinite angular inertia. + let angular_inertia = AngularInertia::new(0.0); + assert_eq!(angular_inertia, AngularInertia::new(Scalar::INFINITY)); + assert_eq!(angular_inertia, AngularInertia::from_inverse(0.0)); + assert_eq!(angular_inertia.value(), 0.0); + assert_eq!(angular_inertia.inverse(), 0.0); + assert!(angular_inertia.is_infinite()); + assert!(!angular_inertia.is_finite()); + assert!(!angular_inertia.is_nan()); + } + + #[test] + #[cfg(feature = "2d")] + fn infinite_angular_inertia() { + let angular_inertia = AngularInertia::INFINITY; + assert_eq!(angular_inertia, AngularInertia::new(Scalar::INFINITY)); + assert_eq!(angular_inertia, AngularInertia::from_inverse(0.0)); + assert_eq!(angular_inertia.value(), 0.0); + assert_eq!(angular_inertia.inverse(), 0.0); + assert!(angular_inertia.is_infinite()); + assert!(!angular_inertia.is_finite()); + assert!(!angular_inertia.is_nan()); + } + + #[test] + #[should_panic] + #[cfg(feature = "2d")] + fn negative_angular_inertia_panics() { + AngularInertia::new(-1.0); + } + + #[test] + #[cfg(feature = "2d")] + fn negative_angular_inertia_error() { + assert_eq!( + AngularInertia::try_new(-1.0), + Err(AngularInertiaError::Negative), + "negative angular inertia should return an error" + ); + } + + #[test] + #[cfg(feature = "2d")] + fn nan_angular_inertia_error() { + assert_eq!( + AngularInertia::try_new(Scalar::NAN), + Err(AngularInertiaError::NaN), + "NaN angular inertia should return an error" + ); + } + + #[test] + #[cfg(feature = "3d")] + fn angular_inertia_creation() { + let angular_inertia = AngularInertia::new(Vector::new(10.0, 20.0, 30.0)); + assert_relative_eq!( + angular_inertia.inverse_tensor(), + AngularInertia::from_inverse_tensor(Matrix::from_diagonal(Vector::new( + 0.1, + 0.05, + 1.0 / 30.0 + ))) + .inverse_tensor() + ); + assert_relative_eq!( + angular_inertia.tensor(), + Matrix::from_diagonal(Vector::new(10.0, 20.0, 30.0)) + ); + assert_relative_eq!( + angular_inertia.inverse_tensor(), + Matrix::from_diagonal(Vector::new(0.1, 0.05, 1.0 / 30.0)) + ); + } + + #[test] + #[cfg(feature = "3d")] + fn zero_angular_inertia() { + let angular_inertia = AngularInertia::new(Vector::ZERO); + assert_eq!(angular_inertia, AngularInertia::new(Vector::INFINITY)); + assert_eq!( + angular_inertia, + AngularInertia::from_inverse_tensor(Matrix::from_diagonal(Vector::ZERO)) + ); + assert_relative_eq!(angular_inertia.tensor(), Matrix::ZERO); + assert_relative_eq!(angular_inertia.inverse_tensor(), Matrix::ZERO); + assert!(angular_inertia.is_infinite()); + assert!(!angular_inertia.is_finite()); + assert!(!angular_inertia.is_nan()); + } + + #[test] + #[cfg(feature = "3d")] + fn infinite_angular_inertia() { + let angular_inertia = AngularInertia::INFINITY; + assert_eq!(angular_inertia, AngularInertia::new(Vector::INFINITY)); + assert_eq!( + angular_inertia, + AngularInertia::from_inverse_tensor(Matrix::ZERO) + ); + assert_relative_eq!(angular_inertia.tensor(), Matrix::ZERO); + assert_relative_eq!(angular_inertia.inverse_tensor(), Matrix::ZERO); + assert!(angular_inertia.is_infinite()); + assert!(!angular_inertia.is_finite()); + assert!(!angular_inertia.is_nan()); + } + + #[test] + #[should_panic] + #[cfg(feature = "3d")] + fn negative_angular_inertia_panics() { + AngularInertia::new(Vector::new(-1.0, 2.0, 3.0)); + } + + #[test] + #[cfg(feature = "3d")] + fn negative_angular_inertia_error() { + assert_eq!( + AngularInertia::try_new(Vector::new(-1.0, 2.0, 3.0)), + Err(AngularInertiaError::Negative), + "negative angular inertia should return an error" + ); + } + + #[test] + #[cfg(feature = "3d")] + fn nan_angular_inertia_error() { + assert_eq!( + AngularInertia::try_new(Vector::new(Scalar::NAN, 2.0, 3.0)), + Err(AngularInertiaError::NaN), + "NaN angular inertia should return an error" + ); + } +} diff --git a/src/dynamics/rigid_body/mass_properties/mod.rs b/src/dynamics/rigid_body/mass_properties/mod.rs index 9b8e71e6..1e4263aa 100644 --- a/src/dynamics/rigid_body/mass_properties/mod.rs +++ b/src/dynamics/rigid_body/mass_properties/mod.rs @@ -1,1182 +1,151 @@ -use crate::prelude::*; -use bevy::prelude::*; -use derive_more::From; +//! Mass property functionality for rigid bodies. + +#[cfg(feature = "3d")] +use crate::prepare::match_any; +use crate::{prelude::*, prepare::PrepareSet}; +#[cfg(feature = "3d")] +use bevy::ecs::query::QueryFilter; +use bevy::{ + ecs::{intern::Interned, schedule::ScheduleLabel}, + prelude::*, +}; + +mod components; +pub use components::*; mod world_query; pub use world_query::MassPropertiesQuery; -/// An error returned for an invalid mass. -#[derive(Clone, Copy, Debug, PartialEq)] -pub enum MassError { - /// The mass is negative. - Negative, - /// The mass is NaN. - NaN, -} - -/// The mass of a dynamic [rigid body]. -/// -/// Note that zero mass is treated as a special case, and is used to represent infinite mass. -/// -/// [rigid body]: RigidBody -/// -/// ## Representation -/// -/// Internally, the mass is actually stored as the inverse mass `1.0 / mass`. -/// This is because most physics calculations operate on the inverse mass, and storing it directly -/// allows for fewer divisions and guards against division by zero. -/// -/// When using [`Mass`], you shouldn't need to worry about this internal representation. -/// The provided constructors and getters abstract away the implementation details. +/// A plugin for managing mass properties of rigid bodies. /// -/// In terms of performance, the main thing to keep in mind is that [`inverse`](Self::inverse) is a no-op -/// and [`value`](Self::value) contains a division. When dividing by the mass, it's better to use -/// `foo * mass.inverse()` than `foo / mass.value()`. -#[derive(Reflect, Clone, Copy, Component, Debug, Default, PartialEq)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, Default, PartialEq)] -pub struct Mass { - /// The inverse mass. - /// - /// This is stored as an inverse because most physics calculations - /// operate on the inverse mass, and storing it directly allows for - /// fewer divisions and guards against division by zero. - inverse: Scalar, +/// - Updates mass properties of rigid bodies when colliders are added or removed, or when their [`ColliderMassProperties`] are changed. +/// - Logs warnings when dynamic bodies have invalid [`Mass`] or [`AngularInertia`]. +pub struct MassPropertyPlugin { + schedule: Interned, } -impl Mass { - /// Infinite mass. - pub const INFINITY: Self = Self { inverse: 0.0 }; - - /// Creates a new [`Mass`] from the given mass. - /// - /// # Panics - /// - /// Panics if the mass is negative when `debug_assertions` are enabled. - #[inline] - pub fn new(mass: Scalar) -> Self { - Self::from_inverse(mass.recip_or_zero()) - } - - /// Tries to create a new [`Mass`] from the given mass. - /// - /// # Errors - /// - /// Returns [`Err(MassError)`](MassError) if the mass is negative or NaN. - #[inline] - pub fn try_new(mass: Scalar) -> Result { - if mass.is_nan() { - Err(MassError::NaN) - } else if mass < 0.0 { - Err(MassError::Negative) - } else { - Ok(Self::from_inverse(mass.recip_or_zero())) - } - } - - /// Creates a new [`Mass`] from the given inverse mass. - /// - /// # Panics +impl MassPropertyPlugin { + /// Creates a [`MassPropertyPlugin`] with the schedule that is used for running the [`PhysicsSchedule`]. /// - /// Panics if the inverse mass is negative when `debug_assertions` are enabled. - #[inline] - pub fn from_inverse(inverse_mass: Scalar) -> Self { - debug_assert!( - inverse_mass >= 0.0 && !inverse_mass.is_nan(), - "mass must be positive or zero" - ); - + /// The default schedule is `FixedPostUpdate`. + pub fn new(schedule: impl ScheduleLabel) -> Self { Self { - inverse: inverse_mass, + schedule: schedule.intern(), } } - - /// Tries to create a new [`Mass`] from the given inverse mass. - /// - /// # Errors - /// - /// Returns [`Err(MassError)`](MassError) if the inverse mass is negative or NaN. - pub fn try_from_inverse(inverse_mass: Scalar) -> Result { - if inverse_mass.is_nan() { - Err(MassError::NaN) - } else if inverse_mass < 0.0 { - Err(MassError::Negative) - } else { - Ok(Self { - inverse: inverse_mass, - }) - } - } - - /// Returns the mass. If it is infinite, returns zero. - /// - /// Note that this involves a division because [`Mass`] internally stores the inverse mass. - /// If dividing by the mass, consider using `foo * mass.inverse()` instead of `foo / mass.value()`. - #[inline] - pub fn value(self) -> Scalar { - self.inverse.recip_or_zero() - } - - /// Returns the inverse mass. - /// - /// This is a no-op because [`Mass`] internally stores the inverse mass. - #[inline] - pub fn inverse(self) -> Scalar { - self.inverse - } - - /// Sets the mass. - #[inline] - pub fn set(&mut self, mass: impl Into) { - *self = mass.into(); - } - - /// Returns `true` if the mass is neither infinite nor NaN. - #[inline] - pub fn is_finite(self) -> bool { - !self.is_infinite() && !self.is_nan() - } - - /// Returns `true` if the mass is positive infinity or negative infinity. - #[inline] - pub fn is_infinite(self) -> bool { - self == Self::INFINITY - } - - /// Returns `true` if the mass is NaN. - #[inline] - pub fn is_nan(self) -> bool { - self.inverse.is_nan() - } -} - -impl From for Mass { - fn from(mass: Scalar) -> Self { - Self::new(mass) - } } -// TODO: Add errors for asymmetric and non-positive definite matrices in 3D. -/// An error returned for an invalid angular inertia. -#[derive(Clone, Copy, Debug, PartialEq)] -pub enum AngularInertiaError { - /// The angular inertia is negative. - Negative, - /// The angular inertia is NaN. - NaN, -} - -/// The moment of inertia of a dynamic [rigid body]. This represents the torque needed for a desired angular acceleration. -/// -/// Note that zero angular inertia is treated as a special case, and is used to represent infinite angular inertia. -/// -/// [rigid body]: RigidBody -/// -/// ## Representation -/// -/// Internally, the angular inertia is actually stored as the inverse angular inertia `1.0 / angular_inertia`. -/// This is because most physics calculations operate on the inverse angular inertia, and storing it directly -/// allows for fewer divisions and guards against division by zero. -/// -/// When using [`AngularInertia`], you shouldn't need to worry about this internal representation. -/// The provided constructors and getters abstract away the implementation details. -/// -/// In terms of performance, the main thing to keep in mind is that [`inverse`](Self::inverse) is a no-op -/// and [`value`](Self::value) contains a division. When dividing by the angular inertia, it's better to use -/// `foo * angular_inertia.inverse()` than `foo / angular_inertia.value()`. -#[cfg(feature = "2d")] -#[derive(Reflect, Clone, Copy, Component, Debug, PartialEq)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, Default, PartialEq)] -pub struct AngularInertia { - /// The inverse angular inertia. - /// - /// This is stored as an inverse to minimize the number of divisions - /// and to guard against division by zero. Most physics calculations - /// use the inverse angular inertia. - inverse: Scalar, -} - -#[cfg(feature = "2d")] -impl AngularInertia { - /// Infinite angular inertia. - pub const INFINITY: Self = Self { inverse: 0.0 }; - - /// Creates a new [`AngularInertia`] from the given angular inertia. - /// - /// # Panics - /// - /// Panics if the angular inertia is negative when `debug_assertions` are enabled. - #[inline] - pub fn new(angular_inertia: Scalar) -> Self { - Self::from_inverse(angular_inertia.recip_or_zero()) - } - - /// Tries to create a new [`AngularInertia`] from the given angular inertia. - /// - /// # Errors - /// - /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if the angular inertia is negative or NaN. - #[inline] - pub fn try_new(angular_inertia: Scalar) -> Result { - if angular_inertia.is_nan() { - Err(AngularInertiaError::NaN) - } else if angular_inertia < 0.0 { - Err(AngularInertiaError::Negative) - } else { - Ok(Self::from_inverse(angular_inertia.recip_or_zero())) - } - } - - /// Creates a new [`AngularInertia`] from the given inverse angular inertia. - /// - /// # Panics - /// - /// Panics if the inverse angular inertia is negative when `debug_assertions` are enabled. - #[inline] - pub fn from_inverse(inverse_angular_inertia: Scalar) -> Self { - debug_assert!( - inverse_angular_inertia >= 0.0 && !inverse_angular_inertia.is_nan(), - "angular inertia must be positive or zero" - ); - - Self { - inverse: inverse_angular_inertia, - } - } - - /// Tries to create a new [`AngularInertia`] from the given inverse angular inertia. - /// - /// # Errors - /// - /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if the inverse angular inertia is negative or NaN. - #[inline] - pub fn try_from_inverse(inverse_angular_inertia: Scalar) -> Result { - if inverse_angular_inertia.is_nan() { - Err(AngularInertiaError::NaN) - } else if inverse_angular_inertia < 0.0 { - Err(AngularInertiaError::Negative) - } else { - Ok(Self { - inverse: inverse_angular_inertia, - }) - } - } - - /// Returns the angular inertia. If it is infinite, returns zero. - /// - /// Note that this involves a division because [`AngularInertia`] internally stores the inverse angular inertia. - /// If dividing by the angular inertia, consider using `foo * angular_inertia.inverse()` instead of `foo / angular_inertia.value()`. - #[inline] - pub fn value(self) -> Scalar { - self.inverse.recip_or_zero() - } - - /// Returns the inverse angular inertia. - /// - /// This is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. - #[inline] - pub fn inverse(self) -> Scalar { - self.inverse - } - - /// Returns a mutable reference to the inverse of the angular inertia. - /// - /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. - #[inline] - pub fn inverse_mut(&mut self) -> &mut Scalar { - &mut self.inverse - } - - /// Sets the angular inertia. - #[inline] - pub fn set(&mut self, angular_inertia: impl Into) { - *self = angular_inertia.into(); - } - - /// Computes the angular inertia shifted by the given offset, taking into account the given mass. - #[inline] - pub fn shifted(&self, mass: Scalar, offset: Vector) -> Scalar { - shifted_angular_inertia(self.value(), mass, offset) - } - - /// Computes the angular inertia shifted by the given offset, taking into account the given mass. - #[inline] - pub fn shifted_inverse(&self, mass: Scalar, offset: Vector) -> Scalar { - shifted_angular_inertia(self.value(), mass, offset).recip_or_zero() - } - - /// Returns `true` if the angular inertia is neither infinite nor NaN. - #[inline] - pub fn is_finite(self) -> bool { - !self.is_infinite() && !self.is_nan() - } - - /// Returns `true` if the angular inertia is positive infinity or negative infinity. - #[inline] - pub fn is_infinite(self) -> bool { - self == Self::INFINITY - } - - /// Returns `true` if the angular inertia is NaN. - #[inline] - pub fn is_nan(self) -> bool { - self.inverse.is_nan() - } -} - -#[cfg(feature = "2d")] -impl From for AngularInertia { - fn from(angular_inertia: Scalar) -> Self { - Self::new(angular_inertia) - } -} - -/// The local moment of inertia of a dynamic [rigid body] as a 3x3 tensor matrix. -/// This represents the torque needed for a desired angular acceleration about the XYZ axes. -/// -/// This is computed in local space, so the object's orientation is not taken into account. -/// The world-space version is stored in [`GlobalAngularInertia`], which is automatically recomputed -/// whenever the local angular inertia or rotation is changed. -/// -/// To manually compute the world-space version that takes the body's rotation into account, -/// use the associated `rotated` method. Note that this operation is quite expensive, so use it sparingly. -/// -/// The angular inertia tensor should be symmetric and positive definite. -/// -/// Note that zero angular inertia is treated as a special case, and is used to represent infinite angular inertia. -/// -/// [rigid body]: RigidBody -/// -/// ## Representation -/// -/// Internally, the angular inertia is actually stored as the inverse angular inertia tensor `angular_inertia_matrix.inverse()`. -/// This is because most physics calculations operate on the inverse angular inertia, and storing it directly -/// allows for fewer inversions and guards against division by zero. -/// -/// When using [`AngularInertia`], you shouldn't need to worry about this internal representation. -/// The provided constructors and getters abstract away the implementation details. -/// -/// In terms of performance, the main thing to keep in mind is that [`inverse`](Self::inverse) is a no-op -/// and [`value`](Self::value) contains an inversion. When multiplying by the inverse angular inertia, it's better to use -/// `angular_inertia.inverse() * foo` than `angular_inertia.value().inverse() * foo`. -#[cfg(feature = "3d")] -#[derive(Reflect, Clone, Copy, Component, Debug, PartialEq)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, PartialEq)] -pub struct AngularInertia { - // TODO: The matrix should be symmetric and positive definite. - // We could add a custom `SymmetricMat3` type to enforce symmetricity and reduce memory usage. - inverse: Matrix, -} - -impl Default for AngularInertia { +impl Default for MassPropertyPlugin { fn default() -> Self { - Self::INFINITY - } -} - -// TODO: Add helpers for getting the principal angular inertia and local inertial frame. This requires an eigensolver. -// `bevy_heavy` has this functionality. -#[cfg(feature = "3d")] -impl AngularInertia { - /// Infinite angular inertia. - pub const INFINITY: Self = Self { - inverse: Matrix::ZERO, - }; - - /// Creates a new [`AngularInertia`] from the given principal angular inertia. - /// - /// The principal angular inertia represents the torque needed for a desired angular acceleration - /// about the local coordinate axes. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - /// - /// To specify the orientation of the local inertial frame, consider using [`AngularInertia::new_with_local_frame`]. - /// - /// # Panics - /// - /// Panics if any component of the principal angular inertia is negative when `debug_assertions` are enabled. - #[inline] - #[doc(alias = "from_principal_angular_inertia")] - pub fn new(principal_angular_inertia: Vector) -> Self { - debug_assert!( - principal_angular_inertia.cmpge(Vector::ZERO).all() - && !principal_angular_inertia.is_nan(), - "principal angular inertia must be positive or zero for all axes" + Self::new(FixedPostUpdate) + } +} + +impl Plugin for MassPropertyPlugin { + fn build(&self, app: &mut App) { + // Update mass properties of rigid bodies when the mass properties of attached colliders are changed. + // This includes adding, removing, or modifying colliders. + app.observe( + |trigger: Trigger, + collider_parents: Query<( + &ColliderParent, + &PreviousColliderTransform, + &ColliderTransform, + )>, + mut mass_properties: Query| { + let Ok((collider_parent, previous_collider_transform, collider_transform)) = + collider_parents.get(trigger.entity()) + else { + return; + }; + + if let Ok(mut mass_props) = mass_properties.get_mut(collider_parent.get()) { + let event = trigger.event(); + + // Subtract the collider's previous mass properties, if any. + if let Some(previous) = event.previous { + mass_props -= previous.transformed_by(previous_collider_transform); + } + + // Add the collider's current mass properties, if any. + if let Some(current) = event.current { + mass_props += current.transformed_by(collider_transform); + } + } + }, ); - Self::from_inverse_tensor(Matrix::from_diagonal( - principal_angular_inertia.recip_or_zero(), - )) - } - - /// Tries to create a new [`AngularInertia`] from the given principal angular inertia. - /// - /// The principal angular inertia represents the torque needed for a desired angular acceleration - /// about the local coordinate axes. To specify the orientation of the local inertial frame, - /// consider using [`AngularInertia::try_new_with_local_frame`]. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - /// - /// # Errors - /// - /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if any component of the principal angular inertia is negative or NaN. - #[inline] - pub fn try_new(principal_angular_inertia: Vector) -> Result { - if principal_angular_inertia.is_nan() { - Err(AngularInertiaError::NaN) - } else if !principal_angular_inertia.cmpge(Vector::ZERO).all() { - Err(AngularInertiaError::Negative) - } else { - Ok(Self::from_inverse_tensor(Matrix::from_diagonal( - principal_angular_inertia.recip_or_zero(), - ))) - } - } - - /// Creates a new [`AngularInertia`] from the given principal angular inertia - /// and the orientation of the local inertial frame. - /// - /// The principal angular inertia represents the torque needed for a desired angular acceleration - /// about the local coordinate axes defined by the given `orientation`. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - /// - /// # Panics - /// - /// Panics if any component of the principal angular inertia is negative when `debug_assertions` are enabled. - #[inline] - #[doc(alias = "from_principal_angular_inertia_with_local_frame")] - pub fn new_with_local_frame( - principal_angular_inertia: Vector, - orientation: Quaternion, - ) -> Self { - debug_assert!( - principal_angular_inertia.cmpge(Vector::ZERO).all() - && !principal_angular_inertia.is_nan(), - "principal angular inertia must be positive or zero for all axes" + // Update `GlobalAngularInertia` for new rigid bodies. + #[cfg(feature = "3d")] + app.add_systems( + self.schedule, + update_global_angular_inertia::> + .run_if(match_any::>) + .in_set(PrepareSet::Finalize), ); - Self::from_inverse_tensor( - Matrix::from_quat(orientation) - * Matrix::from_diagonal(principal_angular_inertia.recip_or_zero()) - * Matrix::from_quat(orientation.inverse()), - ) - } - - /// Tries to create a new [`AngularInertia`] from the given principal angular inertia - /// and the orientation of the local inertial frame. - /// - /// The principal angular inertia represents the torque needed for a desired angular acceleration - /// about the local coordinate axes defined by the given `orientation`. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - /// - /// # Errors - /// - /// Returns [`Err(AngularInertiaError)`](AngularInertiaError) if any component of the principal angular inertia is negative or NaN. - #[inline] - pub fn try_new_with_local_frame( - principal_angular_inertia: Vector, - orientation: Quaternion, - ) -> Result { - if principal_angular_inertia.is_nan() { - Err(AngularInertiaError::NaN) - } else if !principal_angular_inertia.cmpge(Vector::ZERO).all() { - Err(AngularInertiaError::Negative) - } else { - Ok(Self::from_inverse_tensor( - Matrix::from_quat(orientation) - * Matrix::from_diagonal(principal_angular_inertia.recip_or_zero()) - * Matrix::from_quat(orientation.inverse()), - )) - } - } - - /// Creates a new [`AngularInertia`] from the given angular inertia tensor. - /// - /// The tensor should be symmetric and positive definite. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - #[inline] - #[doc(alias = "from_mat3")] - pub fn from_tensor(tensor: Matrix) -> Self { - Self::from_inverse_tensor(tensor.inverse_or_zero()) - } - - /// Creates a new [`AngularInertia`] from the given angular inertia tensor. - /// - /// The tensor should be symmetric and positive definite. - #[inline] - #[doc(alias = "from_inverse_mat3")] - pub fn from_inverse_tensor(inverse_tensor: Matrix) -> Self { - Self { - inverse: inverse_tensor, - } - } - - /// Returns the angular inertia tensor. If it is infinite, returns zero. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - /// If multiplying by the inverse angular inertia, consider using `angular_inertia.inverse() * foo` - /// instead of `angular_inertia.value().inverse() * foo`. - /// - /// Equivalent to [`AngularInertia::tensor`]. - #[inline] - pub(crate) fn value(self) -> Matrix { - self.tensor() - } - - /// Returns the inverse of the angular inertia tensor. - /// - /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. - /// - /// Equivalent to [`AngularInertia::inverse_tensor`]. - #[inline] - pub(crate) fn inverse(self) -> Matrix { - self.inverse_tensor() - } - - /// Returns a mutable reference to the inverse of the angular inertia tensor. - /// - /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. - #[inline] - pub(crate) fn inverse_mut(&mut self) -> &mut Matrix { - self.inverse_tensor_mut() - } - - /// Returns the angular inertia tensor. - /// - /// Note that this involves an invertion because [`AngularInertia`] internally stores the inverse angular inertia. - /// If multiplying by the inverse angular inertia, consider using `angular_inertia.inverse() * foo` - /// instead of `angular_inertia.value().inverse() * foo`. - #[inline] - #[doc(alias = "as_mat3")] - pub fn tensor(self) -> Matrix { - self.inverse.inverse_or_zero() - } - - /// Returns the inverse of the angular inertia tensor. - /// - /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. - #[inline] - #[doc(alias = "as_inverse_mat3")] - pub fn inverse_tensor(self) -> Matrix { - self.inverse - } - - /// Returns a mutable reference to the inverse of the angular inertia tensor. - /// - /// Note that this is a no-op because [`AngularInertia`] internally stores the inverse angular inertia. - #[inline] - #[doc(alias = "as_inverse_mat3_mut")] - pub fn inverse_tensor_mut(&mut self) -> &mut Matrix { - &mut self.inverse - } - - /// Sets the angular inertia tensor. - #[inline] - pub fn set(&mut self, angular_inertia: impl Into) { - *self = angular_inertia.into(); - } - - /// Computes the angular inertia tensor with the given rotation. - /// - /// This can be used to transform local angular inertia to world space. - #[inline] - pub fn rotated(self, rotation: Quaternion) -> Self { - let rot_mat3 = Matrix::from_quat(rotation); - Self::from_inverse_tensor((rot_mat3 * self.inverse) * rot_mat3.transpose()) - } - - /// Computes the angular inertia tensor shifted by the given offset, taking into account the given mass. - #[inline] - pub fn shifted_tensor(&self, mass: Scalar, offset: Vector) -> Matrix3 { - shifted_angular_inertia(self.tensor(), mass, offset) - } - - /// Computes the inverse angular inertia tensor shifted by the given offset, taking into account the given mass. - #[inline] - pub fn shifted_inverse_tensor(&self, mass: Scalar, offset: Vector) -> Matrix3 { - shifted_angular_inertia(self.tensor(), mass, offset).inverse_or_zero() - } - - /// Returns `true` if the angular inertia is neither infinite nor NaN. - #[inline] - pub fn is_finite(self) -> bool { - !self.is_infinite() && !self.is_nan() - } - - /// Returns `true` if the angular inertia is positive infinity or negative infinity. - #[inline] - pub fn is_infinite(self) -> bool { - self == Self::INFINITY - } - - /// Returns `true` if the angular inertia is NaN. - #[inline] - pub fn is_nan(self) -> bool { - self.inverse.is_nan() - } -} - -#[cfg(feature = "3d")] -impl From for AngularInertia { - fn from(tensor: Matrix) -> Self { - Self::from_tensor(tensor) - } -} - -#[cfg(feature = "2d")] -impl core::ops::Mul for AngularInertia { - type Output = Scalar; - - #[inline] - fn mul(self, rhs: Scalar) -> Scalar { - self.value() * rhs - } -} - -impl core::ops::Mul for AngularInertia { - type Output = Vector; - - #[inline] - fn mul(self, rhs: Vector) -> Vector { - self.value() * rhs + app.add_systems( + self.schedule, + (warn_missing_mass, clamp_collider_density) + .chain() + .in_set(PrepareSet::Finalize), + ); } } -// In 2D, the global angular inertia is the same as the local angular inertia. -#[cfg(feature = "2d")] -pub(crate) type GlobalAngularInertia = AngularInertia; - -/// The world-space moment of inertia of a dynamic [rigid body] as a 3x3 tensor matrix. -/// This represents the torque needed for a desired angular acceleration about the XYZ axes. +/// Event triggered when the mass properties of a collider are changed. /// -/// This component is automatically updated whenever the local [`AngularInertia`] or rotation is changed. -/// To manually update it, use the associated [`update`](Self::update) method. -/// -/// [rigid body]: RigidBody -#[cfg(feature = "3d")] -#[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, PartialEq, From)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, PartialEq)] -pub struct GlobalAngularInertia(AngularInertia); - -#[cfg(feature = "3d")] -impl GlobalAngularInertia { - /// Creates a new [`GlobalAngularInertia`] from the given local angular inertia and rotation. - pub fn new( - local_angular_inertia: impl Into, - rotation: impl Into, - ) -> Self { - let local_angular_inertia: AngularInertia = local_angular_inertia.into(); - Self(local_angular_inertia.rotated(rotation.into())) - } - - /// Updates the global angular inertia with the given local angular inertia and rotation. - pub fn update( - &mut self, - local_angular_inertia: impl Into, - rotation: impl Into, - ) { - *self = Self::new(local_angular_inertia, rotation); - } +/// Used to update the mass properties of rigid bodies when colliders are added, removed, or modified. +#[derive(Event)] +pub struct OnChangeColliderMassProperties { + /// The previous mass properties of the collider. + pub previous: Option, + /// The current mass properties of the collider. + pub current: Option, } +/// Updates [`GlobalAngularInertia`] for entities that match the given query filter `F`. #[cfg(feature = "3d")] -impl From for AngularInertia { - fn from(inertia: GlobalAngularInertia) -> Self { - inertia.0 - } -} - -#[cfg(feature = "3d")] -impl From for GlobalAngularInertia { - fn from(tensor: Matrix) -> Self { - Self(AngularInertia::from_tensor(tensor)) - } -} - -/// The local center of mass of a dynamic [rigid body]. -/// -/// [rigid body]: RigidBody -#[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, DerefMut, PartialEq, From)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, Default, PartialEq)] -pub struct CenterOfMass(pub Vector); - -impl CenterOfMass { - /// A center of mass set at the local origin. - pub const ZERO: Self = Self(Vector::ZERO); -} - -/// A bundle containing mass properties. -/// -/// ## Example -/// -/// The easiest way to create a new bundle is to use the [`new_computed`](Self::new_computed) method -/// that computes the mass properties based on a given [`Collider`] and density. -/// -/// ``` -#[cfg_attr(feature = "2d", doc = "use avian2d::prelude::*;")] -#[cfg_attr(feature = "3d", doc = "use avian3d::prelude::*;")] -/// use bevy::prelude::*; -/// -/// fn setup(mut commands: Commands) { -/// commands.spawn(( -/// RigidBody::Dynamic, -#[cfg_attr( - feature = "2d", - doc = " MassPropertiesBundle::new_computed(&Collider::circle(0.5), 1.0)," -)] -#[cfg_attr( - feature = "3d", - doc = " MassPropertiesBundle::new_computed(&Collider::sphere(0.5), 1.0)," -)] -/// )); -/// } -/// ``` -#[allow(missing_docs)] -#[derive(Bundle, Debug, Default, Clone, PartialEq)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -pub struct MassPropertiesBundle { - pub mass: Mass, - pub angular_inertia: AngularInertia, - pub center_of_mass: CenterOfMass, -} - -impl MassPropertiesBundle { - /// Computes the mass properties for a [`Collider`] based on its shape and a given density. - #[cfg(all( - feature = "default-collider", - any(feature = "parry-f32", feature = "parry-f64") - ))] - pub fn new_computed(collider: &Collider, density: Scalar) -> Self { - let ColliderMassProperties { - mass, - angular_inertia, - center_of_mass, - .. - } = collider.mass_properties(density); - - Self { - mass: Mass::new(mass), - #[cfg(feature = "2d")] - angular_inertia: AngularInertia::new(angular_inertia), - #[cfg(feature = "3d")] - angular_inertia: AngularInertia::from_tensor(angular_inertia), - center_of_mass: CenterOfMass(center_of_mass), - } - } -} - -/// The density of a [`Collider`], 1.0 by default. This is used for computing -/// the [`ColliderMassProperties`] for each collider. -/// -/// ## Example -/// -/// ``` -#[cfg_attr(feature = "2d", doc = "use avian2d::prelude::*;")] -#[cfg_attr(feature = "3d", doc = "use avian3d::prelude::*;")] -/// use bevy::prelude::*; -/// -/// // Spawn a body with a collider that has a density of 2.5 -/// fn setup(mut commands: Commands) { -/// commands.spawn(( -/// RigidBody::Dynamic, -#[cfg_attr(feature = "2d", doc = " Collider::circle(0.5),")] -#[cfg_attr(feature = "3d", doc = " Collider::sphere(0.5),")] -/// ColliderDensity(2.5), -/// )); -/// } -/// ``` -#[derive(Reflect, Clone, Copy, Component, Debug, Deref, DerefMut, PartialEq, PartialOrd)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, PartialEq)] -pub struct ColliderDensity(pub Scalar); - -impl ColliderDensity { - /// The density of the [`Collider`] is zero. It has no mass. - pub const ZERO: Self = Self(0.0); -} - -impl Default for ColliderDensity { - fn default() -> Self { - Self(1.0) - } -} - -impl From for ColliderDensity { - fn from(density: Scalar) -> Self { - Self(density) - } -} - -/// An automatically added component that contains the read-only mass properties of a [`Collider`]. -/// The density used for computing the mass properties can be configured using the [`ColliderDensity`] -/// component. -/// -/// These mass properties will be added to the [rigid body's](RigidBody) actual [`Mass`], -/// [`AngularInertia`] and [`CenterOfMass`] components. -/// -/// ## Example -/// -/// ```no_run -#[cfg_attr(feature = "2d", doc = "use avian2d::prelude::*;")] -#[cfg_attr(feature = "3d", doc = "use avian3d::prelude::*;")] -/// use bevy::prelude::*; -/// -/// fn main() { -/// App::new() -/// .add_plugins((DefaultPlugins, PhysicsPlugins::default())) -/// .add_systems(Startup, setup) -/// .add_systems(Update, print_collider_masses) -/// .run(); -/// } -/// -/// fn setup(mut commands: Commands) { -#[cfg_attr(feature = "2d", doc = " commands.spawn(Collider::circle(0.5));")] -#[cfg_attr(feature = "3d", doc = " commands.spawn(Collider::sphere(0.5));")] -/// } -/// -/// fn print_collider_masses(query: Query<&ColliderMassProperties>) { -/// for mass_props in &query { -/// println!("{}", mass_props.mass); -/// } -/// } -/// ``` -#[derive(Reflect, Clone, Copy, Component, Debug, PartialEq)] -#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] -#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] -#[reflect(Debug, Component, PartialEq)] -pub struct ColliderMassProperties { - /// Mass given by the collider. - pub mass: Scalar, - - /// Angular inertia given by the collider. - #[cfg(feature = "2d")] - pub angular_inertia: Scalar, - - /// Angular inertia tensor given by the collider. - #[cfg(feature = "3d")] - pub angular_inertia: Matrix, - - /// Local center of mass given by the collider. - pub center_of_mass: Vector, -} - -impl ColliderMassProperties { - /// The collider has no mass. - pub const ZERO: Self = Self { - mass: 0.0, +pub fn update_global_angular_inertia( + mut query: Query< + (&Rotation, &AngularInertia, &mut GlobalAngularInertia), + (Or<(Changed, Changed)>, F), + >, +) { + query + .par_iter_mut() + .for_each(|(rotation, angular_inertia, mut global_angular_inertia)| { + global_angular_inertia.update(*angular_inertia, rotation.0); + }); +} + +/// Logs warnings when dynamic bodies have invalid [`Mass`] or [`AngularInertia`]. +pub fn warn_missing_mass( + mut bodies: Query< + (Entity, &RigidBody, Ref, Ref), + Or<(Changed, Changed)>, + >, +) { + for (entity, rb, mass, inertia) in &mut bodies { + let is_mass_valid = mass.value().is_finite() && mass.value() >= Scalar::EPSILON; #[cfg(feature = "2d")] - angular_inertia: 0.0, + let is_inertia_valid = inertia.value().is_finite() && inertia.value() >= Scalar::EPSILON; #[cfg(feature = "3d")] - angular_inertia: Matrix::ZERO, - center_of_mass: Vector::ZERO, - }; - - /// The collider has infinite mass. - pub const INFINITY: Self = Self { - mass: Scalar::INFINITY, - #[cfg(feature = "2d")] - angular_inertia: Scalar::INFINITY, - #[cfg(feature = "3d")] - angular_inertia: Matrix::from_diagonal(Vector::INFINITY), - center_of_mass: Vector::ZERO, - }; - - /// Computes mass properties from a given collider and density. - /// - /// Because [`ColliderMassProperties`] is read-only, adding this as a component manually - /// has no effect. The mass properties will be recomputed using the [`ColliderDensity`]. - pub fn new(collider: &C, density: Scalar) -> Self { - collider.mass_properties(density) - } - - /// Transforms the center of mass by the given [`ColliderTransform`]. - #[inline] - pub fn transformed_by(mut self, transform: &ColliderTransform) -> Self { - self.center_of_mass = transform.transform_point(self.center_of_mass); - self - } - - /// Computes the angular inertia shifted by the given offset, taking into account mass. - #[cfg(feature = "2d")] - #[inline] - pub fn shifted_angular_inertia(&self, offset: Vector) -> Scalar { - shifted_angular_inertia(self.angular_inertia, self.mass, offset) - } - - /// Computes the angular inertia shifted by the given offset, taking into account mass. - #[cfg(feature = "3d")] - #[inline] - pub fn shifted_angular_inertia(&self, offset: Vector) -> Matrix { - shifted_angular_inertia(self.angular_inertia, self.mass, offset) - } - - /// Computes the inverse angular inertia shifted by the given offset, taking into account mass. - #[cfg(feature = "2d")] - #[inline] - pub fn shifted_inverse_angular_inertia(&self, offset: Vector) -> Scalar { - shifted_angular_inertia(self.angular_inertia, self.mass, offset).recip_or_zero() - } - - /// Computes the inverse angular inertia shifted by the given offset, taking into account mass. - #[cfg(feature = "3d")] - #[inline] - pub fn shifted_inverse_angular_inertia(&self, offset: Vector) -> Matrix { - shifted_angular_inertia(self.angular_inertia, self.mass, offset).inverse_or_zero() - } -} - -impl Default for ColliderMassProperties { - fn default() -> Self { - Self::ZERO - } -} - -#[cfg(feature = "2d")] -#[inline] -pub(crate) fn shifted_angular_inertia( - angular_inertia: Scalar, - mass: Scalar, - offset: Vector, -) -> Scalar { - if mass > 0.0 && mass.is_finite() && offset != Vector::ZERO { - angular_inertia + offset.length_squared() * mass - } else { - angular_inertia - } -} - -#[cfg(feature = "3d")] -#[inline] -pub(crate) fn shifted_angular_inertia(tensor: Matrix, mass: Scalar, offset: Vector) -> Matrix { - if mass > 0.0 && mass.is_finite() && offset != Vector::ZERO { - let diagonal_element = offset.length_squared(); - let diagonal_mat = Matrix3::from_diagonal(Vector::splat(diagonal_element)); - let offset_outer_product = - Matrix3::from_cols(offset * offset.x, offset * offset.y, offset * offset.z); - tensor + (diagonal_mat + offset_outer_product) * mass - } else { - tensor + let is_inertia_valid = inertia.value().is_finite(); + + // Warn about dynamic bodies with no mass or inertia + if rb.is_dynamic() && !(is_mass_valid && is_inertia_valid) { + warn!( + "Dynamic rigid body {:?} has no mass or inertia. This can cause NaN values. Consider adding a `MassPropertiesBundle` or a `Collider` with mass.", + entity + ); + } } } -#[cfg(test)] -mod tests { - use super::*; - #[cfg(feature = "3d")] - use approx::assert_relative_eq; - - #[test] - fn mass_creation() { - let mass = Mass::new(10.0); - assert_eq!(mass, Mass::from_inverse(0.1)); - assert_eq!(mass.value(), 10.0); - assert_eq!(mass.inverse(), 0.1); - } - - #[test] - fn zero_mass() { - // Zero mass should be equivalent to infinite mass. - let mass = Mass::new(0.0); - assert_eq!(mass, Mass::new(Scalar::INFINITY)); - assert_eq!(mass, Mass::from_inverse(0.0)); - assert_eq!(mass.value(), 0.0); - assert_eq!(mass.inverse(), 0.0); - assert!(mass.is_infinite()); - assert!(!mass.is_finite()); - assert!(!mass.is_nan()); - } - - #[test] - fn infinite_mass() { - let mass = Mass::INFINITY; - assert_eq!(mass, Mass::new(Scalar::INFINITY)); - assert_eq!(mass, Mass::from_inverse(0.0)); - assert_eq!(mass.value(), 0.0); - assert_eq!(mass.inverse(), 0.0); - assert!(mass.is_infinite()); - assert!(!mass.is_finite()); - assert!(!mass.is_nan()); - } - - #[test] - #[should_panic] - fn negative_mass_panics() { - Mass::new(-1.0); - } - - #[test] - fn negative_mass_error() { - assert_eq!( - Mass::try_new(-1.0), - Err(MassError::Negative), - "negative mass should return an error" - ); - } - - #[test] - fn nan_mass_error() { - assert_eq!( - Mass::try_new(Scalar::NAN), - Err(MassError::NaN), - "NaN mass should return an error" - ); - } - - #[test] - #[cfg(feature = "2d")] - fn angular_inertia_creation() { - let angular_inertia = AngularInertia::new(10.0); - assert_eq!(angular_inertia, AngularInertia::from_inverse(0.1)); - assert_eq!(angular_inertia.value(), 10.0); - assert_eq!(angular_inertia.inverse(), 0.1); - } - - #[test] - #[cfg(feature = "2d")] - fn zero_angular_inertia() { - // Zero angular inertia should be equivalent to infinite angular inertia. - let angular_inertia = AngularInertia::new(0.0); - assert_eq!(angular_inertia, AngularInertia::new(Scalar::INFINITY)); - assert_eq!(angular_inertia, AngularInertia::from_inverse(0.0)); - assert_eq!(angular_inertia.value(), 0.0); - assert_eq!(angular_inertia.inverse(), 0.0); - assert!(angular_inertia.is_infinite()); - assert!(!angular_inertia.is_finite()); - assert!(!angular_inertia.is_nan()); - } - - #[test] - #[cfg(feature = "2d")] - fn infinite_angular_inertia() { - let angular_inertia = AngularInertia::INFINITY; - assert_eq!(angular_inertia, AngularInertia::new(Scalar::INFINITY)); - assert_eq!(angular_inertia, AngularInertia::from_inverse(0.0)); - assert_eq!(angular_inertia.value(), 0.0); - assert_eq!(angular_inertia.inverse(), 0.0); - assert!(angular_inertia.is_infinite()); - assert!(!angular_inertia.is_finite()); - assert!(!angular_inertia.is_nan()); - } - - #[test] - #[should_panic] - #[cfg(feature = "2d")] - fn negative_angular_inertia_panics() { - AngularInertia::new(-1.0); - } - - #[test] - #[cfg(feature = "2d")] - fn negative_angular_inertia_error() { - assert_eq!( - AngularInertia::try_new(-1.0), - Err(AngularInertiaError::Negative), - "negative angular inertia should return an error" - ); - } - - #[test] - #[cfg(feature = "2d")] - fn nan_angular_inertia_error() { - assert_eq!( - AngularInertia::try_new(Scalar::NAN), - Err(AngularInertiaError::NaN), - "NaN angular inertia should return an error" - ); - } - - #[test] - #[cfg(feature = "3d")] - fn angular_inertia_creation() { - let angular_inertia = AngularInertia::new(Vector::new(10.0, 20.0, 30.0)); - assert_relative_eq!( - angular_inertia.inverse_tensor(), - AngularInertia::from_inverse_tensor(Matrix::from_diagonal(Vector::new( - 0.1, - 0.05, - 1.0 / 30.0 - ))) - .inverse_tensor() - ); - assert_relative_eq!( - angular_inertia.tensor(), - Matrix::from_diagonal(Vector::new(10.0, 20.0, 30.0)) - ); - assert_relative_eq!( - angular_inertia.inverse_tensor(), - Matrix::from_diagonal(Vector::new(0.1, 0.05, 1.0 / 30.0)) - ); - } - - #[test] - #[cfg(feature = "3d")] - fn zero_angular_inertia() { - let angular_inertia = AngularInertia::new(Vector::ZERO); - assert_eq!(angular_inertia, AngularInertia::new(Vector::INFINITY)); - assert_eq!( - angular_inertia, - AngularInertia::from_inverse_tensor(Matrix::from_diagonal(Vector::ZERO)) - ); - assert_relative_eq!(angular_inertia.tensor(), Matrix::ZERO); - assert_relative_eq!(angular_inertia.inverse_tensor(), Matrix::ZERO); - assert!(angular_inertia.is_infinite()); - assert!(!angular_inertia.is_finite()); - assert!(!angular_inertia.is_nan()); - } - - #[test] - #[cfg(feature = "3d")] - fn infinite_angular_inertia() { - let angular_inertia = AngularInertia::INFINITY; - assert_eq!(angular_inertia, AngularInertia::new(Vector::INFINITY)); - assert_eq!( - angular_inertia, - AngularInertia::from_inverse_tensor(Matrix::ZERO) - ); - assert_relative_eq!(angular_inertia.tensor(), Matrix::ZERO); - assert_relative_eq!(angular_inertia.inverse_tensor(), Matrix::ZERO); - assert!(angular_inertia.is_infinite()); - assert!(!angular_inertia.is_finite()); - assert!(!angular_inertia.is_nan()); - } - - #[test] - #[should_panic] - #[cfg(feature = "3d")] - fn negative_angular_inertia_panics() { - AngularInertia::new(Vector::new(-1.0, 2.0, 3.0)); - } - - #[test] - #[cfg(feature = "3d")] - fn negative_angular_inertia_error() { - assert_eq!( - AngularInertia::try_new(Vector::new(-1.0, 2.0, 3.0)), - Err(AngularInertiaError::Negative), - "negative angular inertia should return an error" - ); - } - - #[test] - #[cfg(feature = "3d")] - fn nan_angular_inertia_error() { - assert_eq!( - AngularInertia::try_new(Vector::new(Scalar::NAN, 2.0, 3.0)), - Err(AngularInertiaError::NaN), - "NaN angular inertia should return an error" - ); +/// Clamps [`ColliderDensity`] to be above `0.0`. +fn clamp_collider_density(mut query: Query<&mut ColliderDensity, Changed>) { + for mut density in &mut query { + density.0 = density.max(Scalar::EPSILON); } } diff --git a/src/dynamics/rigid_body/mass_properties/world_query.rs b/src/dynamics/rigid_body/mass_properties/world_query.rs index 94df6d33..4379234a 100644 --- a/src/dynamics/rigid_body/mass_properties/world_query.rs +++ b/src/dynamics/rigid_body/mass_properties/world_query.rs @@ -22,21 +22,21 @@ impl<'w> MassPropertiesQueryItem<'w> { #[cfg(feature = "2d")] #[inline] pub fn shifted_angular_inertia(&self, offset: Vector) -> Scalar { - shifted_angular_inertia(self.angular_inertia.value(), self.mass.value(), offset) + super::shifted_angular_inertia(self.angular_inertia.value(), self.mass.value(), offset) } /// Computes the angular inertia shifted by the given offset, taking into account mass. #[cfg(feature = "3d")] #[inline] pub fn shifted_angular_inertia(&self, offset: Vector) -> Matrix { - shifted_angular_inertia(self.angular_inertia.tensor(), self.mass.value(), offset) + super::shifted_angular_inertia(self.angular_inertia.tensor(), self.mass.value(), offset) } /// Computes the inverse angular inertia shifted by the given offset, taking into account mass. #[cfg(feature = "2d")] #[inline] pub fn shifted_inverse_angular_inertia(&self, offset: Vector) -> Scalar { - shifted_angular_inertia(self.angular_inertia.value(), self.mass.value(), offset) + super::shifted_angular_inertia(self.angular_inertia.value(), self.mass.value(), offset) .recip_or_zero() } @@ -44,7 +44,7 @@ impl<'w> MassPropertiesQueryItem<'w> { #[cfg(feature = "3d")] #[inline] pub fn shifted_inverse_angular_inertia(&self, offset: Vector) -> Matrix { - shifted_angular_inertia(self.angular_inertia.tensor(), self.mass.value(), offset) + super::shifted_angular_inertia(self.angular_inertia.tensor(), self.mass.value(), offset) .inverse_or_zero() } } diff --git a/src/dynamics/rigid_body/mod.rs b/src/dynamics/rigid_body/mod.rs index 6b764ca0..5b336d38 100644 --- a/src/dynamics/rigid_body/mod.rs +++ b/src/dynamics/rigid_body/mod.rs @@ -1,14 +1,14 @@ //! Common components and bundles for rigid bodies. +pub mod mass_properties; + // Components mod forces; mod locked_axes; -mod mass_properties; mod world_query; pub use forces::{ExternalAngularImpulse, ExternalForce, ExternalImpulse, ExternalTorque}; pub use locked_axes::LockedAxes; -pub use mass_properties::*; pub use world_query::*; #[cfg(feature = "2d")] diff --git a/src/lib.rs b/src/lib.rs index a04a8d91..28304f2a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -479,7 +479,7 @@ pub mod prelude { }, dynamics::{self, ccd::SpeculativeMargin, prelude::*}, position::{Position, Rotation}, - prepare::{init_transforms, warn_missing_mass, PrepareConfig, PreparePlugin}, + prepare::{init_transforms, PrepareConfig, PreparePlugin}, schedule::*, spatial_query::{self, *}, sync::SyncPlugin, @@ -513,6 +513,7 @@ use prelude::*; /// | [`PhysicsSchedulePlugin`] | Sets up the physics engine by initializing the necessary schedules, sets and resources. | /// | [`PhysicsTypeRegistrationPlugin`] | Registers physics types to the `TypeRegistry` resource in `bevy_reflect`. | /// | [`PreparePlugin`] | Runs systems at the start of each physics frame. Initializes [rigid bodies](RigidBody) and updates components. | +/// | [`MassPropertyPlugin`] | Manages mass properties of dynamic [rigid bodies](RigidBody). | /// | [`ColliderBackendPlugin`] | Handles generic collider backend logic, like initializing colliders and AABBs and updating related components. | /// | [`ColliderHierarchyPlugin`] | Handles transform propagation and [`ColliderParent`] updates for colliders. | /// | [`BroadPhasePlugin`] | Collects pairs of potentially colliding entities into [`BroadCollisionPairs`] using [AABB](ColliderAabb) intersection checks. | @@ -709,6 +710,7 @@ impl PluginGroup for PhysicsPlugins { .add(PhysicsSchedulePlugin::new(self.schedule)) .add(PhysicsTypeRegistrationPlugin) .add(PreparePlugin::new(self.schedule)) + .add(MassPropertyPlugin::new(self.schedule)) .add(ColliderHierarchyPlugin::new(self.schedule)); #[cfg(all( diff --git a/src/prepare.rs b/src/prepare.rs index 7fc63ede..35d6ad34 100644 --- a/src/prepare.rs +++ b/src/prepare.rs @@ -6,11 +6,7 @@ use crate::{prelude::*, sync::SyncConfig}; use bevy::{ - ecs::{ - intern::Interned, - query::{QueryData, QueryFilter}, - schedule::ScheduleLabel, - }, + ecs::{intern::Interned, query::QueryFilter, schedule::ScheduleLabel}, prelude::*, }; @@ -18,8 +14,6 @@ use bevy::{ /// and updates components. /// /// - Adds missing rigid body components for entities with a [`RigidBody`] component -/// - Adds missing mass properties for entities with a [`RigidBody`] component -/// - Updates mass properties /// - Clamps restitution coefficients between 0 and 1 /// /// The [`Transform`] component will be initialized based on [`Position`] or [`Rotation`] @@ -162,18 +156,7 @@ impl Plugin for PreparePlugin { ) .add_systems( self.schedule, - ( - warn_missing_mass, - #[cfg(feature = "3d")] - update_global_angular_inertia::> - .run_if(match_any::>), - clamp_collider_density, - clamp_restitution, - // All the components we added above must exist before we can simulate the bodies. - apply_deferred, - ) - .chain() - .in_set(PrepareSet::Finalize), + clamp_restitution.in_set(PrepareSet::Finalize), ); } } @@ -410,61 +393,6 @@ pub fn init_transforms( } } -/// Updates [`GlobalAngularInertia`] for entities that match the given query filter `F`. -#[cfg(feature = "3d")] -pub fn update_global_angular_inertia( - mut query: Query< - (&Rotation, &AngularInertia, &mut GlobalAngularInertia), - (Or<(Changed, Changed)>, F), - >, -) { - query - .par_iter_mut() - .for_each(|(rotation, angular_inertia, mut global_angular_inertia)| { - global_angular_inertia.update(*angular_inertia, rotation.0); - }); -} - -#[derive(QueryData)] -struct RigidBodyInitializationQuery { - lin_vel: Option<&'static LinearVelocity>, - ang_vel: Option<&'static AngularVelocity>, - force: Option<&'static ExternalForce>, - torque: Option<&'static ExternalTorque>, - impulse: Option<&'static ExternalImpulse>, - angular_impulse: Option<&'static ExternalAngularImpulse>, - restitution: Option<&'static Restitution>, - friction: Option<&'static Friction>, - time_sleeping: Option<&'static TimeSleeping>, - mass: Option<&'static Mass>, - angular_inertia: Option<&'static AngularInertia>, - center_of_mass: Option<&'static CenterOfMass>, -} - -/// Logs warnings when dynamic bodies have invalid [`Mass`] or [`AngularInertia`]. -pub fn warn_missing_mass( - mut bodies: Query< - (Entity, &RigidBody, Ref, Ref), - Or<(Changed, Changed)>, - >, -) { - for (entity, rb, mass, inertia) in &mut bodies { - let is_mass_valid = mass.value().is_finite() && mass.value() >= Scalar::EPSILON; - #[cfg(feature = "2d")] - let is_inertia_valid = inertia.value().is_finite() && inertia.value() >= Scalar::EPSILON; - #[cfg(feature = "3d")] - let is_inertia_valid = inertia.value().is_finite(); - - // Warn about dynamic bodies with no mass or inertia - if rb.is_dynamic() && !(is_mass_valid && is_inertia_valid) { - warn!( - "Dynamic rigid body {:?} has no mass or inertia. This can cause NaN values. Consider adding a `MassPropertiesBundle` or a `Collider` with mass.", - entity - ); - } - } -} - /// Clamps coefficients of [restitution](Restitution) to be between 0.0 and 1.0. fn clamp_restitution(mut query: Query<&mut Restitution, Changed>) { for mut restitution in &mut query { @@ -472,13 +400,6 @@ fn clamp_restitution(mut query: Query<&mut Restitution, Changed>) { } } -/// Clamps [`ColliderDensity`] to be above 0.0. -fn clamp_collider_density(mut query: Query<&mut ColliderDensity, Changed>) { - for mut density in &mut query { - density.0 = density.max(Scalar::EPSILON); - } -} - #[cfg(test)] mod tests { use super::*;