use crate::SimTime;
use serde::{Deserialize, Serialize};
#[derive(Clone, Copy, Debug, Default, Deserialize, PartialEq, Serialize)]
pub struct Position3D {
pub x_m: f64,
pub y_m: f64,
pub z_m: f64,
}
impl Position3D {
pub const ORIGIN: Self = Self {
x_m: 0.0,
y_m: 0.0,
z_m: 0.0,
};
#[must_use]
pub const fn new(x_m: f64, y_m: f64, z_m: f64) -> Self {
Self { x_m, y_m, z_m }
}
#[must_use]
pub fn distance_to(self, other: Self) -> f64 {
(self.x_m - other.x_m)
.hypot(self.y_m - other.y_m)
.hypot(self.z_m - other.z_m)
}
pub(crate) fn is_finite(self) -> bool {
self.x_m.is_finite() && self.y_m.is_finite() && self.z_m.is_finite()
}
}
#[derive(Clone, Copy, Debug, Default, Deserialize, PartialEq, Serialize)]
pub struct Velocity3D {
pub x_mps: f64,
pub y_mps: f64,
pub z_mps: f64,
}
impl Velocity3D {
#[must_use]
pub const fn new(x_mps: f64, y_mps: f64, z_mps: f64) -> Self {
Self {
x_mps,
y_mps,
z_mps,
}
}
pub(crate) fn is_finite(self) -> bool {
self.x_mps.is_finite() && self.y_mps.is_finite() && self.z_mps.is_finite()
}
}
#[derive(Clone, Copy, Debug, Deserialize, PartialEq, Serialize)]
pub struct Waypoint {
pub at_ns: u64,
pub position: Position3D,
}
#[derive(Clone, Debug, Deserialize, PartialEq, Serialize)]
#[serde(tag = "kind", rename_all = "snake_case")]
pub enum MobilityModel {
Static {
#[serde(default)]
position: Position3D,
},
Linear {
position_at_epoch: Position3D,
velocity_mps: Velocity3D,
#[serde(default)]
epoch_ns: u64,
},
Waypoints {
waypoints: Vec<Waypoint>,
},
}
impl Default for MobilityModel {
fn default() -> Self {
Self::Static {
position: Position3D::ORIGIN,
}
}
}
impl MobilityModel {
#[must_use]
pub fn position_at(&self, time: SimTime) -> Position3D {
match self {
Self::Static { position } => *position,
Self::Linear {
position_at_epoch,
velocity_mps,
epoch_ns,
} => {
let elapsed_seconds = (time.as_nanos() as f64 - *epoch_ns as f64) / 1_000_000_000.0;
Position3D {
x_m: position_at_epoch.x_m + velocity_mps.x_mps * elapsed_seconds,
y_m: position_at_epoch.y_m + velocity_mps.y_mps * elapsed_seconds,
z_m: position_at_epoch.z_m + velocity_mps.z_mps * elapsed_seconds,
}
}
Self::Waypoints { waypoints } => interpolate_waypoints(waypoints, time.as_nanos()),
}
}
}
fn interpolate_waypoints(waypoints: &[Waypoint], time_ns: u64) -> Position3D {
let Some(first) = waypoints.first() else {
return Position3D::ORIGIN;
};
if time_ns <= first.at_ns {
return first.position;
}
let Some(last) = waypoints.last() else {
return first.position;
};
if time_ns >= last.at_ns {
return last.position;
}
let upper_index = waypoints.partition_point(|waypoint| waypoint.at_ns <= time_ns);
let lower = waypoints[upper_index - 1];
let upper = waypoints[upper_index];
let fraction = (time_ns - lower.at_ns) as f64 / (upper.at_ns - lower.at_ns) as f64;
Position3D {
x_m: lower.position.x_m + (upper.position.x_m - lower.position.x_m) * fraction,
y_m: lower.position.y_m + (upper.position.y_m - lower.position.y_m) * fraction,
z_m: lower.position.z_m + (upper.position.z_m - lower.position.z_m) * fraction,
}
}