use crate::dimensions::Vector3;
use crate::time::Epoch;
use celestia::{Frame, State};
use std::f64::consts::FRAC_PI_2 as half_pi;
pub trait ThrustControl
where
Self: Clone + Sized,
{
fn direction(&self, state: &State) -> Vector3<f64>;
fn throttle(&self, state: &State) -> f64;
fn next(&mut self, state: &State);
}
#[derive(Clone)]
pub struct NoThrustControl {}
impl ThrustControl for NoThrustControl {
fn direction(&self, _: &State) -> Vector3<f64> {
unimplemented!();
}
fn throttle(&self, _: &State) -> f64 {
unimplemented!();
}
fn next(&mut self, _: &State) {
unimplemented!();
}
}
#[derive(Copy, Clone, Debug)]
pub enum Achieve {
Sma { target: f64, tol: f64 },
Ecc { target: f64, tol: f64 },
Inc { target: f64, tol: f64 },
Raan { target: f64, tol: f64 },
Aop { target: f64, tol: f64 },
}
impl Achieve {
pub fn achieved(&self, state: &State) -> bool {
match *self {
Achieve::Sma { target, tol } => (state.sma() - target).abs() < tol,
Achieve::Ecc { target, tol } => (state.ecc() - target).abs() < tol,
Achieve::Inc { target, tol } => (state.inc() - target).abs() < tol,
Achieve::Raan { target, tol } => (state.raan() - target).abs() < tol,
Achieve::Aop { target, tol } => (state.aop() - target).abs() < tol,
}
}
}
#[derive(Copy, Clone, Debug)]
pub struct Mnvr {
pub start: Epoch,
pub end: Epoch,
pub thrust_lvl: f64,
pub vector: Vector3<f64>,
}
impl Mnvr {
pub fn instantaneous(dt: Epoch, vector: Vector3<f64>) -> Self {
let mut end_dt = dt;
end_dt.mut_add_secs(1e-6);
Self {
start: dt,
end: end_dt,
thrust_lvl: 1.0,
vector,
}
}
}
#[derive(Clone, Debug)]
pub struct Ruggiero {
objectives: Vec<Achieve>,
init_state: State,
achieved: bool,
}
impl Ruggiero {
pub fn new(objectives: Vec<Achieve>, initial: State) -> Self {
Self {
objectives,
init_state: initial,
achieved: false,
}
}
fn weighting(init: f64, target: f64, osc: f64, tol: f64) -> f64 {
if (osc - target).abs() < tol {
0.0
} else {
(target - osc)
/ (target
- if (init - target).abs() < tol {
init + tol
} else {
init
})
.abs()
}
}
pub fn achieved(&self, state: &State) -> bool {
for obj in &self.objectives {
if !obj.achieved(state) {
return false;
}
}
true
}
}
impl ThrustControl for Ruggiero {
fn direction(&self, osc: &State) -> Vector3<f64> {
if self.achieved {
Vector3::zeros()
} else {
let mut ctrl = Vector3::zeros();
for obj in &self.objectives {
match *obj {
Achieve::Sma { target, tol } => {
let weight = Self::weighting(self.init_state.sma(), target, osc.sma(), tol);
if weight.abs() > 0.0 {
let num = osc.ecc() * osc.ta().to_radians().sin();
let denom = 1.0 + osc.ecc() * osc.ta().to_radians().cos();
let alpha = num.atan2(denom);
ctrl += unit_vector_from_angles(alpha, 0.0) * weight;
}
}
Achieve::Ecc { target, tol } => {
let weight = Self::weighting(self.init_state.ecc(), target, osc.ecc(), tol);
if weight.abs() > 0.0 {
let num = osc.ta().to_radians().sin();
let denom = osc.ta().to_radians().cos() + osc.ea().to_radians().cos();
let alpha = num.atan2(denom);
ctrl += unit_vector_from_angles(alpha, 0.0) * weight;
}
}
Achieve::Inc { target, tol } => {
let weight = Self::weighting(self.init_state.inc(), target, osc.inc(), tol);
if weight.abs() > 0.0 {
let beta =
half_pi.copysign(((osc.ta() + osc.aop()).to_radians()).cos());
ctrl += unit_vector_from_angles(0.0, beta) * weight;
}
}
Achieve::Raan { target, tol } => {
let weight =
Self::weighting(self.init_state.raan(), target, osc.raan(), tol);
if weight.abs() > 0.0 {
let beta =
half_pi.copysign(((osc.ta() + osc.aop()).to_radians()).sin());
ctrl += unit_vector_from_angles(0.0, beta) * weight;
}
}
Achieve::Aop { target, tol } => {
let weight = Self::weighting(self.init_state.aop(), target, osc.aop(), tol);
let oe2 = 1.0 - osc.ecc().powi(2);
let e3 = osc.ecc().powi(3);
let sqrt_val = (0.25 * (oe2 / e3).powi(2) + 1.0 / 27.0).sqrt();
let opti_ta_alpha = ((oe2 / (2.0 * e3) + sqrt_val).powf(1.0 / 3.0)
- (-oe2 / (2.0 * e3) + sqrt_val).powf(1.0 / 3.0)
- 1.0 / osc.ecc())
.acos();
let opti_ta_beta = (-osc.ecc() * osc.aop().to_radians().cos()).acos()
- osc.aop().to_radians();
if (osc.ta().to_radians() - opti_ta_alpha).abs()
< (osc.ta().to_radians() - opti_ta_beta).abs()
{
let p = osc.semi_parameter();
let (sin_ta, cos_ta) = osc.ta().to_radians().sin_cos();
let alpha = (-p * cos_ta).atan2((p + osc.rmag()) * sin_ta);
ctrl += unit_vector_from_angles(alpha, 0.0) * weight;
} else {
let beta = half_pi
.copysign(-(osc.ta().to_radians() + osc.aop().to_radians()).sin())
* osc.inc().to_radians().cos();
ctrl += unit_vector_from_angles(0.0, beta) * weight;
};
}
}
}
ctrl = if ctrl.norm() > 0.0 {
ctrl / ctrl.norm()
} else {
ctrl
};
osc.dcm_to_inertial(Frame::RCN) * ctrl
}
}
fn throttle(&self, osc: &State) -> f64 {
if self.achieved {
0.0
} else {
for obj in &self.objectives {
match *obj {
Achieve::Sma { target, tol } => {
let weight = Self::weighting(self.init_state.sma(), target, osc.sma(), tol);
if weight.abs() > 0.0 {
return 1.0;
}
}
Achieve::Ecc { target, tol } => {
let weight = Self::weighting(self.init_state.ecc(), target, osc.ecc(), tol);
if weight.abs() > 0.0 {
return 1.0;
}
}
Achieve::Inc { target, tol } => {
let weight = Self::weighting(self.init_state.inc(), target, osc.inc(), tol);
if weight.abs() > 0.0 {
return 1.0;
}
}
Achieve::Raan { target, tol } => {
let weight =
Self::weighting(self.init_state.raan(), target, osc.raan(), tol);
if weight.abs() > 0.0 {
return 1.0;
}
}
Achieve::Aop { target, tol } => {
let weight = Self::weighting(self.init_state.aop(), target, osc.aop(), tol);
if weight.abs() > 0.0 {
return 1.0;
}
}
}
}
0.0
}
}
fn next(&mut self, osc: &State) {
if self.throttle(osc) > 0.0 {
if self.achieved {
info!("enabling control: {:o}", osc);
}
self.achieved = false;
} else {
if !self.achieved {
info!("disabling control: {:o}", osc);
}
self.achieved = true;
}
}
}
#[derive(Clone, Debug)]
pub struct FiniteBurns {
pub mnvrs: Vec<Mnvr>,
pub mnvr_no: usize,
}
impl FiniteBurns {
pub fn from_mnvrs(mnvrs: Vec<Mnvr>) -> Self {
Self { mnvrs, mnvr_no: 0 }
}
}
impl ThrustControl for FiniteBurns {
fn direction(&self, osc: &State) -> Vector3<f64> {
if self.mnvr_no >= self.mnvrs.len() {
Vector3::zeros()
} else {
let next_mnvr = self.mnvrs[self.mnvr_no];
if next_mnvr.start <= osc.dt {
osc.dcm_to_inertial(Frame::VNC) * next_mnvr.vector
} else {
Vector3::zeros()
}
}
}
fn throttle(&self, osc: &State) -> f64 {
if self.mnvr_no >= self.mnvrs.len() {
0.0
} else {
let next_mnvr = self.mnvrs[self.mnvr_no];
if next_mnvr.start <= osc.dt {
next_mnvr.thrust_lvl
} else {
0.0
}
}
}
fn next(&mut self, osc: &State) {
if self.mnvr_no < self.mnvrs.len() {
let cur_mnvr = self.mnvrs[self.mnvr_no];
if osc.dt >= cur_mnvr.end {
self.mnvr_no += 1;
}
}
}
}
fn unit_vector_from_angles(alpha: f64, beta: f64) -> Vector3<f64> {
Vector3::new(
alpha.sin() * beta.cos(),
alpha.cos() * beta.cos(),
beta.sin(),
)
}
#[cfg(test)]
mod tests {
use super::*;
use celestia::Cosm;
#[test]
fn ruggiero_weight() {
let mut cosm = Cosm::from_xb("./de438s");
cosm.mut_gm_for_frame("EME2000", 398_600.433);
let eme2k = cosm.frame("EME2000");
let start_time = Epoch::from_gregorian_tai_at_midnight(2020, 1, 1);
let orbit = State::keplerian(7378.1363, 0.01, 0.05, 0.0, 0.0, 1.0, start_time, eme2k);
let objectives = vec![
Achieve::Sma {
target: 42164.0,
tol: 1.0,
},
Achieve::Ecc {
target: 0.01,
tol: 5e-5,
},
];
let ruggiero = Ruggiero::new(objectives, orbit);
let osc = State::cartesian(
7_303.253_461_441_64f64,
127.478_714_816_381_75,
0.111_246_193_227_445_4,
-0.128_284_025_765_195_6,
7.422_889_151_816_439,
0.006_477_694_429_837_2,
start_time,
eme2k,
);
let expected = Vector3::new(
-0.017_279_636_133_108_3,
0.999_850_315_226_803,
0.000_872_534_222_883_2,
);
let got = ruggiero.direction(&osc);
println!("{}", expected - got);
assert!(
(expected - got).norm() < 1e-12,
"incorrect direction computed"
);
}
}