feat!: Kinematic bodies (jcornaz#34)

CHANGELOG.md

@@ -33,6 +33,12 @@ In the future it will be extended to define more physics properties, like the fr
 
 Since the restitution is now defined in `PhysicMaterial`, the `Restitution` component has been removed.
 
+### ⚠ Kinematic bodies
+
+There is a new variant to `BodyType`: `Kinematic`. That makes possible to create "kinematic" bodies.
+A kinematic body is controlled programmatically (usually by updating the transform) and affect the other bodies normally, 
+bot is not affected by them.
+
 
 ### ⚠ Dependency requirements updated (breaking)
 

README.md

@@ -30,7 +30,7 @@ fn spawn(commands: &mut Commands) {
         .with(Body::Sphere { radius: 10.0 })
 
         // Optionally define a type (if absent, the body will be *dynamic*)
-        .with(BodyType::Static)
+        .with(BodyType::Kinematic)
 
         // Optionally define the velocity (works only with dynamic and kinematic bodies)
         .with(Velocity::from(Vec2::unit_x() * 2.0));

core/src/lib.rs

@@ -151,20 +151,35 @@ impl Default for Body {
 /// ```
 #[derive(Debug, Copy, Clone, Eq, PartialEq, Reflect)]
 pub enum BodyType {
-    /// A dynamic body is normally affected by physic forces and affect the other bodies too.
+    /// A dynamic body is normally affected by physic forces and affect the other bodies normally too.
     ///
     /// This is the most "natural" type in the sense that, in the real life, everything is dynamic.
+    ///
+    /// It is the default type.
     Dynamic,
 
     /// A static body is not affected by physic forces and doesn't move. But it does affect the other bodies.
     ///
     /// This effectively behaves like a dynamic body with infinite mass and zero velocity.
     ///
-    /// It is especially useful to model terrain and static obstacles.
+    /// It is well suited for terrain and static obstacles.
     Static,
 
+    /// A kinematic body is not moved by the physics engine. But it can have user-defined velocity.
+    ///
+    /// It affects the other bodies normally, but is not affected by them.
+    ///
+    /// If the transform is updated, then a velocity will be automatically calculated, producing
+    /// realistic interaction with other bodies.
+    ///
+    /// It can also have a velocity be applied.
+    ///
+    /// It is well suited fo moving-platforms.
+    Kinematic,
+
     /// A sensor is not affected by physics forces and doesn't affect other bodies either.
-    /// Other bodies will be able to penetrate the sensor.
+    ///
+    /// Other bodies will be able to penetrate the sensor. But it still participate in collision events.
     ///
     /// A sensor is useful when we are only interested in collision events.
     /// One may for example add a sensor to detect when the player reach a certain area.
@@ -177,6 +192,17 @@ impl Default for BodyType {
     }
 }
 
+impl BodyType {
+    /// Returns true if this body type can be moved by [`Velocity`]
+    #[must_use]
+    pub fn can_have_velocity(self) -> bool {
+        match self {
+            BodyType::Dynamic | BodyType::Kinematic => true,
+            BodyType::Static | BodyType::Sensor => false,
+        }
+    }
+}
+
 /// An event fired when the collision state between two entities changed
 ///
 /// # Example

core/src/velocity.rs

@@ -2,6 +2,7 @@ use bevy::math::prelude::*;
 use bevy::reflect::prelude::*;
 
 use crate::utils::NearZero;
+use std::ops::{Mul, MulAssign};
 
 /// Component that defines the linear and angular velocity.
 ///
@@ -133,6 +134,30 @@ impl NearZero for Velocity {
     }
 }
 
+impl MulAssign<f32> for AxisAngle {
+    fn mul_assign(&mut self, rhs: f32) {
+        self.0 = self.0 * rhs;
+    }
+}
+
+impl Mul<f32> for AxisAngle {
+    type Output = Self;
+
+    fn mul(mut self, rhs: f32) -> Self::Output {
+        self *= rhs;
+        self
+    }
+}
+
+impl Mul<AxisAngle> for f32 {
+    type Output = AxisAngle;
+
+    fn mul(self, mut rhs: AxisAngle) -> Self::Output {
+        rhs *= self;
+        rhs
+    }
+}
+
 impl AxisAngle {
     /// Create a new axis-angle
     #[inline]

examples/demo.rs

@@ -55,7 +55,8 @@ fn spawn(commands: &mut Commands, mut materials: ResMut<Assets<ColorMaterial>>)
         })
         // Add an initial velocity. (it is also possible to read/mutate this component later)
         .with(
-            Velocity::from(Vec2::unit_x() * 300.0).with_angular(AxisAngle::new(Vec3::unit_z(), PI)),
+            Velocity::from(Vec2::unit_x() * 300.0)
+                .with_angular(AxisAngle::new(Vec3::unit_z(), -PI)),
         )
         // Define restitution (so that it bounces)
         .with(PhysicMaterial {

rapier/src/body.rs

@@ -134,10 +134,12 @@ pub(crate) fn update_rapier_position(
 ) {
     for (transform, handle) in query.iter() {
         if let Some(body) = bodies.get_mut(handle.rigid_body) {
-            body.set_position(
-                (transform.translation, transform.rotation).into_rapier(),
-                true,
-            );
+            let isometry = (transform.translation, transform.rotation).into_rapier();
+            if body.is_kinematic() {
+                body.set_next_kinematic_position(isometry);
+            } else {
+                body.set_position(isometry, true);
+            }
         }
     }
 }
@@ -155,7 +157,7 @@ pub(crate) fn update_bevy_transform(
     >,
 ) {
     for (mut local, mut global, handle, body_type) in query.iter_mut() {
-        if !matches!(body_type.cloned().unwrap_or_default(), BodyType::Dynamic) {
+        if !body_type.cloned().unwrap_or_default().can_have_velocity() {
             continue;
         }
 
@@ -245,6 +247,7 @@ fn body_status(body_type: BodyType) -> BodyStatus {
     match body_type {
         BodyType::Dynamic => BodyStatus::Dynamic,
         BodyType::Static | BodyType::Sensor => BodyStatus::Static,
+        BodyType::Kinematic => BodyStatus::Kinematic,
     }
 }
 

rapier/src/lib.rs

@@ -116,7 +116,7 @@ impl Plugin for RapierPlugin {
         .stage(heron_core::stage::ROOT, |schedule: &mut Schedule| {
             schedule
                 .add_stage(
-                    "heron-step",
+                    "heron-pre-step",
                     SystemStage::serial()
                         .with_system(
                             bevy::transform::transform_propagate_system::transform_propagate_system
@@ -125,9 +125,14 @@ impl Plugin for RapierPlugin {
                         .with_system(body::remove.system())
                         .with_system(body::recreate_collider.system())
                         .with_system(body::update_rapier_position.system())
-                        .with_system(body::update_rapier_status.system())
                         .with_system(velocity::update_rapier_velocity.system())
-                        .with_system(body::create.system())
+                        .with_system(body::update_rapier_status.system())
+                        .with_system(body::create.system()),
+                )
+                .add_stage(
+                    "heron-step",
+                    SystemStage::serial()
+                        .with_system(velocity::apply_velocity_to_kinematic_bodies.system())
                         .with_system(pipeline::step.system()),
                 )
                 .add_stage(

rapier/src/velocity.rs

@@ -1,25 +1,50 @@
 use bevy::ecs::prelude::*;
+use bevy::math::prelude::*;
 
 use heron_core::utils::NearZero;
-use heron_core::Velocity;
+use heron_core::{BodyType, Velocity};
 
 use crate::convert::{IntoBevy, IntoRapier};
-use crate::rapier::dynamics::RigidBodySet;
+use crate::rapier::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::BodyHandle;
 
 pub(crate) fn update_rapier_velocity(
     mut bodies: ResMut<'_, RigidBodySet>,
-    velocities: Query<'_, (&BodyHandle, &Velocity), Changed<Velocity>>,
+    query: Query<'_, (&BodyHandle, Option<&BodyType>, &Velocity), Changed<Velocity>>,
 ) {
-    for (handle, velocity) in velocities.iter() {
+    let dynamic_bodies = query.iter().filter(|(_, body_type, _)| {
+        matches!(body_type.cloned().unwrap_or_default(), BodyType::Dynamic)
+    });
+
+    for (handle, _, velocity) in dynamic_bodies {
         if let Some(body) = bodies.get_mut(handle.rigid_body) {
-            let wake_up = velocity.is_near_zero();
+            let wake_up = !velocity.is_near_zero();
             body.set_linvel(velocity.linear.into_rapier(), wake_up);
             body.set_angvel(velocity.angular.into_rapier(), wake_up);
         }
     }
 }
 
+pub(crate) fn apply_velocity_to_kinematic_bodies(
+    mut bodies: ResMut<'_, RigidBodySet>,
+    integration_parameters: Res<'_, IntegrationParameters>,
+    query: Query<'_, (&BodyHandle, &BodyType, &Velocity)>,
+) {
+    let delta_time = integration_parameters.dt;
+    let kinematic_bodies = query
+        .iter()
+        .filter(|(_, body_type, _)| matches!(body_type, BodyType::Kinematic));
+
+    for (handle, _, velocity) in kinematic_bodies {
+        if let Some(body) = bodies.get_mut(handle.rigid_body) {
+            let (mut translation, mut rotation) = body.position().into_bevy();
+            translation += velocity.linear * delta_time;
+            rotation *= Quat::from(velocity.angular * delta_time);
+            body.set_next_kinematic_position((translation, rotation).into_rapier())
+        }
+    }
+}
+
 pub(crate) fn update_velocity_component(
     bodies: Res<'_, RigidBodySet>,
     mut velocities: Query<'_, (&BodyHandle, &mut Velocity)>,

rapier/tests/bodies.rs

@@ -10,12 +10,14 @@ use bevy::core::CorePlugin;
 use bevy::prelude::*;
 use bevy::reflect::TypeRegistryArc;
 
-use heron_core::Body;
+use heron_core::{Body, BodyType};
 use heron_rapier::convert::{IntoBevy, IntoRapier};
 use heron_rapier::rapier::dynamics::{IntegrationParameters, RigidBodySet};
 use heron_rapier::rapier::geometry::ColliderSet;
 use heron_rapier::{BodyHandle, RapierPlugin};
 
+use rstest::rstest;
+
 fn test_app() -> App {
     let mut builder = App::build();
     builder
@@ -200,8 +202,13 @@ fn despawn_body_entity() {
     assert_eq!(colliders.len(), 0);
 }
 
-#[test]
-fn update_bevy_transform() {
+#[rstest(
+    body_type,
+    case(Some(BodyType::Dynamic)),
+    case(Some(BodyType::Kinematic)),
+    case(None)
+)]
+fn update_bevy_transform(body_type: Option<BodyType>) {
     let mut app = test_app();
 
     let entity = app.world.spawn((
@@ -210,6 +217,10 @@ fn update_bevy_transform() {
         GlobalTransform::default(),
     ));
 
+    if let Some(body_type) = body_type {
+        app.world.insert_one(entity, body_type).unwrap();
+    }
+
     let translation = Vec3::new(1.0, 2.0, 3.0);
     let rotation = Quat::from_axis_angle(Vec3::unit_z(), PI / 2.0);
 

rapier/tests/body_types.rs

@@ -64,6 +64,26 @@ fn create_static_body() {
     assert!(body.is_static())
 }
 
+#[test]
+fn create_kinematic_body() {
+    let mut app = test_app();
+
+    let entity = app.world.spawn((
+        GlobalTransform::default(),
+        Body::Sphere { radius: 10.0 },
+        BodyType::Kinematic,
+    ));
+
+    app.update();
+
+    let bodies = app.resources.get::<RigidBodySet>().unwrap();
+    let body = bodies
+        .get(app.world.get::<BodyHandle>(entity).unwrap().rigid_body())
+        .unwrap();
+
+    assert!(body.is_kinematic())
+}
+
 #[test]
 fn create_sensor_body() {
     let mut app = test_app();

rapier/tests/velocity.rs

@@ -145,3 +145,40 @@ fn velocity_is_updated_to_reflect_rapier_world() {
 
     assert_eq!(angular, velocity.angular.into());
 }
+
+#[test]
+fn velocity_can_move_kinematic_bodies() {
+    let mut app = test_app();
+
+    let linear = Vec3::new(1.0, 2.0, 3.0);
+    let angular = AxisAngle::new(Vec3::unit_z(), PI * 0.5);
+
+    let entity = app.world.spawn((
+        GlobalTransform::from_rotation(Quat::from_axis_angle(Vec3::unit_z(), 0.0)),
+        Body::Sphere { radius: 1.0 },
+        BodyType::Kinematic,
+        Velocity::from_linear(linear).with_angular(angular),
+    ));
+
+    app.update();
+
+    let bodies = app.resources.get::<RigidBodySet>().unwrap();
+    let body = bodies
+        .get(app.world.get::<BodyHandle>(entity).unwrap().rigid_body())
+        .unwrap();
+
+    let position = body.position().translation.into_bevy();
+    let rotation = body.position().rotation.into_bevy();
+
+    assert_eq!(position.x, linear.x);
+    assert_eq!(position.y, linear.y);
+
+    #[cfg(feature = "3d")]
+    assert_eq!(position.z, linear.z);
+
+    let angular: Quat = angular.into();
+    assert!((rotation.x - angular.x).abs() < 0.00001);
+    assert!((rotation.y - angular.y).abs() < 0.00001);
+    assert!((rotation.z - angular.z).abs() < 0.00001);
+    assert!((rotation.w - angular.w).abs() < 0.00001);
+}