Implement PID controllers and use them to stabilize Agent airship flight.

CHANGELOG.md

@@ -143,6 +143,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Fixed terrain clipping with glider
 - Fixed an issue where prices weren't properly making their way from econsim to the actual trade values.
 - Fixed entities with voxel colliders being off by one physics tick for collision.
+- Airships no longer oscillate dramatically into the sky due to mistaking velocity for acceleration.
 
 ## [0.9.0] - 2021-03-20
 

common/src/comp/agent.rs

@@ -7,7 +7,7 @@ use crate::{
 };
 use specs::{Component, Entity as EcsEntity};
 use specs_idvs::IdvStorage;
-use std::collections::VecDeque;
+use std::{collections::VecDeque, fmt};
 use vek::*;
 
 use super::dialogue::Subject;
@@ -300,6 +300,7 @@ pub struct Target {
     pub selected_at: f64,
 }
 
+#[allow(clippy::type_complexity)]
 #[derive(Clone, Debug, Default)]
 pub struct Agent {
     pub rtsim_controller: RtSimController,
@@ -313,6 +314,7 @@ pub struct Agent {
     pub bearing: Vec2<f32>,
     pub sounds_heard: Vec<Sound>,
     pub awareness: f32,
+    pub pid_controller: Option<PidController<fn(Vec3<f32>, Vec3<f32>) -> f32, 16>>,
 }
 
 #[derive(Clone, Debug, Default)]
@@ -336,6 +338,15 @@ impl Agent {
         self
     }
 
+    #[allow(clippy::type_complexity)]
+    pub fn with_pid_controller(
+        mut self,
+        pid: PidController<fn(Vec3<f32>, Vec3<f32>) -> f32, 16>,
+    ) -> Self {
+        self.pid_controller = Some(pid);
+        self
+    }
+
     pub fn new(
         patrol_origin: Option<Vec3<f32>>,
         body: &Body,
@@ -385,3 +396,83 @@ mod tests {
         assert!(b.can(BehaviorCapability::SPEAK));
     }
 }
+
+#[derive(Clone)]
+pub struct PidController<F: Fn(Vec3<f32>, Vec3<f32>) -> f32, const NUM_SAMPLES: usize> {
+    pub kp: f32,
+    pub ki: f32,
+    pub kd: f32,
+    pub sp: Vec3<f32>,
+    pv_samples: [(f64, Vec3<f32>); NUM_SAMPLES],
+    pv_idx: usize,
+    e: F,
+}
+
+impl<F: Fn(Vec3<f32>, Vec3<f32>) -> f32, const NUM_SAMPLES: usize> fmt::Debug
+    for PidController<F, NUM_SAMPLES>
+{
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        f.debug_struct("PidController")
+            .field("kp", &self.kp)
+            .field("ki", &self.ki)
+            .field("kd", &self.kd)
+            .field("sp", &self.sp)
+            .field("pv_samples", &self.pv_samples)
+            .field("pv_idx", &self.pv_idx)
+            .finish()
+    }
+}
+
+impl<F: Fn(Vec3<f32>, Vec3<f32>) -> f32, const NUM_SAMPLES: usize> PidController<F, NUM_SAMPLES> {
+    pub fn new(kp: f32, ki: f32, kd: f32, sp: Vec3<f32>, time: f64, e: F) -> Self {
+        Self {
+            kp,
+            ki,
+            kd,
+            sp,
+            pv_samples: [(time, Vec3::zero()); NUM_SAMPLES],
+            pv_idx: 0,
+            e,
+        }
+    }
+
+    pub fn add_measurement(&mut self, time: f64, pv: Vec3<f32>) {
+        self.pv_idx += 1;
+        self.pv_idx %= NUM_SAMPLES;
+        self.pv_samples[self.pv_idx] = (time, pv);
+    }
+
+    /// https://en.wikipedia.org/wiki/PID_controller#Mathematical_form
+    pub fn calc_err(&self) -> f32 {
+        self.kp * self.proportional_err()
+            + self.ki * self.integral_err()
+            + self.kd * self.derivative_err()
+    }
+
+    pub fn proportional_err(&self) -> f32 { (self.e)(self.sp, self.pv_samples[self.pv_idx].1) }
+
+    /// https://en.wikipedia.org/wiki/Trapezoidal_rule#Uniform_grid
+    pub fn integral_err(&self) -> f32 {
+        let f = |x| (self.e)(self.sp, x);
+        let (a, x0) = self.pv_samples[(self.pv_idx + 1) % NUM_SAMPLES];
+        let (b, xn) = self.pv_samples[self.pv_idx];
+        let dx = (b - a) / NUM_SAMPLES as f64;
+        let mut err = 0.0;
+        for i in 1..=NUM_SAMPLES - 1 {
+            let xk = self.pv_samples[(self.pv_idx + 1 + i) % NUM_SAMPLES].1;
+            err += f(xk);
+        }
+        err += (f(xn) - f(x0)) / 2.0;
+        err *= dx as f32;
+        err
+    }
+
+    /// https://en.wikipedia.org/wiki/Numerical_differentiation#Finite_differences
+    pub fn derivative_err(&self) -> f32 {
+        let f = |x| (self.e)(self.sp, x);
+        let (a, x0) = self.pv_samples[(self.pv_idx + NUM_SAMPLES - 1) % NUM_SAMPLES];
+        let (b, x1) = self.pv_samples[self.pv_idx];
+        let h = b - a;
+        (f(x1) - f(x0)) / h as f32
+    }
+}

