From 612dff13ee55547e51623990c12cec60fca81824 Mon Sep 17 00:00:00 2001
From: Martin Indra <martin.indra@mgn.cz>
Date: Sat, 26 Nov 2022 15:12:24 +0100
Subject: [PATCH] Properly control object altitude

Fixes #262.
---
 assets/objects/attacker.obj.json  |  3 ++
 crates/movement/src/altitude.rs   | 86 +++++++++++++++++++++++++++++++
 crates/movement/src/kinematics.rs | 59 ++++++++++++++-------
 crates/movement/src/lib.rs        | 13 +++--
 crates/movement/src/movement.rs   | 11 ++--
 crates/movement/src/pathing.rs    |  6 +--
 crates/movement/src/repulsion.rs  |  4 +-
 crates/objects/src/cache.rs       | 25 +++++++--
 crates/objects/src/lib.rs         |  2 +
 crates/objects/src/loader.rs      | 16 ++++++
 crates/objects/src/movement.rs    | 21 ++++++++
 docs/content/objects/schema.json  | 11 ++++
 12 files changed, 223 insertions(+), 34 deletions(-)
 create mode 100644 crates/movement/src/altitude.rs
 create mode 100644 crates/objects/src/movement.rs

diff --git a/assets/objects/attacker.obj.json b/assets/objects/attacker.obj.json
index c5a81e704..89e4ba7bc 100644
--- a/assets/objects/attacker.obj.json
+++ b/assets/objects/attacker.obj.json
@@ -42,5 +42,8 @@
     "range": 50.0,
     "damage": 3.0,
     "recharge_interval": 2.5
+  },
+  "movement": {
+    "max_heigth": 5.0
   }
 }
diff --git a/crates/movement/src/altitude.rs b/crates/movement/src/altitude.rs
new file mode 100644
index 000000000..d857b802e
--- /dev/null
+++ b/crates/movement/src/altitude.rs
@@ -0,0 +1,86 @@
+use bevy::prelude::*;
+use de_core::{
+    objects::MovableSolid,
+    stages::GameStage,
+    state::{AppState, GameState},
+};
+use iyes_loopless::prelude::*;
+
+use crate::{
+    movement::DesiredVelocity,
+    repulsion::{RepulsionLables, RepulsionVelocity},
+    MAX_ACCELERATION, MAX_VERTICAL_SPEED,
+};
+
+pub(crate) struct AltitudePlugin;
+
+impl Plugin for AltitudePlugin {
+    fn build(&self, app: &mut App) {
+        app.add_system_to_stage(
+            GameStage::PreMovement,
+            setup_entities.run_in_state(AppState::InGame),
+        )
+        .add_system_set_to_stage(
+            GameStage::Movement,
+            SystemSet::new().with_system(
+                update
+                    .run_in_state(GameState::Playing)
+                    .label(AltitudeLabels::Update)
+                    .after(RepulsionLables::Apply),
+            ),
+        );
+    }
+}
+
+#[derive(Copy, Clone, Hash, Debug, PartialEq, Eq, SystemLabel)]
+pub(crate) enum AltitudeLabels {
+    Update,
+}
+
+#[derive(Component, Default)]
+pub(crate) struct DesiredClimbing(f32);
+
+impl DesiredClimbing {
+    pub(crate) fn speed(&self) -> f32 {
+        self.0
+    }
+
+    pub(crate) fn set_speed(&mut self, speed: f32) {
+        self.0 = speed;
+    }
+}
+
+fn setup_entities(
+    mut commands: Commands,
+    objects: Query<Entity, (With<MovableSolid>, Without<DesiredClimbing>)>,
+) {
+    for entity in objects.iter() {
+        commands.entity(entity).insert(DesiredClimbing::default());
+    }
+}
+
+fn update(
+    mut objects: Query<(
+        &DesiredVelocity<RepulsionVelocity>,
+        &mut DesiredClimbing,
+        &Transform,
+    )>,
+) {
+    objects.par_for_each_mut(512, |(horizontal, mut climbing, transform)| {
+        let desired_height = if horizontal.stationary() {
+            0.
+        } else {
+            // TODO use max_height
+            10.
+        };
+
+        let remaining = desired_height - transform.translation.y;
+        let desired = remaining.signum()
+            * MAX_VERTICAL_SPEED.min((2. * remaining.abs() * MAX_ACCELERATION).sqrt());
+
+        // Avoid change detection when possible.
+        if climbing.speed() != desired {
+            climbing.set_speed(desired);
+        }
+    });
+}
diff --git a/crates/movement/src/kinematics.rs b/crates/movement/src/kinematics.rs
index 3d9a18223..0adc69450 100644
--- a/crates/movement/src/kinematics.rs
+++ b/crates/movement/src/kinematics.rs
@@ -10,9 +10,10 @@ use de_core::{
 use iyes_loopless::prelude::*;
 
 use crate::{
+    altitude::{AltitudeLabels, DesiredClimbing},
     movement::{DesiredVelocity, MovementLabels, ObjectVelocity},
     repulsion::{RepulsionLables, RepulsionVelocity},
-    MAX_ACCELERATION, MAX_ANGULAR_SPEED, MAX_SPEED,
+    MAX_ACCELERATION, MAX_ANGULAR_SPEED, MAX_HORIZONTAL_SPEED, MAX_VERTICAL_SPEED,
 };
 
 pub(crate) struct KinematicsPlugin;
@@ -30,7 +31,8 @@ impl Plugin for KinematicsPlugin {
                     .run_in_state(GameState::Playing)
                     .label(KinematicsLabels::Kinematics)
                     .before(MovementLabels::UpdateTransform)
-                    .after(RepulsionLables::Apply),
+                    .after(RepulsionLables::Apply)
+                    .after(AltitudeLabels::Update),
             ),
         );
     }
