feat: Acceleration component (#68)

Co-authored-by: Jonathan Cornaz <jonathan.cornaz@gmail.com>

CHANGELOG.md

@@ -17,6 +17,13 @@ smallest possible convex shape that includes all the given points.
 
 Thanks @faassen
 
+### Acceleration component
+
+A new `Acceleration` component make possible to apply and linear and angular accelerations.
+It is by extension also possible to apply a force if you know the mass: `acceleration.linear = force / mass`.
+
+Thanks @ryo33
+
 ### RotationConstraints component
 
 A new `RotationConstraints` component make possible to prevent rotation around the given axes.

core/Cargo.toml

@@ -9,3 +9,4 @@ repository = "https://github.com/jcornaz/heron/"
 
 [dependencies]
 bevy = { version = "^0.4.0", default-features = false }
+duplicate = "^0.2.9"

core/src/lib.rs

@@ -10,7 +10,7 @@ use bevy::prelude::*;
 pub use constraints::RotationConstraints;
 pub use ext::*;
 pub use gravity::Gravity;
-pub use velocity::{AxisAngle, Velocity};
+pub use velocity::{Acceleration, AxisAngle, Velocity};
 
 mod constraints;
 pub mod ext;

core/src/velocity.rs

@@ -1,5 +1,6 @@
 use bevy::math::prelude::*;
 use bevy::reflect::prelude::*;
+use duplicate::duplicate;
 
 use crate::utils::NearZero;
 use std::ops::{Mul, MulAssign};
@@ -34,6 +35,36 @@ pub struct Velocity {
     pub angular: AxisAngle,
 }
 
+/// Component that defines the linear and angular acceleration.
+///
+/// The linear part is in "unit" per second squared on each axis, represented as a `Vec3`. (The unit, being your game unit, be it pixel or anything else)
+/// The angular part is in radians per second squared around an axis, represented as an [`AxisAngle`]
+///
+/// # Example
+///
+/// ```
+/// # use bevy::prelude::*;
+/// # use heron_core::*;
+/// # use std::f32::consts::PI;
+///
+/// fn spawn(commands: &mut Commands) {
+///     commands.spawn(todo!("Spawn your sprite/mesh, incl. at least a GlobalTransform"))
+///         .with(Body::Sphere { radius: 1.0 })
+///         .with(
+///             Acceleration::from_linear(Vec3::unit_x() * 1.0)
+///                 .with_angular(AxisAngle::new(Vec3::unit_z(), 0.05 * PI))
+///         );
+/// }
+/// ```
+#[derive(Debug, Copy, Clone, PartialEq, Default)]
+pub struct Acceleration {
+    /// Linear acceleration in units-per-second-squared on each axis
+    pub linear: Vec3,
+
+    /// Angular acceleration in radians-per-second-squared around an axis
+    pub angular: AxisAngle,
+}
+
 /// An [axis-angle] representation
 ///
 /// [axis-angle]: https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
@@ -74,42 +105,111 @@ impl Velocity {
     }
 }
 
