use astrodyn_quantities::frame_descriptor::FrameUid;
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::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 \
`NeedsIdentity`, `NeedsState`, `NeedsMass`, `HasIntegrator`, or `Ready`.",
label = "not a `BuildState`"
)]
pub trait BuildState: sealed::Sealed {}
pub struct NeedsIdentity;
pub struct NeedsState;
pub struct NeedsMass;
pub struct HasIntegrator;
pub struct Ready;
impl sealed::Sealed for NeedsIdentity {}
impl sealed::Sealed for NeedsState {}
impl sealed::Sealed for NeedsMass {}
impl sealed::Sealed for HasIntegrator {}
impl sealed::Sealed for Ready {}
impl BuildState for NeedsIdentity {}
impl BuildState for NeedsState {}
impl BuildState for NeedsMass {}
impl BuildState for HasIntegrator {}
impl BuildState for Ready {}
pub struct VehicleBuilder<S: BuildState = NeedsIdentity> {
frame_uid: Option<FrameUid>,
trans: Option<TranslationalStateTyped<RootInertial>>,
rot: Option<RotationalStateTyped<SelfRef>>,
mass: Option<MassPropertiesTyped<SelfRef>>,
integrator: Option<IntegratorType>,
t_struct_body: DMat3,
gravity_controls: GravityControls,
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<FrameUid>,
frame_switches: Vec<FrameSwitchConfig>,
_state: PhantomData<S>,
}
impl Default for VehicleBuilder<NeedsIdentity> {
fn default() -> Self {
Self::new()
}
}
impl<S: BuildState> VehicleBuilder<S> {
fn empty() -> VehicleBuilder<NeedsIdentity> {
VehicleBuilder {
frame_uid: None,
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 {
frame_uid: self.frame_uid,
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<NeedsIdentity> {
pub fn new() -> Self {
Self::empty()
}
pub fn vehicle<V: astrodyn_quantities::frame::Vehicle>(mut self) -> VehicleBuilder<NeedsState> {
self.frame_uid = Some(FrameUid::of::<astrodyn_quantities::frame::BodyFrame<V>>());
self.transition()
}
pub fn vehicle_named(mut self, name: impl Into<String>) -> VehicleBuilder<NeedsState> {
self.frame_uid = Some(crate::named_body_frame_uid(&name.into()));
self.transition()
}
}
impl VehicleBuilder<NeedsState> {
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) -> 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<FrameUid>, planet: &PlanetConfig) -> Self {
self.shadow_body = Some(ShadowBody {
source: source.into(),
radius: planet.shadow_radius,
});
self
}
pub fn shadow_with_radius(mut self, source: impl Into<FrameUid>, radius: f64) -> Self {
self.shadow_body = Some(ShadowBody {
source: source.into(),
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<FrameUid>) -> Self {
self.integ_source = Some(source.into());
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<FrameUid>) -> Self {
self.derived.orbital_elements_source = Some(source.into());
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<FrameUid>, planet: &PlanetConfig) -> Self {
self.derived.geodetic = Some(GeodeticConfig {
source: source.into(),
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 {
frame_uid: self
.frame_uid
.expect("typestate guarantees vehicle identity"),
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)]
#[allow(
clippy::float_cmp,
reason = "VehicleBuilder typestate tests assert bit-exact recovery of literal-built mass values"
)]
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()
.vehicle_named("test-veh-2")
.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()
.vehicle_named("test-veh-3")
.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()
.vehicle_named("test-veh-4")
.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,
..Default::default()
})
.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_source_references_store_identities() {
use astrodyn_quantities::frame::{Earth, Moon, PlanetInertial};
let earth_uid = FrameUid::of::<PlanetInertial<Earth>>();
let moon_uid = FrameUid::of::<PlanetInertial<Moon>>();
let cfg = VehicleBuilder::new()
.vehicle_named("test-veh-5")
.with_translational(iss_trans())
.three_dof_point_mass(420_000.0.kg())
.rk4()
.integ_source(earth_uid.clone())
.orbital_elements(earth_uid.clone())
.shadow(moon_uid.clone(), &crate::EARTH)
.geodetic(earth_uid.clone(), &crate::EARTH)
.build();
assert_eq!(cfg.integ_source, Some(earth_uid.clone()));
assert_eq!(cfg.derived.orbital_elements_source, Some(earth_uid.clone()));
assert_eq!(cfg.shadow_body.expect("shadow set above").source, moon_uid);
assert_eq!(
cfg.derived.geodetic.expect("geodetic set above").source,
earth_uid
);
}
}