common/src/comp/body/ship.rs

@@ -52,6 +52,13 @@ impl Body {
     pub fn density(&self) -> Density { Density(AIR_DENSITY) }
 
     pub fn mass(&self) -> Mass { Mass((self.hull_vol() + self.balloon_vol()) * self.density().0) }
+
+    pub fn pid_coefficients(&self) -> (f32, f32, f32) {
+        let kp = 1.0;
+        let ki = 1.0;
+        let kd = 1.0;
+        (kp, ki, kd)
+    }
 }
 
 /// Terrain is 11.0 scale relative to small-scale voxels,

common/src/comp/mod.rs

@@ -46,7 +46,7 @@ pub mod visual;
 pub use self::{
     ability::{CharacterAbility, CharacterAbilityType},
     admin::{Admin, AdminRole},
-    agent::{Agent, Alignment, Behavior, BehaviorCapability, BehaviorState},
+    agent::{Agent, Alignment, Behavior, BehaviorCapability, BehaviorState, PidController},
     aura::{Aura, AuraChange, AuraKind, Auras},
     beam::{Beam, BeamSegment},
     body::{

server/src/cmd.rs

@@ -1152,9 +1152,10 @@ fn handle_spawn_airship(
                 200.0,
             )
     });
+    let ship = comp::ship::Body::DefaultAirship;
     let mut builder = server
         .state
