use crate::{
comp::{
biped_large, biped_small, bird_medium, humanoid, quadruped_low, quadruped_medium,
quadruped_small, ship, Body, UtteranceKind,
},
path::Chaser,
rtsim::{NpcInput, RtSimController},
trade::{PendingTrade, ReducedInventory, SiteId, SitePrices, TradeId, TradeResult},
uid::Uid,
};
use serde::{Deserialize, Serialize};
use specs::{Component, DerefFlaggedStorage, Entity as EcsEntity};
use std::{collections::VecDeque, fmt};
use strum::{EnumIter, IntoEnumIterator};
use vek::*;
use super::{dialogue::Subject, Group, Pos};
pub const DEFAULT_INTERACTION_TIME: f32 = 3.0;
pub const TRADE_INTERACTION_TIME: f32 = 300.0;
const SECONDS_BEFORE_FORGET_SOUNDS: f64 = 180.0;
const ACTIONSTATE_NUMBER_OF_CONCURRENT_TIMERS: usize = 5;
const ACTIONSTATE_NUMBER_OF_CONCURRENT_COUNTERS: usize = 5;
const ACTIONSTATE_NUMBER_OF_CONCURRENT_INT_COUNTERS: usize = 5;
const ACTIONSTATE_NUMBER_OF_CONCURRENT_CONDITIONS: usize = 5;
const ACTIONSTATE_NUMBER_OF_CONCURRENT_POSITIONS: usize = 5;
#[derive(Copy, Clone, Debug, PartialEq, Eq, Serialize, Deserialize)]
pub enum Alignment {
Wild,
Enemy,
Npc,
Tame,
Owned(Uid),
Passive,
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum Mark {
Merchant,
Guard,
}
impl Alignment {
pub fn hostile_towards(self, other: Alignment) -> bool {
match (self, other) {
(Alignment::Passive, _) => false,
(_, Alignment::Passive) => false,
(Alignment::Enemy, Alignment::Enemy) => false,
(Alignment::Enemy, Alignment::Wild) => false,
(Alignment::Wild, Alignment::Enemy) => false,
(Alignment::Wild, Alignment::Wild) => false,
(Alignment::Npc, Alignment::Wild) => false,
(Alignment::Npc, Alignment::Enemy) => true,
(_, Alignment::Enemy) => true,
(Alignment::Enemy, _) => true,
_ => false,
}
}
pub fn passive_towards(self, other: Alignment) -> bool {
match (self, other) {
(Alignment::Enemy, Alignment::Enemy) => true,
(Alignment::Owned(a), Alignment::Owned(b)) if a == b => true,
(Alignment::Npc, Alignment::Npc) => true,
(Alignment::Npc, Alignment::Tame) => true,
(Alignment::Enemy, Alignment::Wild) => true,
(Alignment::Wild, Alignment::Enemy) => true,
(Alignment::Tame, Alignment::Npc) => true,
(Alignment::Tame, Alignment::Tame) => true,
(_, Alignment::Passive) => true,
_ => false,
}
}
pub fn friendly_towards(self, other: Alignment) -> bool {
match (self, other) {
(Alignment::Enemy, Alignment::Enemy) => true,
(Alignment::Owned(a), Alignment::Owned(b)) if a == b => true,
(Alignment::Npc, Alignment::Npc) => true,
(Alignment::Npc, Alignment::Tame) => true,
(Alignment::Tame, Alignment::Npc) => true,
(Alignment::Tame, Alignment::Tame) => true,
(_, Alignment::Passive) => true,
_ => false,
}
}
pub fn group(&self) -> Option<Group> {
match self {
Alignment::Wild => None,
Alignment::Passive => None,
Alignment::Enemy => Some(super::group::ENEMY),
Alignment::Npc | Alignment::Tame => Some(super::group::NPC),
Alignment::Owned(_) => None,
}
}
}
impl Component for Alignment {
type Storage = DerefFlaggedStorage<Self, specs::VecStorage<Self>>;
}
bitflags::bitflags! {
#[derive(Clone, Copy, Debug, Default)]
pub struct BehaviorCapability: u8 {
const SPEAK = 0b00000001;
const TRADE = 0b00000010;
}
}
bitflags::bitflags! {
#[derive(Clone, Copy, Debug, Default)]
pub struct BehaviorState: u8 {
const TRADING = 0b00000001;
const TRADING_ISSUER = 0b00000010;
}
}
#[derive(Default, Copy, Clone, Debug)]
pub enum TradingBehavior {
#[default]
None,
RequireBalanced {
trade_site: SiteId,
},
AcceptFood,
}
impl TradingBehavior {
fn can_trade(&self, alignment: Option<Alignment>, counterparty: Uid) -> bool {
match self {
TradingBehavior::RequireBalanced { .. } => true,
TradingBehavior::AcceptFood => alignment == Some(Alignment::Owned(counterparty)),
TradingBehavior::None => false,
}
}
}
#[derive(Default, Copy, Clone, Debug)]
pub struct Behavior {
capabilities: BehaviorCapability,
state: BehaviorState,
pub trading_behavior: TradingBehavior,
}
impl From<BehaviorCapability> for Behavior {
fn from(capabilities: BehaviorCapability) -> Self {
Behavior {
capabilities,
state: BehaviorState::default(),
trading_behavior: TradingBehavior::None,
}
}
}
impl Behavior {
#[must_use]
pub fn maybe_with_capabilities(
mut self,
maybe_capabilities: Option<BehaviorCapability>,
) -> Self {
if let Some(capabilities) = maybe_capabilities {
self.allow(capabilities)
}
self
}
#[must_use]
pub fn with_trade_site(mut self, trade_site: Option<SiteId>) -> Self {
if let Some(trade_site) = trade_site {
self.trading_behavior = TradingBehavior::RequireBalanced { trade_site };
}
self
}
pub fn allow(&mut self, capabilities: BehaviorCapability) {
self.capabilities.set(capabilities, true)
}
pub fn deny(&mut self, capabilities: BehaviorCapability) {
self.capabilities.set(capabilities, false)
}
pub fn can(&self, capabilities: BehaviorCapability) -> bool {
self.capabilities.contains(capabilities)
}
pub fn can_trade(&self, alignment: Option<Alignment>, counterparty: Uid) -> bool {
self.trading_behavior.can_trade(alignment, counterparty)
}
pub fn set(&mut self, state: BehaviorState) { self.state.set(state, true) }
pub fn unset(&mut self, state: BehaviorState) { self.state.set(state, false) }
pub fn is(&self, state: BehaviorState) -> bool { self.state.contains(state) }
pub fn trade_site(&self) -> Option<SiteId> {
if let TradingBehavior::RequireBalanced { trade_site } = self.trading_behavior {
Some(trade_site)
} else {
None
}
}
}
#[derive(Clone, Debug, Default)]
pub struct Psyche {
pub flee_health: f32,
pub sight_dist: f32,
pub listen_dist: f32,
pub aggro_dist: Option<f32>,
pub idle_wander_factor: f32,
pub aggro_range_multiplier: f32,
pub should_stop_pursuing: bool,
}
impl<'a> From<&'a Body> for Psyche {
fn from(body: &'a Body) -> Self {
Self {
flee_health: match body {
Body::Humanoid(humanoid) => match humanoid.species {
humanoid::Species::Danari => 0.4,
humanoid::Species::Dwarf => 0.3,
humanoid::Species::Elf => 0.4,
humanoid::Species::Human => 0.4,
humanoid::Species::Orc => 0.3,
humanoid::Species::Draugr => 0.3,
},
Body::QuadrupedSmall(quadruped_small) => match quadruped_small.species {
quadruped_small::Species::Pig => 0.5,
quadruped_small::Species::Fox => 0.3,
quadruped_small::Species::Sheep => 0.25,
quadruped_small::Species::Boar => 0.1,
quadruped_small::Species::Skunk => 0.4,
quadruped_small::Species::Cat => 0.99,
quadruped_small::Species::Batfox => 0.1,
quadruped_small::Species::Raccoon => 0.4,
quadruped_small::Species::Hyena => 0.1,
quadruped_small::Species::Dog => 0.8,
quadruped_small::Species::Rabbit | quadruped_small::Species::Jackalope => 0.25,
quadruped_small::Species::Truffler => 0.08,
quadruped_small::Species::Hare => 0.3,
quadruped_small::Species::Goat => 0.3,
quadruped_small::Species::Porcupine => 0.2,
quadruped_small::Species::Turtle => 0.4,
quadruped_small::Species::Beaver => 0.2,
quadruped_small::Species::Rat
| quadruped_small::Species::TreantSapling
| quadruped_small::Species::Holladon
| quadruped_small::Species::MossySnail => 0.0,
_ => 1.0,
},
Body::QuadrupedMedium(quadruped_medium) => match quadruped_medium.species {
quadruped_medium::Species::Antelope => 0.15,
quadruped_medium::Species::Donkey => 0.05,
quadruped_medium::Species::Horse => 0.15,
quadruped_medium::Species::Mouflon => 0.1,
quadruped_medium::Species::Zebra => 0.1,
quadruped_medium::Species::Barghest
| quadruped_medium::Species::Bear
| quadruped_medium::Species::Bristleback
| quadruped_medium::Species::Bonerattler => 0.0,
quadruped_medium::Species::Cattle => 0.1,
quadruped_medium::Species::Frostfang => 0.07,
quadruped_medium::Species::Grolgar => 0.0,
quadruped_medium::Species::Highland => 0.05,
quadruped_medium::Species::Kelpie => 0.35,
quadruped_medium::Species::Lion => 0.0,
quadruped_medium::Species::Moose => 0.15,
quadruped_medium::Species::Panda => 0.35,
quadruped_medium::Species::Saber
| quadruped_medium::Species::Tarasque
| quadruped_medium::Species::Tiger => 0.0,
quadruped_medium::Species::Tuskram => 0.1,
quadruped_medium::Species::Wolf => 0.2,
quadruped_medium::Species::Yak => 0.09,
quadruped_medium::Species::Akhlut
| quadruped_medium::Species::Catoblepas
| quadruped_medium::Species::ClaySteed
| quadruped_medium::Species::Dreadhorn
| quadruped_medium::Species::Hirdrasil
| quadruped_medium::Species::Mammoth
| quadruped_medium::Species::Ngoubou
| quadruped_medium::Species::Roshwalr => 0.0,
_ => 0.15,
},
Body::QuadrupedLow(quadruped_low) => match quadruped_low.species {
quadruped_low::Species::Pangolin => 0.3,
quadruped_low::Species::Tortoise => 0.1,
_ => 0.0,
},
Body::BipedSmall(biped_small) => match biped_small.species {
biped_small::Species::Gnarling => 0.2,
biped_small::Species::Mandragora => 0.1,
biped_small::Species::Adlet => 0.2,
biped_small::Species::Haniwa => 0.1,
biped_small::Species::Sahagin => 0.1,
biped_small::Species::Myrmidon => 0.0,
biped_small::Species::TreasureEgg => 9.9,
biped_small::Species::Husk
| biped_small::Species::Boreal
| biped_small::Species::IronDwarf
| biped_small::Species::Flamekeeper
| biped_small::Species::Irrwurz
| biped_small::Species::GoblinThug
| biped_small::Species::GoblinChucker
| biped_small::Species::GoblinRuffian
| biped_small::Species::GreenLegoom
| biped_small::Species::OchreLegoom
| biped_small::Species::PurpleLegoom
| biped_small::Species::RedLegoom
| biped_small::Species::ShamanicSpirit
| biped_small::Species::Jiangshi
| biped_small::Species::Bushly
| biped_small::Species::Cactid
| biped_small::Species::BloodmoonHeiress
| biped_small::Species::Bloodservant
| biped_small::Species::Harlequin
| biped_small::Species::GnarlingChieftain => 0.0,
_ => 0.5,
},
Body::BirdMedium(bird_medium) => match bird_medium.species {
bird_medium::Species::SnowyOwl => 0.4,
bird_medium::Species::HornedOwl => 0.4,
bird_medium::Species::Duck => 0.6,
bird_medium::Species::Cockatiel => 0.6,
bird_medium::Species::Chicken => 0.5,
bird_medium::Species::Bat => 0.1,
bird_medium::Species::Penguin => 0.5,
bird_medium::Species::Goose => 0.4,
bird_medium::Species::Peacock => 0.3,
bird_medium::Species::Eagle => 0.2,
bird_medium::Species::Parrot => 0.8,
bird_medium::Species::Crow => 0.4,
bird_medium::Species::Dodo => 0.8,
bird_medium::Species::Parakeet => 0.8,
bird_medium::Species::Puffin => 0.8,
bird_medium::Species::Toucan => 0.4,
bird_medium::Species::BloodmoonBat => 0.0,
bird_medium::Species::VampireBat => 0.0,
},
Body::BirdLarge(_) => 0.0,
Body::FishSmall(_) => 1.0,
Body::FishMedium(_) => 0.75,
Body::BipedLarge(_) => 0.0,
Body::Object(_) => 0.0,
Body::ItemDrop(_) => 0.0,
Body::Golem(_) => 0.0,
Body::Theropod(_) => 0.0,
Body::Ship(_) => 0.0,
Body::Dragon(_) => 0.0,
Body::Arthropod(_) => 0.0,
Body::Crustacean(_) => 0.0,
Body::Plugin(body) => body.flee_health(),
},
sight_dist: match body {
Body::BirdLarge(_) => 250.0,
Body::BipedLarge(biped_large) => match biped_large.species {
biped_large::Species::Gigasfrost => 200.0,
_ => 100.0,
},
_ => 40.0,
},
listen_dist: 30.0,
aggro_dist: match body {
Body::Humanoid(_) => Some(20.0),
_ => None, },
idle_wander_factor: 1.0,
aggro_range_multiplier: 1.0,
should_stop_pursuing: match body {
Body::BirdLarge(_) => false,
Body::BipedLarge(biped_large) => {
!matches!(biped_large.species, biped_large::Species::Gigasfrost)
},
Body::BirdMedium(bird_medium) => {
!matches!(bird_medium.species, bird_medium::Species::BloodmoonBat)
},
_ => true,
},
}
}
}
impl Psyche {
pub fn search_dist(&self) -> f32 {
self.sight_dist.max(self.listen_dist) * self.aggro_range_multiplier
}
}
#[derive(Clone, Debug)]
pub enum AgentEvent {
Talk(Uid, Subject),
TradeInvite(Uid),
TradeAccepted(Uid),
FinishedTrade(TradeResult),
UpdatePendingTrade(
Box<(
TradeId,
PendingTrade,
SitePrices,
[Option<ReducedInventory>; 2],
)>,
),
ServerSound(Sound),
Hurt,
}
#[derive(Copy, Clone, Debug)]
pub struct Sound {
pub kind: SoundKind,
pub pos: Vec3<f32>,
pub vol: f32,
pub time: f64,
}
impl Sound {
pub fn new(kind: SoundKind, pos: Vec3<f32>, vol: f32, time: f64) -> Self {
Sound {
kind,
pos,
vol,
time,
}
}
#[must_use]
pub fn with_new_vol(mut self, new_vol: f32) -> Self {
self.vol = new_vol;
self
}
}
#[derive(Copy, Clone, Debug)]
pub enum SoundKind {
Unknown,
Utterance(UtteranceKind, Body),
Movement,
Melee,
Projectile,
Explosion,
Beam,
Shockwave,
Mine,
Trap,
}
#[derive(Clone, Copy, Debug)]
pub struct Target {
pub target: EcsEntity,
pub hostile: bool,
pub selected_at: f64,
pub aggro_on: bool,
pub last_known_pos: Option<Vec3<f32>>,
}
impl Target {
pub fn new(
target: EcsEntity,
hostile: bool,
selected_at: f64,
aggro_on: bool,
last_known_pos: Option<Vec3<f32>>,
) -> Self {
Self {
target,
hostile,
selected_at,
aggro_on,
last_known_pos,
}
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, EnumIter)]
pub enum TimerAction {
Interact,
}
#[derive(Clone, Debug)]
pub struct Timer {
action_starts: Vec<Option<f64>>,
last_action: Option<TimerAction>,
}
impl Default for Timer {
fn default() -> Self {
Self {
action_starts: TimerAction::iter().map(|_| None).collect(),
last_action: None,
}
}
}
impl Timer {
fn idx_for(action: TimerAction) -> usize {
TimerAction::iter()
.enumerate()
.find(|(_, a)| a == &action)
.unwrap()
.0 }
pub fn reset(&mut self, action: TimerAction) -> bool {
self.action_starts[Self::idx_for(action)].take().is_some()
}
pub fn start(&mut self, time: f64, action: TimerAction) {
self.action_starts[Self::idx_for(action)] = Some(time);
self.last_action = Some(action);
}
pub fn progress(&mut self, time: f64, action: TimerAction) {
if self.last_action != Some(action) {
self.start(time, action);
}
}
pub fn time_of_last(&self, action: TimerAction) -> Option<f64> {
self.action_starts[Self::idx_for(action)]
}
pub fn time_since_exceeds(&self, time: f64, action: TimerAction, timeout: f64) -> bool {
self.time_of_last(action)
.map_or(true, |last_time| (time - last_time).max(0.0) > timeout)
}
pub fn timeout_elapsed(
&mut self,
time: f64,
action: TimerAction,
timeout: f64,
) -> Option<bool> {
if self.time_since_exceeds(time, action, timeout) {
Some(self.reset(action))
} else {
self.progress(time, action);
None
}
}
}
#[derive(Clone, Debug)]
pub struct Agent {
pub rtsim_controller: RtSimController,
pub patrol_origin: Option<Vec3<f32>>,
pub target: Option<Target>,
pub chaser: Chaser,
pub behavior: Behavior,
pub psyche: Psyche,
pub inbox: VecDeque<AgentEvent>,
pub combat_state: ActionState,
pub behavior_state: ActionState,
pub timer: Timer,
pub bearing: Vec2<f32>,
pub sounds_heard: Vec<Sound>,
pub position_pid_controller: Option<PidController<fn(Vec3<f32>, Vec3<f32>) -> f32, 16>>,
pub flee_from_pos: Option<Pos>,
pub awareness: Awareness,
pub stay_pos: Option<Pos>,
pub rtsim_outbox: Option<VecDeque<NpcInput>>,
}
#[derive(Clone, Debug)]
pub struct Awareness {
level: f32,
reached: bool,
}
impl fmt::Display for Awareness {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!(f, "{:.2}", self.level) }
}
impl Awareness {
const ALERT: f32 = 1.0;
const HIGH: f32 = 0.6;
const LOW: f32 = 0.1;
const MEDIUM: f32 = 0.3;
const UNAWARE: f32 = 0.0;
pub fn new(level: f32) -> Self {
Self {
level: level.clamp(Self::UNAWARE, Self::ALERT),
reached: false,
}
}
pub fn level(&self) -> f32 { self.level }
pub fn state(&self) -> AwarenessState {
if self.level == Self::ALERT {
AwarenessState::Alert
} else if self.level.is_between(Self::HIGH, Self::ALERT) {
AwarenessState::High
} else if self.level.is_between(Self::MEDIUM, Self::HIGH) {
AwarenessState::Medium
} else if self.level.is_between(Self::LOW, Self::MEDIUM) {
AwarenessState::Low
} else {
AwarenessState::Unaware
}
}
pub fn reached(&self) -> bool { self.reached }
pub fn change_by(&mut self, amount: f32) {
self.level = (self.level + amount).clamp(Self::UNAWARE, Self::ALERT);
if self.state() == AwarenessState::Alert {
self.reached = true;
} else if self.state() == AwarenessState::Unaware {
self.reached = false;
}
}
pub fn set_maximally_aware(&mut self) {
self.reached = true;
self.level = Self::ALERT;
}
}
#[derive(Clone, Debug, PartialOrd, PartialEq, Eq)]
pub enum AwarenessState {
Unaware = 0,
Low = 1,
Medium = 2,
High = 3,
Alert = 4,
}
#[derive(Clone, Debug, Default)]
pub struct ActionState {
pub timers: [f32; ACTIONSTATE_NUMBER_OF_CONCURRENT_TIMERS],
pub counters: [f32; ACTIONSTATE_NUMBER_OF_CONCURRENT_COUNTERS],
pub conditions: [bool; ACTIONSTATE_NUMBER_OF_CONCURRENT_CONDITIONS],
pub int_counters: [u8; ACTIONSTATE_NUMBER_OF_CONCURRENT_INT_COUNTERS],
pub positions: [Option<Vec3<f32>>; ACTIONSTATE_NUMBER_OF_CONCURRENT_POSITIONS],
pub initialized: bool,
}
impl Agent {
pub fn from_body(body: &Body) -> Self {
Agent {
rtsim_controller: RtSimController::default(),
patrol_origin: None,
target: None,
chaser: Chaser::default(),
behavior: Behavior::default(),
psyche: Psyche::from(body),
inbox: VecDeque::new(),
combat_state: ActionState::default(),
behavior_state: ActionState::default(),
timer: Timer::default(),
bearing: Vec2::zero(),
sounds_heard: Vec::new(),
position_pid_controller: None,
flee_from_pos: None,
stay_pos: None,
awareness: Awareness::new(0.0),
rtsim_outbox: None,
}
}
#[must_use]
pub fn with_patrol_origin(mut self, origin: Vec3<f32>) -> Self {
self.patrol_origin = Some(origin);
self
}
#[must_use]
pub fn with_behavior(mut self, behavior: Behavior) -> Self {
self.behavior = behavior;
self
}
#[must_use]
pub fn with_no_flee_if(mut self, condition: bool) -> Self {
if condition {
self.psyche.flee_health = 0.0;
}
self
}
pub fn set_no_flee(&mut self) { self.psyche.flee_health = 0.0; }
#[must_use]
pub fn with_destination(mut self, pos: Vec3<f32>) -> Self {
self.psyche.flee_health = 0.0;
self.rtsim_controller = RtSimController::with_destination(pos);
self.behavior.allow(BehaviorCapability::SPEAK);
self
}
#[must_use]
pub fn with_idle_wander_factor(mut self, idle_wander_factor: f32) -> Self {
self.psyche.idle_wander_factor = idle_wander_factor;
self
}
pub fn with_aggro_range_multiplier(mut self, aggro_range_multiplier: f32) -> Self {
self.psyche.aggro_range_multiplier = aggro_range_multiplier;
self
}
#[must_use]
pub fn with_position_pid_controller(
mut self,
pid: PidController<fn(Vec3<f32>, Vec3<f32>) -> f32, 16>,
) -> Self {
self.position_pid_controller = Some(pid);
self
}
#[must_use]
pub fn with_aggro_no_warn(mut self) -> Self {
self.psyche.aggro_dist = None;
self
}
pub fn forget_old_sounds(&mut self, time: f64) {
if !self.sounds_heard.is_empty() {
self.sounds_heard
.retain(|&sound| time - sound.time <= SECONDS_BEFORE_FORGET_SOUNDS);
}
}
pub fn allowed_to_speak(&self) -> bool { self.behavior.can(BehaviorCapability::SPEAK) }
}
impl Component for Agent {
type Storage = specs::DenseVecStorage<Self>;
}
#[cfg(test)]
mod tests {
use super::{humanoid, Agent, Behavior, BehaviorCapability, BehaviorState, Body};
#[test]
pub fn behavior_basic() {
let mut b = Behavior::default();
assert!(!b.can(BehaviorCapability::SPEAK));
b.allow(BehaviorCapability::SPEAK);
assert!(b.can(BehaviorCapability::SPEAK));
b.deny(BehaviorCapability::SPEAK);
assert!(!b.can(BehaviorCapability::SPEAK));
assert!(!b.is(BehaviorState::TRADING));
b.set(BehaviorState::TRADING);
assert!(b.is(BehaviorState::TRADING));
b.unset(BehaviorState::TRADING);
assert!(!b.is(BehaviorState::TRADING));
let b = Behavior::from(BehaviorCapability::SPEAK);
assert!(b.can(BehaviorCapability::SPEAK));
}
#[test]
pub fn enable_flee() {
let body = Body::Humanoid(humanoid::Body::random());
let mut agent = Agent::from_body(&body);
agent.psyche.flee_health = 1.0;
agent = agent.with_no_flee_if(false);
assert_eq!(agent.psyche.flee_health, 1.0);
}
#[test]
pub fn set_no_flee() {
let body = Body::Humanoid(humanoid::Body::random());
let mut agent = Agent::from_body(&body);
agent.psyche.flee_health = 1.0;
agent.set_no_flee();
assert_eq!(agent.psyche.flee_health, 0.0);
}
#[test]
pub fn with_aggro_no_warn() {
let body = Body::Humanoid(humanoid::Body::random());
let mut agent = Agent::from_body(&body);
agent.psyche.aggro_dist = Some(1.0);
agent = agent.with_aggro_no_warn();
assert_eq!(agent.psyche.aggro_dist, None);
}
}
#[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,
integral_error: f64,
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, sp); NUM_SAMPLES],
pv_idx: 0,
integral_error: 0.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);
self.update_integral_err();
}
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) }
pub fn integral_err(&self) -> f32 { self.integral_error as f32 }
fn update_integral_err(&mut self) {
let f = |x| (self.e)(self.sp, x) as f64;
let (a, x0) = self.pv_samples[(self.pv_idx + NUM_SAMPLES - 1) % NUM_SAMPLES];
let (b, x1) = self.pv_samples[self.pv_idx];
let dx = b - a;
if dx < 5.0 {
self.integral_error += dx * (f(x1) + f(x0)) / 2.0;
}
}
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
}
}
pub fn pid_coefficients(body: &Body) -> Option<(f32, f32, f32)> {
match body {
Body::Ship(ship::Body::DefaultAirship) => {
let kp = 1.0;
let ki = 0.1;
let kd = 1.2;
Some((kp, ki, kd))
},
Body::Ship(ship::Body::AirBalloon) => {
let kp = 1.0;
let ki = 0.1;
let kd = 0.8;
Some((kp, ki, kd))
},
_ => None,
}
}