Add ability to apply forces on physics entities

crates/modor_physics/src/components/collider.rs

@@ -1,9 +1,13 @@
-use crate::components::pipeline::{ColliderHandleRemoval, ColliderUpdate, Pipeline2D};
+use crate::components::collision_groups::CollisionGroupRegistry;
+use crate::components::dynamics::BodyUpdate;
+use crate::components::pipeline::{
+    ColliderHandleRemoval, Pipeline2D, UnsynchronizedHandleDeletion,
+};
 use crate::{CollisionGroup, Dynamics2D, Transform2D};
 use approx::AbsDiffEq;
 use modor::{
-    Custom, Entity, Filter, Not, Query, QuerySystemParam, QuerySystemParamWithLifetime, SingleMut,
-    With,
+    ComponentSystems, Custom, Entity, Filter, Not, Query, QuerySystemParam,
+    QuerySystemParamWithLifetime, SingleMut, With,
 };
 use modor_math::Vec2;
 use modor_resources::{ResKey, ResourceAccessor};
@@ -112,7 +116,6 @@ impl Collider2D {
     }
 
     #[run_as(action(ColliderUpdate))]
-    #[allow(clippy::float_cmp)]
     fn update_pipeline(
         &mut self,
         transform: &Transform2D,
@@ -126,23 +129,24 @@ impl Collider2D {
         let interactions = group.map_or_else(InteractionGroups::none, |group| group.interactions);
         let data = ColliderUserData::new(entity.id(), group.map_or(u64::MAX, |group| group.id));
         if let Some(collider) = self.handle.and_then(|handle| pipeline.collider_mut(handle)) {
-            if dynamics.is_none() && transform.position != transform.old_position {
+            if dynamics.is_none() {
                 collider.set_translation(vector![transform.position.x, transform.position.y]);
-            }
-            if transform.size != transform.old_size {
-                collider.set_shape(self.shape(transform));
-            }
-            if dynamics.is_none() && transform.rotation != transform.old_rotation {
                 collider.set_rotation(Rotation::new(transform.rotation));
             }
+            collider.set_shape(self.shape(transform));
             collider.set_collision_groups(interactions);
             collider.user_data = data.into();
         } else {
-            let builder = self.collider_builder(transform, interactions, data, dynamics.is_some());
-            self.handle = Some(pipeline.create_collider(builder, dynamics.and_then(|d| d.handle)));
+            let collider = self.create_collider(transform, interactions, data, dynamics.is_some());
+            self.handle = Some(pipeline.create_collider(collider, dynamics.and_then(|d| d.handle)));
         }
     }
 
+    #[run_after(component(Pipeline2D))]
+    fn sync_pipeline() {
+        // do nothing, this system only waits for collision update
+    }
+
     /// Returns the detected collisions.
     pub fn collisions(&self) -> &[Collision2D] {
         &self.collisions
@@ -188,22 +192,22 @@ impl Collider2D {
         }
     }
 
-    fn collider_builder(
+    fn create_collider(
         &mut self,
         transform: &Transform2D,
         interactions: InteractionGroups,
         data: ColliderUserData,
         has_dynamics: bool,
-    ) -> ColliderBuilder {
+    ) -> Collider {
         let mut collider = ColliderBuilder::new(self.shape(transform))
             .collision_groups(interactions)
             .active_collision_types(ActiveCollisionTypes::all())
             .active_hooks(ActiveHooks::FILTER_CONTACT_PAIRS | ActiveHooks::FILTER_INTERSECTION_PAIR)
-            .user_data(data.into());
+            .user_data(data.into())
+            .build();
         if !has_dynamics {
-            collider = collider
-                .translation(vector![transform.position.x, transform.position.y])
-                .rotation(transform.rotation);
+            collider.set_translation(vector![transform.position.x, transform.position.y]);
+            collider.set_rotation(Rotation::new(transform.rotation));
         }
         collider
     }
@@ -217,6 +221,14 @@ impl Collider2D {
     }
 }
 
+#[derive(Action)]
+pub(crate) struct ColliderUpdate(
+    UnsynchronizedHandleDeletion,
+    BodyUpdate,
+    <CollisionGroup as ComponentSystems>::Action,
+    <CollisionGroupRegistry as ComponentSystems>::Action,
+);
+
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
 pub(crate) enum Collider2DShape {
     Rectangle,

crates/modor_physics/src/components/dynamics.rs

@@ -1,10 +1,12 @@
-use crate::components::pipeline::{BodyHandleReset, BodyUpdate, Pipeline2D};
+use crate::components::pipeline::{BodyHandleReset, Pipeline2D, UnsynchronizedHandleDeletion};
 use crate::Transform2D;
 use modor::{Entity, Filter, Not, SingleMut, With};
 use modor_math::Vec2;
-use rapier2d::dynamics::{RigidBodyBuilder, RigidBodyHandle, RigidBodyType};
+use rapier2d::dynamics::{
+    MassProperties, RigidBody, RigidBodyBuilder, RigidBodyHandle, RigidBodyType,
+};
 use rapier2d::math::Rotation;
-use rapier2d::na::vector;
+use rapier2d::na::{point, vector};
 use rapier2d::prelude::nalgebra;
 
 /// The dynamics properties of a 2D entity.
@@ -51,6 +53,45 @@ pub struct Dynamics2D {
     ///
     /// Default value is `0.0`.
     pub angular_velocity: f32,
+    /// Force applied on the entity.
+    ///
+    /// Will not have any effect if mass is zero.
+    ///
+    /// The acceleration of the entity corresponds to the force of the entity divided by its mass.
+    ///
+    /// Default value is [`Vec2::ZERO`].
+    pub force: Vec2,
+    /// Torque applied on the entity.
+    ///
+    /// Will not have any effect if angular inertia is zero.
+    ///
+    /// Default value is `0.`.
+    pub torque: f32,
+    /// Mass of the entity.
+    ///
+    /// A mass of zero is considered as infinite. In this case, force will not have any effect.
+    ///
+    /// Default value is `0.`.
+    pub mass: f32,
+    /// Angular inertia of the entity.
+    ///
+    /// An angular inertia of zero is considered as infinite. In this case, torque will not have
+    /// any effect.
+    ///
+    /// Default value is `0.`.
+    pub angular_inertia: f32,
+    /// Linear damping of the entity.
+    ///
+    /// This coefficient is used to automatically slow down the translation of the entity.
+    ///
+    /// Default value is `0.`.
+    pub damping: f32,
+    /// Angular damping of the entity.
+    ///
+    /// This coefficient is used to automatically slow down the rotation of the entity.
+    ///
+    /// Default value is `0.`.
+    pub angular_damping: f32,
     pub(crate) handle: Option<RigidBodyHandle>,
 }
 
@@ -62,6 +103,12 @@ impl Dynamics2D {
         Self {
             velocity: Vec2::ZERO,
             angular_velocity: 0.,
+            force: Vec2::ZERO,
+            torque: 0.,
+            mass: 0.,
+            angular_inertia: 0.,
+            damping: 0.,
+            angular_damping: 0.,
             handle: None,
         }
     }
@@ -72,7 +119,6 @@ impl Dynamics2D {
     }
 
     #[run_as(action(BodyUpdate))]
-    #[allow(clippy::float_cmp)]
     fn update_pipeline(
         &mut self,
         transform: &mut Transform2D,
@@ -81,44 +127,54 @@ impl Dynamics2D {
     ) {
         let pipeline = pipeline.get_mut();
         if let Some(body) = self.handle.and_then(|handle| pipeline.body_mut(handle)) {
-            if transform.position != transform.old_position {
-                body.set_translation(vector![transform.position.x, transform.position.y], true);
-            }
-            if transform.rotation != transform.old_rotation {
-                body.set_rotation(Rotation::new(transform.rotation), true);
-            }
+            body.set_translation(vector![transform.position.x, transform.position.y], true);
+            body.set_rotation(Rotation::new(transform.rotation), true);
             body.set_linvel(vector![self.velocity.x, self.velocity.y], true);
             body.set_angvel(self.angular_velocity, true);
+            body.reset_forces(true);
+            body.add_force(vector![self.force.x, self.force.y], true);
+            body.reset_torques(true);
+            body.add_torque(self.torque, true);
+            let mass = MassProperties::new(point![0., 0.], self.mass, self.angular_inertia);
+            body.set_additional_mass_properties(mass, true);
+            body.set_linear_damping(self.damping);
+            body.set_angular_damping(self.angular_damping);
             body.user_data = entity.id() as u128;
         } else {
-            let builder = self.body_builder(entity.id(), transform);
-            self.handle = Some(pipeline.create_body(builder));
+            let body = self.create_body(entity.id(), transform);
+            self.handle = Some(pipeline.create_body(body));
         }
     }
 
     #[run_after(component(Pipeline2D))]
-    #[allow(clippy::float_cmp)]
-    fn update_from_pipeline(
-        &mut self,
-        transform: &mut Transform2D,
-        mut pipeline: SingleMut<'_, '_, Pipeline2D>,
-    ) {
+    fn update(&mut self, mut pipeline: SingleMut<'_, '_, Pipeline2D>) {
         let pipeline = pipeline.get_mut();
         if let Some(body) = self.handle.and_then(|handle| pipeline.body_mut(handle)) {
-            transform.position.x = body.translation().x;
-            transform.position.y = body.translation().y;
-            transform.rotation = body.rotation().angle();
+            self.velocity.x = body.linvel().x;
+            self.velocity.y = body.linvel().y;
+            self.angular_velocity = body.angvel();
+            self.force.x = body.user_force().x;
+            self.force.y = body.user_force().y;
+            self.torque = body.user_torque();
         }
     }
 
-    fn body_builder(&self, entity_id: usize, transform: &mut Transform2D) -> RigidBodyBuilder {
-        RigidBodyBuilder::new(RigidBodyType::Dynamic)
+    fn create_body(&self, entity_id: usize, transform: &mut Transform2D) -> RigidBody {
+        let mass = MassProperties::new(point![0., 0.], self.mass, self.angular_inertia);
+        let mut body = RigidBodyBuilder::new(RigidBodyType::Dynamic)
             .can_sleep(false)
             .translation(vector![transform.position.x, transform.position.y])
             .rotation(transform.rotation)
             .linvel(vector![self.velocity.x, self.velocity.y])
             .angvel(self.angular_velocity)
+            .additional_mass_properties(mass)
+            .linear_damping(self.damping)
+            .angular_damping(self.angular_damping)
             .user_data(entity_id as u128)
+            .build();
+        body.add_force(vector![self.force.x, self.force.y], true);
+        body.add_torque(self.torque, true);
+        body
     }
 }
 
@@ -127,3 +183,6 @@ impl Default for Dynamics2D {
         Self::new()
     }
 }
+
+#[derive(Action)]
+pub(crate) struct BodyUpdate(UnsynchronizedHandleDeletion);

crates/modor_physics/src/components/pipeline.rs

@@ -1,13 +1,13 @@
-use crate::components::collider::ColliderUserData;
-use crate::components::collision_groups::CollisionGroupRegistry;
+use crate::components::collider::{ColliderUpdate, ColliderUserData};
+use crate::components::dynamics::BodyUpdate;
 use crate::components::physics_hook::PhysicsHook;
-use crate::{Collider2D, Collision2D, CollisionGroup, DeltaTime, Dynamics2D};
-use modor::{ComponentSystems, Query, SingleRef};
+use crate::{Collider2D, Collision2D, DeltaTime, Dynamics2D};
+use modor::{Query, SingleRef};
 use rapier2d::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
-    RigidBodyBuilder, RigidBodyHandle, RigidBodySet,
+    RigidBodyHandle, RigidBodySet,
 };
-use rapier2d::geometry::{BroadPhase, ColliderBuilder, ColliderHandle, ColliderSet, NarrowPhase};
+use rapier2d::geometry::{BroadPhase, ColliderHandle, ColliderSet, NarrowPhase};
 use rapier2d::na::Vector2;
 use rapier2d::pipeline::PhysicsPipeline;
 use rapier2d::prelude::{Collider, RigidBody};
@@ -105,8 +105,8 @@ impl Pipeline2D {
         self.bodies.get_mut(handle)
     }
 
-    pub(crate) fn create_body(&mut self, builder: RigidBodyBuilder) -> RigidBodyHandle {
-        self.bodies.insert(builder)
+    pub(crate) fn create_body(&mut self, body: RigidBody) -> RigidBodyHandle {
+        self.bodies.insert(body)
     }
 
     pub(crate) fn collider_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
@@ -115,14 +115,14 @@ impl Pipeline2D {
 
     pub(crate) fn create_collider(
         &mut self,
-        builder: ColliderBuilder,
+        collider: Collider,
         body_handle: Option<RigidBodyHandle>,
     ) -> ColliderHandle {
         if let Some(body_handle) = body_handle {
             self.colliders
-                .insert_with_parent(builder, body_handle, &mut self.bodies)
+                .insert_with_parent(collider, body_handle, &mut self.bodies)
         } else {
-            self.colliders.insert(builder)
+            self.colliders.insert(collider)
         }
     }
 
@@ -195,14 +195,3 @@ pub(crate) struct ColliderHandleRemoval(BodyHandleReset);
 
 #[derive(Action)]
 pub(crate) struct UnsynchronizedHandleDeletion(BodyHandleReset, ColliderHandleRemoval);
-
-#[derive(Action)]
-pub(crate) struct BodyUpdate(UnsynchronizedHandleDeletion);
-
-#[derive(Action)]
-pub(crate) struct ColliderUpdate(
-    UnsynchronizedHandleDeletion,
-    BodyUpdate,
-    <CollisionGroup as ComponentSystems>::Action,
-    <CollisionGroupRegistry as ComponentSystems>::Action,
-);

crates/modor_physics/src/components/transform.rs

@@ -1,13 +1,14 @@
-use crate::{Collider2D, Dynamics2D};
-use modor::{Filter, Or, With};
+use crate::components::pipeline::Pipeline2D;
+use crate::Dynamics2D;
+use modor::SingleMut;
 use modor_math::Vec2;
 
 /// The positioning of a 2D entity.
 ///
 /// # Related components
 ///
 /// - [`Dynamics2D`]
-/// - [`Collider2D`]
+/// - [`Collider2D`](crate::Collider2D)
 ///
 /// # Example
 ///
@@ -35,9 +36,6 @@ pub struct Transform2D {
     pub size: Vec2,
     /// Rotation of the entity in radians.
     pub rotation: f32,
-    pub(crate) old_position: Vec2,
-    pub(crate) old_size: Vec2,
-    pub(crate) old_rotation: f32,
 }
 
 #[systems]
@@ -49,17 +47,17 @@ impl Transform2D {
             position: Vec2::ZERO,
             size: Vec2::ONE,
             rotation: 0.,
-            old_position: Vec2::ZERO,
-            old_size: Vec2::ONE,
-            old_rotation: 0.,
         }
     }
 
-    #[run_after(component(Dynamics2D), component(Collider2D))]
-    fn update(&mut self, _filter: Filter<Or<(With<Dynamics2D>, With<Collider2D>)>>) {
-        self.old_position = self.position;
-        self.old_size = self.size;
-        self.old_rotation = self.rotation;
+    #[run_after(component(Dynamics2D), component(Pipeline2D))]
+    fn update(&mut self, dynamics: &Dynamics2D, mut pipeline: SingleMut<'_, '_, Pipeline2D>) {
+        let pipeline = pipeline.get_mut();
+        if let Some(body) = dynamics.handle.and_then(|handle| pipeline.body_mut(handle)) {
+            self.position.x = body.translation().x;
+            self.position.y = body.translation().y;
+            self.rotation = body.rotation().angle();
+        }
     }
 }
 
@@ -75,9 +73,6 @@ impl Clone for Transform2D {
             position: self.position,
             size: self.size,
             rotation: self.rotation,
-            old_position: self.position,
-            old_size: self.size,
-            old_rotation: self.rotation,
         }
     }
 }