fix: KinematicVelocityBased bodies not updated by Velocity. (jcornaz#148

) Co-authored-by: Jonathan Cornaz <jonathan.cornaz@gmail.com>
ManevilleF · Oct 24, 2021 · 3f416cd · 3f416cd
1 parent 5ef4b16
commit 3f416cd
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 51 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -10,6 +10,9 @@ The format is inspired from [Keep a Changelog], and this project adheres to [Sem
 
 ## [Unreleased]
 
+## Fixed
+
+* `KinematicVelocityBased` bodies not being moved by velocity (#148).
 
 
 ## [0.12.0] - 2021-10-24

diff --git a/rapier/src/lib.rs b/rapier/src/lib.rs
@@ -17,6 +17,7 @@ pub(crate) use rapier2d as rapier;
 pub(crate) use rapier3d as rapier;
 
 use heron_core::{CollisionEvent, PhysicsSystem};
+pub use pipeline::{PhysicsWorld, RayCastInfo, ShapeCastCollisionInfo, ShapeCastCollisionType};
 
 use crate::rapier::dynamics::{
     CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet,
@@ -29,7 +30,6 @@ mod acceleration;
 mod body;
 pub mod convert;
 mod pipeline;
-pub use pipeline::{PhysicsWorld, RayCastInfo, ShapeCastCollisionInfo, ShapeCastCollisionType};
 mod shape;
 mod velocity;
 
@@ -110,11 +110,6 @@ fn create_collider_stage() -> SystemStage {
 fn step_systems() -> SystemSet {
     SystemSet::new()
         .with_run_criteria(heron_core::should_run.system())
-        .with_system(
-            velocity::apply_velocity_to_kinematic_bodies
-                .system()
-                .before(PhysicsSystem::Events),
-        )
         .with_system(
             pipeline::update_integration_parameters
                 .system()
@@ -133,4 +128,10 @@ fn step_systems() -> SystemSet {
                 .label(PhysicsSystem::VelocityUpdate)
                 .after(PhysicsSystem::Events),
         )
+        .with_system(
+            velocity::update_rapier_velocity
+                .system()
+                .after(PhysicsSystem::Events)
+                .after(PhysicsSystem::VelocityUpdate),
+        )
 }
diff --git a/rapier/src/velocity.rs b/rapier/src/velocity.rs
@@ -1,19 +1,18 @@
 use bevy::ecs::prelude::*;
-use bevy::math::prelude::*;
 
 use heron_core::utils::NearZero;
-use heron_core::{PhysicsSteps, RigidBody, Velocity};
+use heron_core::{RigidBody, Velocity};
 
 use crate::convert::{IntoBevy, IntoRapier};
 use crate::rapier::dynamics::{RigidBodyHandle, RigidBodySet};
 
 pub(crate) fn update_rapier_velocity(
     mut bodies: ResMut<'_, RigidBodySet>,
-    query: Query<'_, (&RigidBodyHandle, Option<&RigidBody>, &Velocity), Changed<Velocity>>,
+    query: Query<'_, (&RigidBodyHandle, Option<&RigidBody>, &Velocity)>,
 ) {
-    let dynamic_bodies = query.iter().filter(|(_, body_type, _)| {
-        matches!(body_type.copied().unwrap_or_default(), RigidBody::Dynamic)
-    });
+    let dynamic_bodies = query
+        .iter()
+        .filter(|(_, body_type, _)| body_type.copied().unwrap_or_default().can_have_velocity());
 
     for (handle, _, velocity) in dynamic_bodies {
         if let Some(body) = bodies.get_mut(*handle) {
@@ -24,30 +23,6 @@ pub(crate) fn update_rapier_velocity(
     }
 }
 
-pub(crate) fn apply_velocity_to_kinematic_bodies(
-    mut bodies: ResMut<'_, RigidBodySet>,
-    physics_step: Res<'_, PhysicsSteps>,
-    bevy_time: Res<'_, bevy::core::Time>,
-    query: Query<'_, (&RigidBodyHandle, &RigidBody, &Velocity)>,
-) {
-    let delta_time = physics_step
-        .duration()
-        .exact(bevy_time.delta())
-        .as_secs_f32();
-    let kinematic_bodies = query
-        .iter()
-        .filter(|(_, body_type, _)| matches!(body_type, RigidBody::KinematicVelocityBased));
-
-    for (handle, _, velocity) in kinematic_bodies {
-        if let Some(body) = bodies.get_mut(*handle) {
-            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<'_, (&RigidBodyHandle, &mut Velocity)>,

diff --git a/rapier/tests/velocity.rs b/rapier/tests/velocity.rs
@@ -25,8 +25,10 @@ fn test_app() -> App {
     builder.app
 }
 
-#[test]
-fn body_is_created_with_velocity() {
+#[rstest]
+#[case(RigidBody::Dynamic)]
+#[case(RigidBody::KinematicVelocityBased)]
+fn body_is_created_with_velocity(#[case] body_type: RigidBody) {
     let mut app = test_app();
 
     let linear = Vec3::new(1.0, 2.0, 3.0);
@@ -38,7 +40,7 @@ fn body_is_created_with_velocity() {
         .insert_bundle((
             Transform::default(),
             GlobalTransform::default(),
-            RigidBody::Dynamic,
+            body_type,
             CollisionShape::Sphere { radius: 1.0 },
             Velocity { linear, angular },
         ))
@@ -65,8 +67,10 @@ fn body_is_created_with_velocity() {
     assert_eq!(angular.angle(), body.angvel());
 }
 
-#[test]
-fn velocity_may_be_added_after_creating_the_body() {
+#[rstest]
+#[case(RigidBody::Dynamic)]
+#[case(RigidBody::KinematicVelocityBased)]
+fn velocity_may_be_added_after_creating_the_body(#[case] body_type: RigidBody) {
     let mut app = test_app();
 
     let entity = app
@@ -75,7 +79,7 @@ fn velocity_may_be_added_after_creating_the_body() {
         .insert_bundle((
             Transform::default(),
             GlobalTransform::default(),
-            RigidBody::Dynamic,
+            body_type,
             CollisionShape::Sphere { radius: 1.0 },
         ))
         .id();
@@ -148,10 +152,9 @@ fn velocity_is_updated_to_reflect_rapier_world() {
 }
 
 #[rstest]
-#[case(Some(RigidBody::Dynamic))]
-#[case(Some(RigidBody::KinematicVelocityBased))]
-#[case(None)]
-fn velocity_can_move_kinematic_bodies(#[case] body_type: Option<RigidBody>) {
+#[case(RigidBody::Dynamic)]
+#[case(RigidBody::KinematicVelocityBased)]
+fn velocity_can_move_kinematic_bodies(#[case] body_type: RigidBody) {
     let mut app = test_app();
     let translation = Vec3::new(1.0, 2.0, 3.0);
     let rotation = Quat::from_axis_angle(Vec3::Z, PI / 2.0);
@@ -160,18 +163,14 @@ fn velocity_can_move_kinematic_bodies(#[case] body_type: Option<RigidBody>) {
         .world
         .spawn()
         .insert_bundle((
-            RigidBody::Dynamic,
+            body_type,
             CollisionShape::Sphere { radius: 2.0 },
             Transform::default(),
             GlobalTransform::default(),
             Velocity::from(translation).with_angular(rotation.into()),
         ))
         .id();
 
-    if let Some(body_type) = body_type {
-        app.world.entity_mut(entity).insert(body_type);
-    }
-
     app.update();
 
     let Transform {