@@ -46,24 +48,36 @@ type Uninitialized<'w, 's> =
 
 #[derive(Component)]
 struct Kinematics {
-    /// Current speed in meters per second.
-    speed: f32,
+    /// Current horizontal speed in meters per second.
+    horizontal_speed: f32,
+    /// Current vertical speed in meters per second.
+    vertical_speed: f32,
     /// Current object heading in radians.
     heading: f32,
 }
 
 impl Kinematics {
-    fn speed(&self) -> f32 {
-        self.speed
+    fn horizontal_speed(&self) -> f32 {
+        self.horizontal_speed
+    }
+
+    fn vertical_speed(&self) -> f32 {
+        self.vertical_speed
     }
 
     fn heading(&self) -> f32 {
         self.heading
     }
 
-    fn update_speed(&mut self, delta: f32) {
+    fn update_horizontal_speed(&mut self, delta: f32) {
         debug_assert!(delta.is_finite());
-        self.speed = (self.speed + delta).clamp(0., MAX_SPEED);
+        self.horizontal_speed = (self.horizontal_speed + delta).clamp(0., MAX_HORIZONTAL_SPEED);
+    }
+
+    fn update_vertical_speed(&mut self, delta: f32) {
+        debug_assert!(delta.is_finite());
+        self.vertical_speed =
+            (self.vertical_speed + delta).clamp(-MAX_VERTICAL_SPEED, MAX_VERTICAL_SPEED);
     }
 
     fn update_heading(&mut self, delta: f32) {
@@ -73,14 +87,15 @@ impl Kinematics {
 
     fn compute_velocity(&self) -> Vec3 {
         let (sin, cos) = self.heading.sin_cos();
-        Vec2::new(self.speed * cos, self.speed * sin).to_msl()
+        (self.horizontal_speed * Vec2::new(cos, sin)).to_altitude(self.vertical_speed)
     }
 }
 
 impl From<&Transform> for Kinematics {
     fn from(transform: &Transform) -> Self {
         Self {
-            speed: 0.,
+            horizontal_speed: 0.,
+            vertical_speed: 0.,
             heading: normalize_angle(transform.rotation.to_euler(EulerRot::YXZ).0),
         }
     }
@@ -96,18 +111,21 @@ fn kinematics(
     time: Res<Time>,
     mut objects: Query<(
         &DesiredVelocity<RepulsionVelocity>,
+        &DesiredClimbing,
         &mut Kinematics,
         &mut ObjectVelocity,
     )>,
 ) {
     let time_delta = time.delta_seconds();
 
-    objects.par_for_each_mut(512, |(movement, mut kinematics, mut velocity)| {
-        let desired_velocity = movement.velocity();
-        let desired_heading = if desired_velocity == Vec2::ZERO {
+    objects.par_for_each_mut(512, |(movement, climbing, mut kinematics, mut velocity)| {
+        let desired_horizontal_velocity = movement.velocity();
+        let desired_heading = if desired_horizontal_velocity == Vec2::ZERO {
             kinematics.heading()
         } else {
-            desired_velocity.y.atan2(desired_velocity.x)
+            desired_horizontal_velocity
+                .y
+                .atan2(desired_horizontal_velocity.x)
         };
 
         let heading_diff = normalize_angle(desired_heading - kinematics.heading());
@@ -116,15 +134,20 @@ fn kinematics(
         kinematics.update_heading(heading_delta);
 
         let max_speed_delta = MAX_ACCELERATION * time_delta;
-        let speed_delta = if (heading_diff - heading_delta).abs() > FRAC_PI_4 {
+        let horizontal_speed_delta = if (heading_diff - heading_delta).abs() > FRAC_PI_4 {
             // Slow down if not going in roughly good direction.
-            -kinematics.speed()
+            -kinematics.horizontal_speed()
         } else {
-            desired_velocity.length() - kinematics.speed()
+            desired_horizontal_velocity.length() - kinematics.horizontal_speed()
         }
         .clamp(-max_speed_delta, max_speed_delta);
 
-        kinematics.update_speed(speed_delta);
+        kinematics.update_horizontal_speed(horizontal_speed_delta);
+
+        let vertical_speed_delta = (climbing.speed() - kinematics.vertical_speed())
+            .clamp(-max_speed_delta, max_speed_delta);
+        kinematics.update_vertical_speed(vertical_speed_delta);
+
         velocity.update(kinematics.compute_velocity(), kinematics.heading());
     });
 }
diff --git a/crates/movement/src/lib.rs b/crates/movement/src/lib.rs
index 6ad7d7ce2..3470a71ba 100644
--- a/crates/movement/src/lib.rs
+++ b/crates/movement/src/lib.rs
@@ -1,3 +1,4 @@
+mod altitude;
 mod cache;
 mod disc;
 mod kinematics;
@@ -8,6 +9,7 @@ mod repulsion;
 
 use std::f32::consts::PI;
 
+use altitude::AltitudePlugin;
 use bevy::{app::PluginGroupBuilder, prelude::PluginGroup};
 use kinematics::KinematicsPlugin;
 use movement::MovementPlugin;
@@ -15,12 +17,16 @@ use obstacles::ObstaclesPlugin;
 use pathing::PathingPlugin;
 use repulsion::RepulsionPlugin;
 
-/// Maximum object speed in meters per second.
-const MAX_SPEED: f32 = 10.;
+/// Maximum object horizontal speed in meters per second.
+const MAX_HORIZONTAL_SPEED: f32 = 10.;
+/// Maximum object vertical ascending / descending rate in meters per second.
+const MAX_VERTICAL_SPEED: f32 = 1.;
 /// Maximum object acceleration in meters per second squared.
-const MAX_ACCELERATION: f32 = 2. * MAX_SPEED;
+const MAX_ACCELERATION: f32 = 2. * MAX_HORIZONTAL_SPEED;
 /// Maximum object angular velocity in radians per second.
 const MAX_ANGULAR_SPEED: f32 = PI;
+/// Maximum altitude in meters (note that this is not height).
+const MAX_ALTITUDE: f32 = 100.;
 
 pub struct MovementPluginGroup;
 
@@ -32,5 +38,6 @@ impl PluginGroup for MovementPluginGroup {
             .add(ObstaclesPlugin)
             .add(RepulsionPlugin)
             .add(KinematicsPlugin)
+            .add(AltitudePlugin)
     }
 }
diff --git a/crates/movement/src/movement.rs b/crates/movement/src/movement.rs
index 0a4579000..32e32ab2a 100644
--- a/crates/movement/src/movement.rs
+++ b/crates/movement/src/movement.rs
@@ -3,7 +3,7 @@ use std::marker::PhantomData;
 use bevy::prelude::*;
 use de_core::{
     objects::MovableSolid,
-    projection::{ToAltitude, ToFlat},
+    projection::ToAltitude,
     stages::GameStage,
     state::{AppState, GameState},
 };
@@ -11,6 +11,8 @@ use de_map::size::MapBounds;
 use de_objects::EXCLUSION_OFFSET;
 use iyes_loopless::prelude::*;
 
+use crate::MAX_ALTITUDE;
+
 pub(crate) struct MovementPlugin;
 
 impl Plugin for MovementPlugin {
@@ -149,8 +151,7 @@ fn update_transform(
 
 fn clamp(bounds: &MapBounds, translation: Vec3) -> Vec3 {
     let offset = Vec2::splat(EXCLUSION_OFFSET);
-    let min = bounds.min() + offset;
-    let max = bounds.max() - offset;
-    let clipped = translation.to_flat().clamp(min, max).to_msl();
-    Vec3::new(clipped.x, translation.y, clipped.z)
+    let min = (bounds.min() + offset).to_msl();
+    let max = (bounds.max() - offset).to_altitude(MAX_ALTITUDE);
+    translation.clamp(min, max)
 }
diff --git a/crates/movement/src/pathing.rs b/crates/movement/src/pathing.rs
index 94b877cfa..a7efc3070 100644
--- a/crates/movement/src/pathing.rs
+++ b/crates/movement/src/pathing.rs
@@ -9,7 +9,7 @@ use iyes_loopless::prelude::*;
 
 use crate::{
     movement::{add_desired_velocity, DesiredVelocity},
-    MAX_ACCELERATION, MAX_SPEED,
+    MAX_ACCELERATION, MAX_HORIZONTAL_SPEED,
 };
 
 const DESTINATION_ACCURACY: f32 = 0.1;
@@ -70,9 +70,9 @@ fn follow_path(
     objects.par_for_each_mut(512, |(transform, mut path, mut movement)| {
         let location = transform.translation.to_flat();
         let remaining = path.destination().distance(location);
-        let advancement = path.advance(location, MAX_SPEED * 0.5);
+        let advancement = path.advance(location, MAX_HORIZONTAL_SPEED * 0.5);
         let direction = (advancement - location).normalize();
-        let desired_speed = MAX_SPEED.min((2. * remaining * MAX_ACCELERATION).sqrt());
+        let desired_speed = MAX_HORIZONTAL_SPEED.min((2. * remaining * MAX_ACCELERATION).sqrt());
         movement.update(desired_speed * direction);
     });
 }
diff --git a/crates/movement/src/repulsion.rs b/crates/movement/src/repulsion.rs
index 65166007b..0068a5417 100644
--- a/crates/movement/src/repulsion.rs
+++ b/crates/movement/src/repulsion.rs
@@ -16,7 +16,7 @@ use crate::{
     movement::{add_desired_velocity, DesiredVelocity},
     obstacles::{MovableObstacles, ObstaclesLables, StaticObstacles},
     pathing::{PathVelocity, PathingLabels},
-    MAX_ACCELERATION, MAX_SPEED,
+    MAX_ACCELERATION, MAX_HORIZONTAL_SPEED,
 };
 
 const MAX_REPULSION_DISTANCE: f32 = 4.0;
@@ -276,7 +276,7 @@ fn apply(
         512,
         |(mut repulsion, path_velocity, mut repulsion_velocity)| {
             let velocity = repulsion.apply(path_velocity.velocity());
-            repulsion_velocity.update(velocity.clamp_length_max(MAX_SPEED));
+            repulsion_velocity.update(velocity.clamp_length_max(MAX_HORIZONTAL_SPEED));
             repulsion.clear();
         },
     );
diff --git a/crates/objects/src/cache.rs b/crates/objects/src/cache.rs
index 0098fa5b4..6f3f996cd 100644
--- a/crates/objects/src/cache.rs
+++ b/crates/objects/src/cache.rs
@@ -15,7 +15,7 @@ use iyes_progress::prelude::*;
 use crate::{
     ichnography::Ichnography,
     loader::{ObjectInfo, ObjectLoader},
-    LaserCannon, ObjectCollider,
+    LaserCannon, ObjectCollider, ObjectMovement,
 };
 
 pub(crate) struct CachePlugin;
@@ -69,6 +69,7 @@ pub struct CacheItem {
     ichnography: Ichnography,
     collider: ObjectCollider,
     cannon: Option<LaserCannon>,
+    movement: Option<ObjectMovement>,
 }
 
 impl CacheItem {
@@ -80,6 +81,12 @@ impl CacheItem {
         self.cannon.as_ref()
     }
 
+    /// Object movement configuration. It is None iff the object is not
+    /// movable.
+    pub fn movement(&self) -> Option<&ObjectMovement> {
+        self.movement.as_ref()
+    }
+
     pub(crate) fn ichnography(&self) -> &Ichnography {
         &self.ichnography
     }
@@ -114,7 +121,7 @@ impl CacheLoader {
         InnerCache {
             objects: self
                 .objects
-                .map(|_, loader| loader.into_cache_item(objects)),
+                .map(|object_type, loader| loader.into_cache_item(object_type, objects)),
         }
     }
 
@@ -148,13 +155,25 @@ impl ItemLoader {
         }
     }
 
-    fn into_cache_item(self, objects: &Assets<ObjectInfo>) -> CacheItem {
+    /// # Panics
+    ///
+    /// Panics if the object is wrongly configured.
+    fn into_cache_item(self, object_type: ObjectType, objects: &Assets<ObjectInfo>) -> CacheItem {
         let object_info = objects.get(&self.object_info).unwrap();
+
+        assert!(
+            matches!(object_type, ObjectType::Active(ActiveObjectType::Unit(_)))
+                == object_info.movement().is_some(),
+            "Object {} has unexpected movement info configured.",
+            object_type
+        );
+
         CacheItem {
             scene: self.scene,
             ichnography: Ichnography::from(object_info.footprint()),
             collider: ObjectCollider::from(object_info.shape()),
             cannon: object_info.cannon().map(LaserCannon::from),
+            movement: object_info.movement().map(ObjectMovement::from),
         }
     }
 
diff --git a/crates/objects/src/lib.rs b/crates/objects/src/lib.rs
index 958d40576..6759d4900 100644
--- a/crates/objects/src/lib.rs
+++ b/crates/objects/src/lib.rs
@@ -9,6 +9,7 @@ pub use collider::{ColliderCache, ObjectCollider};
 use health::HealthPlugin;
 pub use health::{Health, InitialHealths};
 pub use ichnography::{Ichnography, IchnographyCache, EXCLUSION_OFFSET};
+pub use movement::ObjectMovement;
 
 mod cache;
 mod cannon;
@@ -16,6 +17,7 @@ mod collider;
 mod health;
 mod ichnography;
 mod loader;
+mod movement;
 
 pub struct ObjectsPluginGroup;
 
diff --git a/crates/objects/src/loader.rs b/crates/objects/src/loader.rs
index 46864f1aa..42cd91eaf 100644
--- a/crates/objects/src/loader.rs
+++ b/crates/objects/src/loader.rs
@@ -13,6 +13,7 @@ pub(crate) struct ObjectInfo {
     footprint: Footprint,
     shape: TriMeshShape,
     cannon: Option<LaserCannonInfo>,
+    movement: Option<MovementInfo>,
 }
 
 impl ObjectInfo {
@@ -27,6 +28,10 @@ impl ObjectInfo {
     pub(crate) fn cannon(&self) -> Option<&LaserCannonInfo> {
         self.cannon.as_ref()
     }
+
+    pub(crate) fn movement(&self) -> Option<&MovementInfo> {
+        self.movement.as_ref()
+    }
 }
 
 #[derive(Serialize, Deserialize)]
@@ -84,6 +89,17 @@ impl LaserCannonInfo {
     }
 }
 
+#[derive(Serialize, Deserialize)]
+pub(crate) struct MovementInfo {
+    max_height: Option<f32>,
+}
+
+impl MovementInfo {
+    pub(crate) fn max_height(&self) -> Option<f32> {
+        self.max_height
+    }
+}
+
 pub(crate) struct ObjectLoader;
 
 impl AssetLoader for ObjectLoader {
diff --git a/crates/objects/src/movement.rs b/crates/objects/src/movement.rs
new file mode 100644
index 000000000..75ea462bd
--- /dev/null
+++ b/crates/objects/src/movement.rs
@@ -0,0 +1,21 @@
+use crate::loader::MovementInfo;
+
+pub struct ObjectMovement {
+    max_height: Option<f32>,
+}
+
+impl ObjectMovement {
+    /// Returns maximum height (above terrain) of the object if it can fly.
+    /// Returns None otherwise.
+    pub fn max_height(&self) -> Option<f32> {
+        self.max_height
+    }
+}
+
+impl From<&MovementInfo> for ObjectMovement {
+    fn from(info: &MovementInfo) -> Self {
+        Self {
+            max_height: info.max_height(),
+        }
+    }
+}
diff --git a/docs/content/objects/schema.json b/docs/content/objects/schema.json
index 32650519c..e9b663255 100644
--- a/docs/content/objects/schema.json
+++ b/docs/content/objects/schema.json
@@ -103,6 +103,17 @@
         "damage",
         "recharge_interval"
       ]
+    },
+    "movement": {
+      "type": "object",
+      "description": "Configuration of object movement capabilities. This property is not defined for non-movable object.",
+      "properties": {
+        "max_height": {
+          "type": "number",
+          "description": "Maximum height (vertical distance from the terrain) of flying objects. Not defined for object which cannot fly.",
+          "exclusiveMinimum": 0
+        }
+      }
     }
   },
   "required": [