-        .create_ship(pos, comp::ship::Body::DefaultAirship, true)
+        .create_ship(pos, ship, true)
         .with(LightEmitter {
             col: Rgb::new(1.0, 0.65, 0.2),
             strength: 2.0,
@@ -1162,7 +1163,19 @@ fn handle_spawn_airship(
             animated: true,
         });
     if let Some(pos) = destination {
-        builder = builder.with(comp::Agent::default().with_destination(pos))
+        let (kp, ki, kd) = ship.pid_coefficients();
+        fn pure_z(sp: Vec3<f32>, pv: Vec3<f32>) -> f32 { (sp - pv).z }
+        let agent = comp::Agent::default()
+            .with_destination(pos)
+            .with_pid_controller(comp::PidController::new(
+                kp,
+                ki,
+                kd,
+                Vec3::zero(),
+                0.0,
+                pure_z,
+            ));
+        builder = builder.with(agent);
     }
     builder.build();
 

server/src/events/entity_creation.rs

@@ -8,8 +8,8 @@ use common::{
         buff::{BuffCategory, BuffData, BuffKind, BuffSource},
         inventory::loadout::Loadout,
         shockwave, Agent, Alignment, Body, Health, HomeChunk, Inventory, Item, ItemDrop,
-        LightEmitter, Object, Ori, Poise, Pos, Projectile, Scale, SkillSet, Stats, Vel,
-        WaypointArea,
+        LightEmitter, Object, Ori, PidController, Poise, Pos, Projectile, Scale, SkillSet, Stats,
+        Vel, WaypointArea,
     },
     outcome::Outcome,
     rtsim::RtSimEntity,
@@ -146,7 +146,11 @@ pub fn handle_create_ship(
     rtsim_entity: Option<RtSimEntity>,
 ) {
     let mut entity = server.state.create_ship(pos, ship, mountable);
-    if let Some(agent) = agent {
+    if let Some(mut agent) = agent {
+        let (kp, ki, kd) = ship.pid_coefficients();
+        fn pure_z(sp: Vec3<f32>, pv: Vec3<f32>) -> f32 { (sp - pv).z }
+        agent =
+            agent.with_pid_controller(PidController::new(kp, ki, kd, Vec3::zero(), 0.0, pure_z));
         entity = entity.with(agent);
     }
     if let Some(rtsim_entity) = rtsim_entity {

server/src/sys/agent.rs

@@ -271,6 +271,10 @@ impl<'a> System<'a> for Sys {
                         Some(CharacterState::GlideWield) | Some(CharacterState::Glide(_))
                     ) && !physics_state.on_ground;
 
+                    if let Some(pid) = agent.pid_controller.as_mut() {
+                        pid.add_measurement(read_data.time.0, pos.0);
+                    }
+
                     // This controls how picky NPCs are about their pathfinding. Giants are larger
                     // and so can afford to be less precise when trying to move around
                     // the world (especially since they would otherwise get stuck on
@@ -800,7 +804,7 @@ impl<'a> AgentData<'a> {
                 controller.inputs.climb = Some(comp::Climb::Up);
                 //.filter(|_| bearing.z > 0.1 || self.physics_state.in_liquid().is_some());
 
-                controller.inputs.move_z = bearing.z
+                let height_offset = bearing.z
                     + if self.traversal_config.can_fly {
                         // NOTE: costs 4 us (imbris)
                         let obstacle_ahead = read_data
@@ -820,14 +824,14 @@ impl<'a> AgentData<'a> {
                             .body
                             .map(|body| {
                                 #[cfg(feature = "worldgen")]
-                                let height_approx = self.pos.0.y
+                                let height_approx = self.pos.0.z
                                     - read_data
                                         .world
                                         .sim()
                                         .get_alt_approx(self.pos.0.xy().map(|x: f32| x as i32))
                                         .unwrap_or(0.0);
                                 #[cfg(not(feature = "worldgen"))]
-                                let height_approx = self.pos.0.y;
+                                let height_approx = self.pos.0.z;
 
                                 height_approx < body.flying_height()
                             })
@@ -859,14 +863,19 @@ impl<'a> AgentData<'a> {
                         }
 
                         if obstacle_ahead || ground_too_close {
-                            1.0 //fly up when approaching obstacles
+                            5.0 //fly up when approaching obstacles
                         } else {
-                            -0.1
+                            -2.0
                         } //flying things should slowly come down from the stratosphere
                     } else {
                         0.05 //normal land traveller offset
                     };
-
+                if let Some(pid) = agent.pid_controller.as_mut() {
+                    pid.sp = self.pos.0.z + height_offset * Vec3::unit_z();
+                    controller.inputs.move_z = pid.calc_err();
+                } else {
+                    controller.inputs.move_z = height_offset;
+                }
                 // Put away weapon
                 if thread_rng().gen_bool(0.1)
                     && matches!(