astrodynamics 0.1.0

Numerical astrodynamics engine for orbit propagation, force models, and flight-dynamics primitives
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

use crate::state::{CartesianState, StateDerivative};
use crate::propagator::api::{PropagationContext, IntegratorOptions};
use crate::propagator::result::{PropagationResult, PropagationPoint, PropagationStats};
use crate::error::PropagationError;
use crate::integrators::{Integrator, DynamicsModel};

pub struct RK4;

impl Integrator for RK4 {
    fn propagate(
        &self,
        initial: CartesianState,
        t_end_seconds: f64,
        rhs: &dyn DynamicsModel,
        ctx: &PropagationContext,
        opts: &IntegratorOptions,
    ) -> Result<PropagationResult, PropagationError> {
        let mut state = initial;
        let mut t = initial.epoch_tdb_seconds;
        let dt_target = t_end_seconds - t;
        let sign = dt_target.signum();
        let target_abs = dt_target.abs();
        
        let h_initial = opts.initial_step.min(target_abs) * sign;
        let mut h = h_initial;
        let mut steps = 0;
        let mut points = Vec::new();
        
        points.push(PropagationPoint {
            epoch_tdb_seconds: t,
            position_km: state.position_array(),
            velocity_km_s: state.velocity_array(),
        });

        while (t - initial.epoch_tdb_seconds).abs() < target_abs {
            if steps >= opts.max_steps {
                return Err(PropagationError::MaxStepsExceeded);
            }
            
            if (t + h - initial.epoch_tdb_seconds).abs() > target_abs {
                h = t_end_seconds - t;
            }

            let next_state = self.step(state, h, rhs, ctx)?;
            state = next_state;
            t += h;
            steps += 1;

            if opts.dense_output {
                points.push(PropagationPoint {
                    epoch_tdb_seconds: t,
                    position_km: state.position_array(),
                    velocity_km_s: state.velocity_array(),
                });
            }
        }

        if !opts.dense_output {
             points.push(PropagationPoint {
                epoch_tdb_seconds: t,
                position_km: state.position_array(),
                velocity_km_s: state.velocity_array(),
            });
        }

        Ok(PropagationResult {
            final_state: state,
            points,
            events: Vec::new(),
            stats: PropagationStats {
                accepted_steps: steps,
                rejected_steps: 0,
                evaluations: steps * 4,
            },
        })
    }
}

impl RK4 {
    fn step(
        &self,
        state: CartesianState,
        h: f64,
        rhs: &dyn DynamicsModel,
        ctx: &PropagationContext,
    ) -> Result<CartesianState, PropagationError> {
        let k1 = rhs.derivative(&state, ctx)?;
        
        let s2 = self.advance(&state, &k1, h / 2.0);
        let k2 = rhs.derivative(&s2, ctx)?;
        
        let s3 = self.advance(&state, &k2, h / 2.0);
        let k3 = rhs.derivative(&s3, ctx)?;
        
        let s4 = self.advance(&state, &k3, h);
        let k4 = rhs.derivative(&s4, ctx)?;
        
        let dpos = (k1.dpos_km_s + k2.dpos_km_s * 2.0 + k3.dpos_km_s * 2.0 + k4.dpos_km_s) * (h / 6.0);
        let dvel = (k1.dvel_km_s2 + k2.dvel_km_s2 * 2.0 + k3.dvel_km_s2 * 2.0 + k4.dvel_km_s2) * (h / 6.0);
        
        Ok(CartesianState {
            epoch_tdb_seconds: state.epoch_tdb_seconds + h,
            position_km: state.position_km + dpos,
            velocity_km_s: state.velocity_km_s + dvel,
        })
    }

    fn advance(&self, state: &CartesianState, deriv: &StateDerivative, h: f64) -> CartesianState {
        CartesianState {
            epoch_tdb_seconds: state.epoch_tdb_seconds + h,
            position_km: state.position_km + deriv.dpos_km_s * h,
            velocity_km_s: state.velocity_km_s + deriv.dvel_km_s2 * h,
        }
    }
}