+impl Acceleration {
+    /// Returns a linear acceleration from a vector
+    #[must_use]
+    pub fn from_linear(linear: Vec3) -> Self {
+        Self {
+            linear,
+            angular: AxisAngle::default(),
+        }
+    }
+
+    /// Returns an angular acceleration from a vector
+    #[must_use]
+    pub fn from_angular(angular: AxisAngle) -> Self {
+        Self {
+            angular,
+            linear: Vec3::zero(),
+        }
+    }
+
+    /// Returns a new version with the given linear acceleration
+    #[must_use]
+    pub fn with_linear(mut self, linear: Vec3) -> Self {
+        self.linear = linear;
+        self
+    }
+
+    /// Returns a new version with the given angular acceleration
+    #[must_use]
+    pub fn with_angular(mut self, angular: AxisAngle) -> Self {
+        self.angular = angular;
+        self
+    }
+}
+
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<Vec2> for Velocity {
     fn from(v: Vec2) -> Self {
         Self::from_linear(v.extend(0.0))
     }
 }
 
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<Vec3> for Velocity {
     fn from(linear: Vec3) -> Self {
         Self::from_linear(linear)
     }
 }
 
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<Velocity> for Vec3 {
     fn from(Velocity { linear, .. }: Velocity) -> Self {
         linear
     }
 }
 
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<AxisAngle> for Velocity {
     fn from(angular: AxisAngle) -> Self {
         Self::from_angular(angular)
     }
 }
 
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<Quat> for Velocity {
     fn from(quat: Quat) -> Self {
         Self::from_angular(quat.into())
     }
 }
 
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<Velocity> for AxisAngle {
     fn from(Velocity { angular, .. }: Velocity) -> Self {
         angular
     }
 }
 
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl From<Velocity> for Quat {
     fn from(Velocity { angular, .. }: Velocity) -> Self {
         angular.into()
@@ -128,6 +228,17 @@ impl From<AxisAngle> for Vec3 {
     }
 }
 
+impl From<AxisAngle> for f32 {
+    fn from(AxisAngle(v): AxisAngle) -> Self {
+        v.length()
+    }
+}
+
+#[duplicate(
+  Velocity;
+  [ Velocity ];
+  [ Acceleration ];
+)]
 impl NearZero for Velocity {
     fn is_near_zero(self) -> bool {
         self.linear.is_near_zero() && self.angular.is_near_zero()

rapier/src/acceleration.rs

@@ -0,0 +1,46 @@
+use bevy::prelude::*;
+use rapier::{
+    dynamics::RigidBody,
+    math::{Real, Vector},
+};
+
+use heron_core::{utils::NearZero, Acceleration};
+
+use crate::convert::IntoRapier;
+use crate::rapier::dynamics::RigidBodySet;
+use crate::rapier::math::AngVector;
+use crate::BodyHandle;
+
+pub(crate) fn update_rapier_force_and_torque(
+    mut bodies: ResMut<'_, RigidBodySet>,
+    accelerations: Query<'_, (&BodyHandle, &Acceleration)>,
+) {
+    for (handle, acceleration) in accelerations.iter() {
+        if let Some(body) = bodies.get_mut(handle.rigid_body) {
+            update_acceleration(body, acceleration)
+        }
+    }
+}
+
+fn update_acceleration(body: &mut RigidBody, acceleration: &Acceleration) {
+    let wake_up = !acceleration.is_near_zero();
+    let linear_acceleration: Vector<Real> = acceleration.linear.into_rapier();
+    let angular_acceleration: AngVector<f32> = acceleration.angular.into_rapier();
+    let inertia = {
+        #[cfg(feature = "3d")]
+        {
+            body.mass_properties().reconstruct_inertia_matrix()
+        }
+        #[cfg(feature = "2d")]
+        {
+            let val = body.mass_properties().inv_principal_inertia_sqrt;
+            if val == 0.0 {
+                0.0
+            } else {
+                (1.0 / val).powi(2)
+            }
+        }
+    };
+    body.apply_force(linear_acceleration * body.mass(), wake_up);
+    body.apply_torque(inertia * angular_acceleration, wake_up)
+}

rapier/src/lib.rs

@@ -24,6 +24,7 @@ use crate::rapier::geometry::{BroadPhase, ColliderHandle, ColliderSet, NarrowPha
 pub use crate::rapier::na as nalgebra;
 use crate::rapier::pipeline::PhysicsPipeline;
 
+mod acceleration;
 mod body;
 pub mod convert;
 mod pipeline;
@@ -130,6 +131,7 @@ impl Plugin for RapierPlugin {
                         .with_system(body::update_rapier_position.system())
                         .with_system(velocity::update_rapier_velocity.system())
                         .with_system(body::update_rapier_status.system())
+                        .with_system(acceleration::update_rapier_force_and_torque.system())
                         .with_system(body::create.system()),
                 )
                 .add_stage(

rapier/tests/acceleration.rs

@@ -0,0 +1,129 @@
+#![cfg(all(
+    any(feature = "2d", feature = "3d"),
+    not(all(feature = "2d", feature = "3d")),
+))]
+
+use bevy::core::CorePlugin;
+use bevy::prelude::*;
+use bevy::prelude::{GlobalTransform, Transform};
+use bevy::reflect::TypeRegistryArc;
+use heron_core::{Acceleration, AxisAngle, Body};
+use heron_rapier::convert::IntoBevy;
+
+#[cfg(feature = "3d")]
+use heron_rapier::rapier::math::Vector;
+use heron_rapier::{
+    rapier::dynamics::{IntegrationParameters, RigidBodySet},
+    BodyHandle, RapierPlugin,
+};
+
+fn test_app() -> App {
+    let mut builder = App::build();
+    builder
+        .init_resource::<TypeRegistryArc>()
+        .add_plugin(CorePlugin)
+        .add_plugin(RapierPlugin {
+            step_per_second: None,
+            parameters: {
+                let mut params = IntegrationParameters::default();
+                params.dt = 1.0;
+                params
+            },
+        });
+    builder.app
+}
+
+#[test]
+fn body_is_created_with_acceleration() {
+    let mut app = test_app();
+
+    #[cfg(feature = "3d")]
+    let linear = Vec3::new(1.0, 2.0, 3.0);
+    #[cfg(feature = "2d")]
+    let linear = Vec3::new(1.0, 2.0, 0.0);
+
+    let angular = AxisAngle::new(Vec3::unit_z(), 1.0);
+
+    let entity = app.world.spawn((
+        Transform::default(),
+        GlobalTransform::default(),
+        Body::Sphere { radius: 1.0 },
+        Acceleration { linear, angular },
+    ));
+
+    app.update();
+
+    {
+        let bodies = app.resources.get::<RigidBodySet>().unwrap();
+
+        let body = bodies
+            .get(app.world.get::<BodyHandle>(entity).unwrap().rigid_body())
+            .unwrap();
+
+        println!("{:?}", body);
+        assert_eq!(body.linvel().into_bevy(), Vec3::zero());
+        assert_eq_angular(body.angvel(), AxisAngle::from(Vec3::zero()));
+    }
+
+    app.update();
+
+    let bodies = app.resources.get::<RigidBodySet>().unwrap();
+
+    let body = bodies
+        .get(app.world.get::<BodyHandle>(entity).unwrap().rigid_body())
+        .unwrap();
+
+    println!("{:?}", body);
+    assert_eq!(body.linvel().into_bevy(), linear);
+    assert_eq_angular(body.angvel(), angular);
+}
+
+#[test]
+fn acceleration_may_be_added_after_creating_the_body() {
+    let mut app = test_app();
+
+    let entity = app.world.spawn((
+        Transform::default(),
+        GlobalTransform::default(),
+        Body::Sphere { radius: 1.0 },
+    ));
+
+    app.update();
+
+    #[cfg(feature = "3d")]
+    let linear = Vec3::new(1.0, 2.0, 3.0);
+    #[cfg(feature = "2d")]
+    let linear = Vec3::new(1.0, 2.0, 0.0);
+
+    let angular = AxisAngle::new(Vec3::unit_z(), 2.0);
+
+    app.world
+        .insert_one(entity, Acceleration { linear, angular })
+        .unwrap();
+
+    app.update();
+
+    let bodies = app.resources.get::<RigidBodySet>().unwrap();
+
+    let body = bodies
+        .get(app.world.get::<BodyHandle>(entity).unwrap().rigid_body())
+        .unwrap();
+
+    assert_eq!(body.linvel().into_bevy(), linear);
+    assert_eq_angular(body.angvel(), angular);
+}
+
+#[cfg(feature = "3d")]
+fn assert_eq_angular(actual: &Vector<f32>, expected: AxisAngle) {
+    assert_eq!(actual.into_bevy(), expected.into());
+}
+
+#[cfg(feature = "2d")]
+fn assert_eq_angular(expected: f32, actual: AxisAngle) {
+    assert!(
+        (expected - actual.angle()).abs() < 0.00001,
+        "actual rapier angle ({}) doesn't match expected axis-angle: {:?}",
+        expected,
+        actual
+    );
+}