use core::marker::PhantomData;
use glam::DMat3;
use astrodyn_dynamics::body_init::init_from_orbital_elements_typed;
use astrodyn_dynamics::state::TranslationalStateTyped;
use astrodyn_dynamics::{
MassProperties, MassPropertiesTyped, RotationalState, RotationalStateTyped,
};
use crate::integrator::{GaussJacksonConfig, IntegratorType};
use astrodyn_gravity::{GravityControl, GravityControls};
use astrodyn_interactions::DragConfig;
use astrodyn_math::{EulerSequence, OrbitalElements};
use astrodyn_quantities::aliases::{AngularVelocity, InertiaTensor, Position};
use astrodyn_quantities::body_attitude::BodyAttitude;
use astrodyn_quantities::dims::GravParam;
use astrodyn_quantities::frame::{BodyFrame, RootInertial, StructuralFrame};
use uom::si::f64::{Angle, Length, Mass};
use uom::si::mass::kilogram;
use astrodyn_quantities::frame::SelfRef;
use crate::interactions::FlatPlateState;
use crate::planet_config::PlanetConfig;
use crate::source_handle::SourceHandle;
use crate::vehicle_config::{
DerivedStateConfig, EarthLightingConfig, FrameSwitchConfig, GeodeticConfig, ShadowBody,
SrpModel, VehicleConfig,
};
mod sealed {
pub trait Sealed {}
}
#[diagnostic::on_unimplemented(
message = "`{Self}` is not a valid `VehicleBuilder` state. Use \
`NeedsState`, `NeedsMass`, `HasIntegrator`, or `Ready`.",
label = "not a `BuildState`"
)]
pub trait BuildState: sealed::Sealed {}
pub struct NeedsState;
pub struct NeedsMass;
pub struct HasIntegrator;
pub struct Ready;
impl sealed::Sealed for NeedsState {}
impl sealed::Sealed for NeedsMass {}
impl sealed::Sealed for HasIntegrator {}
impl sealed::Sealed for Ready {}
impl BuildState for NeedsState {}
impl BuildState for NeedsMass {}
impl BuildState for HasIntegrator {}
impl BuildState for Ready {}
pub struct VehicleBuilder<S: BuildState = NeedsState> {
trans: Option<TranslationalStateTyped<RootInertial>>,
rot: Option<RotationalStateTyped<SelfRef>>,
mass: Option<MassPropertiesTyped<SelfRef>>,
integrator: Option<IntegratorType>,
t_struct_body: DMat3,
gravity_controls: GravityControls<usize>,
compute_gravity_gradient: bool,
drag: Option<DragConfig>,
srp: Option<SrpModel>,
shadow_body: Option<ShadowBody>,
derived: DerivedStateConfig,
external_force: astrodyn_quantities::aliases::Force<RootInertial>,
external_torque: astrodyn_quantities::aliases::Torque<
astrodyn_quantities::frame::BodyFrame<astrodyn_quantities::frame::SelfRef>,
>,
integ_source: Option<usize>,
frame_switches: Vec<FrameSwitchConfig>,
_state: PhantomData<S>,
}
impl Default for VehicleBuilder<NeedsState> {
fn default() -> Self {
Self::new()
}
}
impl<S: BuildState> VehicleBuilder<S> {
fn empty() -> VehicleBuilder<NeedsState> {
VehicleBuilder {
trans: None,
rot: None,
mass: None,
integrator: None,
t_struct_body: DMat3::IDENTITY,
gravity_controls: GravityControls::default(),
compute_gravity_gradient: false,
drag: None,
srp: None,
shadow_body: None,
derived: DerivedStateConfig::default(),
external_force: astrodyn_quantities::aliases::Force::<RootInertial>::zero(),
external_torque: astrodyn_quantities::aliases::Torque::<
astrodyn_quantities::frame::BodyFrame<astrodyn_quantities::frame::SelfRef>,
>::zero(),
integ_source: None,
frame_switches: Vec::new(),
_state: PhantomData,
}
}
fn transition<T: BuildState>(self) -> VehicleBuilder<T> {
VehicleBuilder {
trans: self.trans,
rot: self.rot,
mass: self.mass,
integrator: self.integrator,
t_struct_body: self.t_struct_body,
gravity_controls: self.gravity_controls,
compute_gravity_gradient: self.compute_gravity_gradient,
drag: self.drag,
srp: self.srp,
shadow_body: self.shadow_body,
derived: self.derived,
external_force: self.external_force,
external_torque: self.external_torque,
integ_source: self.integ_source,
frame_switches: self.frame_switches,
_state: PhantomData,
}
}
}
impl VehicleBuilder<NeedsState> {
pub fn new() -> Self {
Self::empty()
}
pub fn with_translational(
mut self,
s: TranslationalStateTyped<RootInertial>,
) -> VehicleBuilder<NeedsMass> {
self.trans = Some(s);
self.transition()
}
pub fn from_orbital_elements<P: astrodyn_quantities::frame::Planet>(
self,
oe: OrbitalElements<P>,
mu: GravParam<P>,
) -> VehicleBuilder<NeedsMass> {
use uom::si::angle::radian;
use uom::si::length::meter;
let trans = init_from_orbital_elements_typed(
Length::new::<meter>(oe.semi_major_axis),
oe.e_mag,
Angle::new::<radian>(oe.inclination),
Angle::new::<radian>(oe.long_asc_node),
Angle::new::<radian>(oe.arg_periapsis),
Angle::new::<radian>(oe.true_anom),
mu,
);
self.with_translational(trans)
}
}
impl VehicleBuilder<NeedsMass> {
pub fn three_dof_point_mass(mut self, mass: Mass) -> VehicleBuilder<HasIntegrator> {
self.mass = Some(MassPropertiesTyped::<SelfRef>::new(mass));
self.transition()
}
pub fn sixdof(
mut self,
rot: RotationalState,
mass: MassProperties,
) -> VehicleBuilder<HasIntegrator> {
self.rot = Some(RotationalStateTyped::<SelfRef>::new(
BodyAttitude::from_jeod_quat(rot.quaternion),
AngularVelocity::<BodyFrame<SelfRef>>::from_raw_si(rot.ang_vel_body), ));
let typed_mass = MassPropertiesTyped::<SelfRef>::with_inertia(
Mass::new::<kilogram>(mass.mass),
InertiaTensor::<BodyFrame<SelfRef>>::from_dmat3_unchecked(mass.inertia), Position::<StructuralFrame<SelfRef>>::from_raw_si(mass.position), )
.with_t_parent_this(mass.t_parent_this);
self.mass = Some(typed_mass);
self.transition()
}
}
impl VehicleBuilder<HasIntegrator> {
pub fn rk4(self) -> VehicleBuilder<Ready> {
self.with_integrator(IntegratorType::Rk4)
}
pub fn rkf45(self) -> VehicleBuilder<Ready> {
self.with_integrator(IntegratorType::Rkf45)
}
pub fn gauss_jackson(self, cfg: GaussJacksonConfig) -> VehicleBuilder<Ready> {
self.with_integrator(IntegratorType::GaussJackson(cfg))
}
pub fn with_integrator(mut self, integrator: IntegratorType) -> VehicleBuilder<Ready> {
self.integrator = Some(integrator);
self.transition()
}
}
impl VehicleBuilder<Ready> {
pub fn gravity(mut self, control: GravityControl<usize>) -> Self {
self.gravity_controls.controls.push(control);
self
}
pub fn gravity_gradient(mut self) -> Self {
self.compute_gravity_gradient = true;
self
}
pub fn drag(mut self, cfg: DragConfig) -> Self {
self.drag = Some(cfg);
self
}
pub fn flat_plate_srp(
mut self,
state: FlatPlateState<astrodyn_quantities::frame::SelfRef>,
) -> Self {
self.srp = Some(SrpModel::FlatPlate(state));
self
}
pub fn cannonball_srp(mut self, cx_area: f64, albedo: f64, diffuse: f64) -> Self {
self.srp = Some(SrpModel::Cannonball {
cx_area,
albedo,
diffuse,
});
self
}
pub fn shadow(mut self, source: impl Into<SourceHandle>, planet: &PlanetConfig) -> Self {
self.shadow_body = Some(ShadowBody {
source_idx: source.into().into_raw(),
radius: planet.shadow_radius,
});
self
}
pub fn shadow_with_radius(mut self, source: impl Into<SourceHandle>, radius: f64) -> Self {
self.shadow_body = Some(ShadowBody {
source_idx: source.into().into_raw(),
radius,
});
self
}
pub fn structural_transform(mut self, t: DMat3) -> Self {
self.t_struct_body = t;
self
}
pub fn external_force(mut self, f: astrodyn_quantities::aliases::Force<RootInertial>) -> Self {
self.external_force = f;
self
}
pub fn external_torque(
mut self,
t: astrodyn_quantities::aliases::Torque<
astrodyn_quantities::frame::BodyFrame<astrodyn_quantities::frame::SelfRef>,
>,
) -> Self {
self.external_torque = t;
self
}
pub fn integ_source(mut self, source: impl Into<SourceHandle>) -> Self {
self.integ_source = Some(source.into().into_raw());
self
}
pub fn frame_switches(mut self, switches: Vec<FrameSwitchConfig>) -> Self {
self.frame_switches = switches;
self
}
pub fn orbital_elements(mut self, source: impl Into<SourceHandle>) -> Self {
self.derived.orbital_elements_source = Some(source.into().into_raw());
self
}
pub fn euler_angles(mut self, sequence: EulerSequence) -> Self {
self.derived.euler_sequence = Some(sequence);
self
}
pub fn lvlh(mut self) -> Self {
self.derived.lvlh = true;
self
}
pub fn geodetic(mut self, source: impl Into<SourceHandle>, planet: &PlanetConfig) -> Self {
self.derived.geodetic = Some(GeodeticConfig {
source_idx: source.into().into_raw(),
r_eq: planet.shape.r_eq,
r_pol: planet.shape.r_pol,
});
self
}
pub fn solar_beta(mut self) -> Self {
self.derived.solar_beta = true;
self
}
pub fn earth_lighting(
mut self,
earth: &PlanetConfig,
moon: &PlanetConfig,
sun: &PlanetConfig,
) -> Self {
self.derived.earth_lighting = Some(EarthLightingConfig {
earth_radius: earth.shape.r_eq,
moon_radius: moon.shape.r_eq,
sun_radius: sun.shape.r_eq,
});
self
}
pub fn build(self) -> VehicleConfig {
VehicleConfig {
trans: self
.trans
.expect("typestate guarantees translational state"),
rot: self.rot,
mass: Some(self.mass.expect("typestate guarantees mass")),
integrator: self.integrator.expect("typestate guarantees integrator"),
t_struct_body: self.t_struct_body,
gravity_controls: self.gravity_controls,
compute_gravity_gradient: self.compute_gravity_gradient,
drag: self.drag,
srp: self.srp,
shadow_body: self.shadow_body,
derived: self.derived,
external_force: self.external_force,
external_torque: self.external_torque,
integ_source: self.integ_source,
frame_switches: self.frame_switches,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use astrodyn_quantities::ext::F64Ext;
use glam::DVec3;
fn iss_trans() -> TranslationalStateTyped<RootInertial> {
use astrodyn_quantities::aliases::{Position, Velocity};
TranslationalStateTyped::<RootInertial> {
position: Position::<RootInertial>::from_raw_si(DVec3::new(7_000_000.0, 0.0, 0.0)),
velocity: Velocity::<RootInertial>::from_raw_si(DVec3::new(0.0, 7_500.0, 0.0)),
}
}
#[test]
fn three_dof_happy_path() {
let cfg = VehicleBuilder::new()
.with_translational(iss_trans())
.three_dof_point_mass(420_000.0.kg())
.rk4()
.build();
assert_eq!(cfg.integrator, IntegratorType::Rk4);
assert_eq!(
cfg.mass
.expect("mass set by typestate")
.mass
.get::<uom::si::mass::kilogram>(),
420_000.0
);
assert!(cfg.rot.is_none());
}
#[test]
fn six_dof_happy_path() {
use astrodyn_math::JeodQuat;
let rot = RotationalState {
quaternion: JeodQuat::identity(),
ang_vel_body: DVec3::ZERO,
};
let inertia = DMat3::IDENTITY * 100.0;
let mass = MassProperties::with_inertia(420_000.0, inertia, DVec3::ZERO);
let cfg = VehicleBuilder::new()
.with_translational(iss_trans())
.sixdof(rot, mass)
.rk4()
.build();
assert!(cfg.rot.is_some());
assert_eq!(
cfg.mass
.expect("mass set by typestate")
.mass
.get::<uom::si::mass::kilogram>(),
420_000.0
);
}
#[test]
fn ready_state_full_surface() {
use astrodyn_interactions::DragConfig;
use astrodyn_quantities::aliases::{Position, Velocity};
let cfg = VehicleBuilder::new()
.with_translational(TranslationalStateTyped::<RootInertial> {
position: Position::<RootInertial>::from_raw_si(DVec3::new(7_000_000.0, 0.0, 0.0)),
velocity: Velocity::<RootInertial>::from_raw_si(DVec3::new(0.0, 7_500.0, 0.0)),
})
.three_dof_point_mass(1_000.0.kg())
.rk4()
.gravity_gradient()
.drag(DragConfig {
cd: 2.2,
area: 1.0,
constant_density: None,
})
.lvlh()
.solar_beta()
.build();
assert!(cfg.compute_gravity_gradient);
assert!(cfg.drag.is_some());
assert!(cfg.derived.lvlh);
assert!(cfg.derived.solar_beta);
}
#[test]
fn builder_accepts_source_handle_and_bare_usize_interchangeably() {
let typed = VehicleBuilder::new()
.with_translational(iss_trans())
.three_dof_point_mass(420_000.0.kg())
.rk4()
.integ_source(SourceHandle::central())
.orbital_elements(SourceHandle::central())
.shadow(SourceHandle::index(2), &crate::EARTH)
.geodetic(SourceHandle::central(), &crate::EARTH)
.build();
let untyped = VehicleBuilder::new()
.with_translational(iss_trans())
.three_dof_point_mass(420_000.0.kg())
.rk4()
.integ_source(0_usize)
.orbital_elements(0_usize)
.shadow(2_usize, &crate::EARTH)
.geodetic(0_usize, &crate::EARTH)
.build();
assert_eq!(typed.integ_source, untyped.integ_source);
assert_eq!(typed.integ_source, Some(0));
assert_eq!(
typed.derived.orbital_elements_source,
untyped.derived.orbital_elements_source
);
assert_eq!(typed.derived.orbital_elements_source, Some(0));
assert_eq!(
typed.shadow_body.expect("shadow set above").source_idx,
untyped.shadow_body.expect("shadow set above").source_idx
);
assert_eq!(typed.shadow_body.expect("shadow set above").source_idx, 2);
assert_eq!(
typed
.derived
.geodetic
.expect("geodetic set above")
.source_idx,
untyped
.derived
.geodetic
.expect("geodetic set above")
.source_idx
);
}
}