diff --git a/CHANGELOG.md b/CHANGELOG.md index 542c1b35d2..cf8840897a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -144,6 +144,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 diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs index 0bb02f01ec..3fade71b8c 100644 --- a/common/src/comp/agent.rs +++ b/common/src/comp/agent.rs @@ -1,5 +1,5 @@ use crate::{ - comp::{humanoid, quadruped_low, quadruped_medium, quadruped_small, Body}, + comp::{humanoid, quadruped_low, quadruped_medium, quadruped_small, ship, Body}, path::Chaser, rtsim::RtSimController, trade::{PendingTrade, ReducedInventory, SiteId, SitePrices, TradeId, TradeResult}, @@ -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, pub sounds_heard: Vec, pub awareness: f32, + pub position_pid_controller: Option, Vec3) -> f32, 16>>, } #[derive(Clone, Debug, Default)] @@ -336,6 +338,15 @@ impl Agent { self } + #[allow(clippy::type_complexity)] + pub fn with_position_pid_controller( + mut self, + pid: PidController, Vec3) -> f32, 16>, + ) -> Self { + self.position_pid_controller = Some(pid); + self + } + pub fn new( patrol_origin: Option>, body: &Body, @@ -385,3 +396,125 @@ mod tests { assert!(b.can(BehaviorCapability::SPEAK)); } } + +/// PID controllers are used for automatically adapting nonlinear controls (like +/// buoyancy for airships) to target specific outcomes (i.e. a specific height) +#[derive(Clone)] +pub struct PidController, Vec3) -> f32, const NUM_SAMPLES: usize> { + /// The coefficient of the proportional term + pub kp: f32, + /// The coefficient of the integral term + pub ki: f32, + /// The coefficient of the derivative term + pub kd: f32, + /// The setpoint that the process has as its goal + pub sp: Vec3, + /// A ring buffer of the last NUM_SAMPLES measured process variables + pv_samples: [(f64, Vec3); NUM_SAMPLES], + /// The index into the ring buffer of process variables + pv_idx: usize, + /// The error function, to change how the difference between the setpoint + /// and process variables are calculated + e: F, +} + +impl, Vec3) -> f32, const NUM_SAMPLES: usize> fmt::Debug + for PidController +{ + 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, Vec3) -> f32, const NUM_SAMPLES: usize> PidController { + /// Constructs a PidController with the specified weights, setpoint, + /// starting time, and error function + pub fn new(kp: f32, ki: f32, kd: f32, sp: Vec3, time: f64, e: F) -> Self { + Self { + kp, + ki, + kd, + sp, + pv_samples: [(time, Vec3::zero()); NUM_SAMPLES], + pv_idx: 0, + e, + } + } + + /// Adds a measurement of the process variable to the ringbuffer + pub fn add_measurement(&mut self, time: f64, pv: Vec3) { + self.pv_idx += 1; + self.pv_idx %= NUM_SAMPLES; + self.pv_samples[self.pv_idx] = (time, pv); + } + + /// The amount to set the control variable to is a weighed sum of the + /// proportional error, the integral error, and the derivative error. + /// 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() + } + + /// The proportional error is the error function applied to the set point + /// and the most recent process variable measurement + pub fn proportional_err(&self) -> f32 { (self.e)(self.sp, self.pv_samples[self.pv_idx].1) } + + /// The integral error is the error function integrated over the last + /// NUM_SAMPLES values. The trapezoid rule for numerical integration was + /// chosen because it's fairly easy to calculate and sufficiently accurate. + /// 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; + // \Sigma_{k=1}^{N-1} f(x_k) + for k in 1..=NUM_SAMPLES - 1 { + let xk = self.pv_samples[(self.pv_idx + 1 + k) % NUM_SAMPLES].1; + err += f(xk); + } + // (\Sigma_{k=1}^{N-1} f(x_k)) + \frac{f(x_N) + f(x_0)}{2} + err += (f(xn) - f(x0)) / 2.0; + // \Delta x * ((\Sigma_{k=1}^{N-1} f(x_k)) + \frac{f(x_N) + f(x_0)}{2}) + err *= dx as f32; + err + } + + /// The derivative error is the numerical derivative of the error function + /// based on the most recent 2 samples. Using more than 2 samples might + /// improve the accuracy of the estimate of the derivative, but it would be + /// an estimate of the derivative error further in the past. + /// 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 + } +} + +/// Get the PID coefficients associated with some Body, since it will likely +/// need to be tuned differently for each body type +pub fn pid_coefficients(body: &Body) -> (f32, f32, f32) { + match body { + Body::Ship(ship::Body::DefaultAirship) => { + let kp = 1.0; + let ki = 1.0; + let kd = 1.0; + (kp, ki, kd) + }, + // default to a pure-proportional controller, which is the first step when tuning + _ => (1.0, 0.0, 0.0), + } +} diff --git a/common/src/comp/mod.rs b/common/src/comp/mod.rs index bde3d3c3ee..f120798be7 100644 --- a/common/src/comp/mod.rs +++ b/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::{ diff --git a/server/src/cmd.rs b/server/src/cmd.rs index 1338707477..b350359a88 100644 --- a/server/src/cmd.rs +++ b/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) = comp::agent::pid_coefficients(&comp::Body::Ship(ship)); + fn pure_z(sp: Vec3, pv: Vec3) -> f32 { (sp - pv).z } + let agent = comp::Agent::default() + .with_destination(pos) + .with_position_pid_controller(comp::PidController::new( + kp, + ki, + kd, + Vec3::zero(), + 0.0, + pure_z, + )); + builder = builder.with(agent); } builder.build(); diff --git a/server/src/events/entity_creation.rs b/server/src/events/entity_creation.rs index 1a59e6096c..c66317cf0e 100644 --- a/server/src/events/entity_creation.rs +++ b/server/src/events/entity_creation.rs @@ -3,13 +3,14 @@ use common::{ character::CharacterId, comp::{ self, + agent::pid_coefficients, aura::{Aura, AuraKind, AuraTarget}, beam, 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 +147,17 @@ pub fn handle_create_ship( rtsim_entity: Option, ) { 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) = pid_coefficients(&Body::Ship(ship)); + fn pure_z(sp: Vec3, pv: Vec3) -> f32 { (sp - pv).z } + agent = agent.with_position_pid_controller(PidController::new( + kp, + ki, + kd, + Vec3::zero(), + 0.0, + pure_z, + )); entity = entity.with(agent); } if let Some(rtsim_entity) = rtsim_entity { diff --git a/server/src/sys/agent.rs b/server/src/sys/agent.rs index 7ac35e0642..7f26303ef6 100644 --- a/server/src/sys/agent.rs +++ b/server/src/sys/agent.rs @@ -273,6 +273,10 @@ impl<'a> System<'a> for Sys { Some(CharacterState::GlideWield) | Some(CharacterState::Glide(_)) ) && !physics_state.on_ground; + if let Some(pid) = agent.position_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 @@ -802,7 +806,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 @@ -822,14 +826,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() }) @@ -861,14 +865,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.position_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!(