use super::ForceModel;
use crate::cosmic::{BaseSpacecraft, Cosm, Frame, SpacecraftExt};
use crate::errors::NyxError;
use crate::linalg::{Matrix3, Vector3};
use std::fmt;
use std::sync::Arc;
#[derive(Clone, Copy, Debug)]
pub enum AtmDensity {
Constant(f64),
Exponential { rho0: f64, r0: f64, ref_alt_m: f64 },
StdAtm { max_alt_m: f64 },
}
#[derive(Clone)]
pub struct ConstantDrag {
pub rho: f64,
pub drag_frame: Frame,
pub cosm: Arc<Cosm>,
}
impl fmt::Display for ConstantDrag {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(
f,
"\tConstant Drag rho = {} kg/m^3 in frame {}",
self.rho, self.drag_frame
)
}
}
impl<X: SpacecraftExt> ForceModel<X> for ConstantDrag {
fn eom(&self, ctx: &BaseSpacecraft<X>) -> Result<Vector3<f64>, NyxError> {
let osc = self.cosm.frame_chg(&ctx.orbit, self.drag_frame);
let velocity = osc.velocity();
Ok(-0.5 * self.rho * ctx.cd * ctx.drag_area_m2 * velocity.norm() * velocity)
}
fn dual_eom(
&self,
_osc_ctx: &BaseSpacecraft<X>,
) -> Result<(Vector3<f64>, Matrix3<f64>), NyxError> {
Err(NyxError::PartialsUndefined)
}
}
#[derive(Clone)]
pub struct Drag {
pub density: AtmDensity,
pub drag_frame: Frame,
pub cosm: Arc<Cosm>,
}
impl Drag {
pub fn earth_exp(cosm: Arc<Cosm>) -> Arc<Self> {
Arc::new(Self {
density: AtmDensity::Exponential {
rho0: 3.614e-13,
r0: 700_000.0,
ref_alt_m: 88_667.0,
},
drag_frame: cosm.frame("IAU Earth"),
cosm,
})
}
pub fn std_atm1976(cosm: Arc<Cosm>) -> Arc<Self> {
Arc::new(Self {
density: AtmDensity::StdAtm {
max_alt_m: 1_000_000.0,
},
drag_frame: cosm.frame("IAU Earth"),
cosm,
})
}
}
impl fmt::Display for Drag {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(
f,
"\tDrag density {:?} in frame {}",
self.density, self.drag_frame
)
}
}
impl<X: SpacecraftExt> ForceModel<X> for Drag {
fn eom(&self, ctx: &BaseSpacecraft<X>) -> Result<Vector3<f64>, NyxError> {
let integration_frame = ctx.orbit.frame;
let osc = self.cosm.frame_chg(&ctx.orbit, self.drag_frame);
match self.density {
AtmDensity::Constant(rho) => {
let velocity = osc.velocity();
Ok(-0.5 * rho * ctx.cd * ctx.drag_area_m2 * velocity.norm() * velocity)
}
AtmDensity::Exponential {
rho0,
r0,
ref_alt_m,
} => {
let rho = rho0
* (-(osc.rmag() - (r0 + self.drag_frame.equatorial_radius())) / ref_alt_m)
.exp();
let velocity_integr_frame = self.cosm.frame_chg(&osc, integration_frame).velocity();
let velocity = velocity_integr_frame - osc.velocity();
Ok(-0.5 * rho * ctx.cd * ctx.drag_area_m2 * velocity.norm() * velocity)
}
AtmDensity::StdAtm { max_alt_m } => {
let altitude_km = osc.rmag() - self.drag_frame.equatorial_radius();
let rho = if altitude_km > max_alt_m / 1_000.0 {
10.0_f64.powf((-7e-5) * altitude_km - 14.464)
} else {
let scale = (altitude_km - 526.8000) / 292.8563;
let logdensity =
0.34047 * scale.powi(6) - 0.5889 * scale.powi(5) - 0.5269 * scale.powi(4)
+ 1.0036 * scale.powi(3)
+ 0.60713 * scale.powi(2)
- 2.3024 * scale
- 12.575;
10.0_f64.powf(logdensity)
};
let velocity_integr_frame = self.cosm.frame_chg(&osc, integration_frame).velocity();
let velocity = velocity_integr_frame - osc.velocity();
Ok(-0.5 * rho * ctx.cd * ctx.drag_area_m2 * velocity.norm() * velocity)
}
}
}
fn dual_eom(
&self,
_osc_ctx: &BaseSpacecraft<X>,
) -> Result<(Vector3<f64>, Matrix3<f64>), NyxError> {
Err(NyxError::PartialsUndefined)
}
}