use core::ops::Deref;
use crate::flags::impl_flags;
use core::{error, fmt};
use libm::{fabsf, hypotf};
use super::{Trajectory, TrajectoryPlayer, TrajectoryPlayerState, TrajectorySegment};
use crate::basic_types::Vector3WithYaw;
use crate::motion::travel_time_for_distance;
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
pub struct TrajectoryStatComponents(u8);
impl TrajectoryStatComponents {
pub const NONE: Self = Self(0);
pub const DURATION: Self = Self(1);
pub const START_END_DISTANCE: Self = Self(2);
pub const TAKEOFF_TIME: Self = Self(4);
pub const LANDING_TIME: Self = Self(8);
pub const ALL: Self = Self(15);
}
impl_flags!(TrajectoryStatComponents);
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct TrajectoryStats {
pub initial_pos: Option<Vector3WithYaw>,
pub duration_msec: Option<u32>,
pub takeoff: Option<TakeoffStats>,
pub start_to_end_distance_xy: Option<f32>,
pub landing: Option<LandingStats>,
}
impl TrajectoryStats {
#[must_use]
pub fn duration_sec(&self) -> Option<f32> {
self.duration_msec.map(super::msec_to_sec)
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TakeoffStats {
pub earliest_above_sec: f32,
pub takeoff_time_sec: f32,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct LandingStats {
pub landing_time_sec: f32,
pub pos_at_landing_time: Vector3WithYaw,
pub vel_at_landing_time: Vector3WithYaw,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum TrajectoryStatsError {
InvalidAcceleration,
InvalidTakeoffSpeed,
InvalidMinAscent,
InvalidPreferredDescent,
}
impl fmt::Display for TrajectoryStatsError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.write_str(match self {
Self::InvalidAcceleration => "the acceleration is not positive",
Self::InvalidTakeoffSpeed => "the takeoff speed is not positive or not finite",
Self::InvalidMinAscent => "the minimum ascent is negative or not finite",
Self::InvalidPreferredDescent => "the preferred descent is negative or not finite",
})
}
}
impl error::Error for TrajectoryStatsError {}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TrajectoryStatsCalculator {
pub components: TrajectoryStatComponents,
pub takeoff_speed: f32,
pub acceleration: f32,
pub min_ascent: f32,
pub preferred_descent: f32,
pub verticality_threshold: f32,
}
impl TrajectoryStatsCalculator {
#[must_use]
pub fn new(scale: f32) -> Self {
Self {
components: TrajectoryStatComponents::ALL,
takeoff_speed: 2.0 * scale,
acceleration: 4.0 * scale,
min_ascent: 2.5 * scale,
preferred_descent: 2.5 * scale,
verticality_threshold: scale * 0.05,
}
}
pub const fn set_components(&mut self, components: TrajectoryStatComponents) {
self.components = components;
}
pub fn run(
&self,
trajectory: &Trajectory<'_>,
) -> Result<TrajectoryStats, TrajectoryStatsError> {
self.validate()?;
let mut result = TrajectoryStats::default();
let mut components = self.components;
if components.is_empty() {
return Ok(result);
}
let mut player = TrajectoryPlayer::new(trajectory);
let start = player.position_at(0.0);
result.initial_pos = Some(start);
let takeoff_altitude = start.z + self.min_ascent;
let mut duration_msec: u32 = 0;
let mut earliest_above_sec = f32::INFINITY;
let mut landing = None;
let mut vertical: Option<VerticalRun> = None;
while player.has_more_segments() {
if components.contains(TrajectoryStatComponents::DURATION) {
let segment = player.current_segment();
duration_msec = duration_msec.saturating_add(segment.duration_msec());
}
if components.contains(TrajectoryStatComponents::TAKEOFF_TIME) {
let segment = player.current_segment();
if let Some(rel_t) = segment.poly().z.touches(takeoff_altitude) {
earliest_above_sec = segment.start_time_sec() + rel_t * segment.duration_sec();
components.remove(TrajectoryStatComponents::TAKEOFF_TIME);
if components.is_empty() {
break;
}
}
}
if components.contains(TrajectoryStatComponents::LANDING_TIME) {
let descending = is_segment_descending_vertically(
player.current_segment(),
self.verticality_threshold,
);
if descending {
let end_altitude = player.current_segment().end().z;
let run = vertical.get_or_insert_with(|| {
let state = player.save_state();
VerticalRun {
start_altitude: state.segment().start().z,
end_altitude,
state,
}
});
run.end_altitude = end_altitude;
} else {
landing = Some(landing_at_end(player.current_segment()));
vertical = None;
}
}
player.build_next_segment();
}
if self.components.contains(TrajectoryStatComponents::DURATION) {
result.duration_msec = Some(duration_msec);
}
if self
.components
.contains(TrajectoryStatComponents::TAKEOFF_TIME)
{
let adjustment =
travel_time_for_distance(self.min_ascent, self.takeoff_speed, self.acceleration);
let takeoff_time_sec = if earliest_above_sec.is_finite() && adjustment.is_finite() {
earliest_above_sec - adjustment
} else {
f32::INFINITY
};
result.takeoff = Some(TakeoffStats {
earliest_above_sec,
takeoff_time_sec,
});
}
if self
.components
.contains(TrajectoryStatComponents::START_END_DISTANCE)
{
let end = player.position_at(f32::INFINITY);
result.start_to_end_distance_xy = Some(hypotf(end.x - start.x, end.y - start.y));
}
if self
.components
.contains(TrajectoryStatComponents::LANDING_TIME)
{
result.landing = self.landing(&mut player, landing, vertical);
}
Ok(result)
}
fn validate(&self) -> Result<(), TrajectoryStatsError> {
if self.acceleration <= 0.0 {
Err(TrajectoryStatsError::InvalidAcceleration)
} else if !self.takeoff_speed.is_finite() || self.takeoff_speed <= 0.0 {
Err(TrajectoryStatsError::InvalidTakeoffSpeed)
} else if !self.min_ascent.is_finite() || self.min_ascent < 0.0 {
Err(TrajectoryStatsError::InvalidMinAscent)
} else if !self.preferred_descent.is_finite() || self.preferred_descent < 0.0 {
Err(TrajectoryStatsError::InvalidPreferredDescent)
} else {
Ok(())
}
}
fn landing<'a, T: Deref<Target = Trajectory<'a>>>(
&self,
player: &mut TrajectoryPlayer<T>,
mut landing: Option<LandingStats>,
vertical: Option<VerticalRun>,
) -> Option<LandingStats> {
if let Some(run) = vertical {
player.restore_state(&run.state);
let mut to_descend = run.start_altitude - (run.end_altitude + self.preferred_descent);
landing = None;
if to_descend > 0.0 {
let mut altitude = player.current_segment().start().z;
while player.has_more_segments() {
let segment = player.current_segment();
let delta = altitude - segment.end().z;
if delta < 0.0 {
return None;
} else if delta <= to_descend {
to_descend -= delta;
altitude = segment.end().z;
landing = Some(landing_at_end(segment));
} else {
let poly = *segment.poly();
let rel_t = poly.z.touches(altitude - to_descend).unwrap_or(0.0);
landing = Some(LandingStats {
landing_time_sec: segment.start_time_sec()
+ rel_t * segment.duration_sec(),
pos_at_landing_time: poly.eval(rel_t),
vel_at_landing_time: segment.dpoly().eval(rel_t),
});
break;
}
player.build_next_segment();
}
}
}
landing.or_else(|| {
let segment = player.current_segment();
Some(LandingStats {
landing_time_sec: segment.start_time_sec(),
pos_at_landing_time: segment.start(),
vel_at_landing_time: segment.dpoly().eval(0.0),
})
})
}
}
impl Default for TrajectoryStatsCalculator {
fn default() -> Self {
Self::new(1.0)
}
}
struct VerticalRun {
state: TrajectoryPlayerState,
start_altitude: f32,
end_altitude: f32,
}
fn landing_at_end(segment: &TrajectorySegment) -> LandingStats {
LandingStats {
landing_time_sec: segment.end_time_sec(),
pos_at_landing_time: segment.end(),
vel_at_landing_time: segment.dpoly().eval(1.0),
}
}
fn is_segment_descending_vertically(segment: &TrajectorySegment, threshold: f32) -> bool {
let (start, end) = (segment.start(), segment.end());
if start.z < end.z {
return false;
}
let dx = fabsf(start.x - end.x);
let dy = fabsf(start.y - end.y);
let dz = start.z - end.z;
dx <= threshold && dy <= threshold && dz >= dx && dz >= dy
}
impl Trajectory<'_> {
#[must_use]
pub fn propose_takeoff_time_sec(&self, min_ascent: f32, speed: f32, acceleration: f32) -> f32 {
let calc = TrajectoryStatsCalculator {
components: TrajectoryStatComponents::TAKEOFF_TIME,
acceleration,
takeoff_speed: speed,
min_ascent,
..TrajectoryStatsCalculator::new(1.0)
};
calc.run(self)
.ok()
.and_then(|stats| stats.takeoff)
.map_or(f32::INFINITY, |takeoff| takeoff.takeoff_time_sec)
}
#[must_use]
pub fn propose_landing_time_sec(
&self,
preferred_descent: f32,
verticality_threshold: f32,
) -> f32 {
if !verticality_threshold.is_finite()
|| !preferred_descent.is_finite()
|| preferred_descent <= f32::MIN_POSITIVE
{
return self.total_duration_sec();
}
let calc = TrajectoryStatsCalculator {
components: TrajectoryStatComponents::LANDING_TIME,
preferred_descent,
verticality_threshold: verticality_threshold.max(0.0),
..TrajectoryStatsCalculator::new(1.0)
};
calc.run(self)
.ok()
.and_then(|stats| stats.landing)
.map_or(f32::INFINITY, |landing| landing.landing_time_sec)
}
}
#[cfg(test)]
mod tests {
use alloc::vec;
use super::*;
fn short_sideways_descent() -> Trajectory<'static> {
let mut bytes = vec![1];
for coordinate in [0_i16, 0, 3000, 0] {
bytes.extend(coordinate.to_le_bytes());
}
bytes.push(0x11);
bytes.extend(1000_u16.to_le_bytes());
bytes.extend(40_i16.to_le_bytes());
bytes.extend(2970_i16.to_le_bytes());
Trajectory::from_vec(bytes).unwrap()
}
#[test]
fn segments_that_move_more_sideways_than_down_are_not_vertical() {
let stats = TrajectoryStatsCalculator::new(1000.0)
.run(&short_sideways_descent())
.unwrap();
let landing = stats.landing.unwrap();
assert_eq!(landing.landing_time_sec, 1.0);
assert_eq!(landing.pos_at_landing_time.x, 40.0);
}
#[test]
fn no_components_calculate_nothing() {
let mut calc = TrajectoryStatsCalculator::new(1000.0);
calc.set_components(TrajectoryStatComponents::NONE);
let stats = calc.run(&short_sideways_descent()).unwrap();
assert_eq!(stats, TrajectoryStats::default());
}
#[test]
fn invalid_settings_are_rejected() {
let calc = TrajectoryStatsCalculator {
min_ascent: f32::NAN,
..TrajectoryStatsCalculator::new(1000.0)
};
assert_eq!(
calc.run(&short_sideways_descent()),
Err(TrajectoryStatsError::InvalidMinAscent)
);
}
}