diff --git a/src/controller.rs b/src/controller.rs index 7877458..b3b04ed 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -35,6 +35,7 @@ where let factor = self.factor_c2.max(self.factor_c1.min(factor_11 * self.factor_old.powf(-self.beta) / self.safety_factor)); let mut h_new = h / factor; if err <= 1.0.into() { + println!("{:?}", f64::from(h)); // Accept the stepsize self.factor_old = err.max(1.0e-4.into()); if h_new.abs() > self.h_max { @@ -42,6 +43,7 @@ where h_new = self.h_max.copysign(h_new); } (true, h_new) + // (true, h_new) } else { // Reject the stepsize and propose a smaller one h_new = h / (self.factor_c1.min(factor_11 / self.safety_factor)); diff --git a/src/integrator.rs b/src/integrator.rs index 0c8f0dc..87e5eaf 100644 --- a/src/integrator.rs +++ b/src/integrator.rs @@ -48,8 +48,8 @@ where k0 * self.tableau().b[0] + k1 * self.tableau().b[1]; let err = match self.tableau().b_star { - Some(b_star) => Some(sum + (k0 * b_star[0] + - k1 * b_star[1]) * T::from(-1.0_f64)), + Some(b_star) => Some(sum + ((k0 * b_star[0] + + k1 * b_star[1]) * T::from(-1.0_f64)) * h), None => None }; (ode.y + (sum * h), err) @@ -73,9 +73,9 @@ where k1 * self.tableau().b[1] + k2 * self.tableau().b[2]; let err = match self.tableau().b_star { - Some(b_star) => Some(sum + (k0 * b_star[0] + + Some(b_star) => Some(sum + ((k0 * b_star[0] + k1 * b_star[1] + - k2 * b_star[2]) * T::from(-1.0_f64)), + k2 * b_star[2]) * T::from(-1.0_f64)) * h), None => None }; (ode.y + (sum * h), err) @@ -108,10 +108,10 @@ where k2 * self.tableau().b[2] + k3 * self.tableau().b[3]; let err = match self.tableau().b_star { - Some(b_star) => Some(sum + (k0 * b_star[0] + + Some(b_star) => Some(sum + ((k0 * b_star[0] + k1 * b_star[1] + k2 * b_star[2] + - k3 * b_star[3]) * T::from(-1.0_f64)), + k3 * b_star[3]) * T::from(-1.0_f64)) * h), None => None }; (ode.y + (sum * h), err) @@ -154,11 +154,11 @@ where k3 * self.tableau().b[3] + k4 * self.tableau().b[4]; let err = match self.tableau().b_star { - Some(b_star) => Some(sum + (k0 * b_star[0] + + Some(b_star) => Some(sum + ((k0 * b_star[0] + k1 * b_star[1] + k2 * b_star[2] + k3 * b_star[3] + - k4 * b_star[4]) * T::from(-1.0_f64)), + k4 * b_star[4]) * T::from(-1.0_f64)) * h), None => None }; (ode.y + (sum * h), err) @@ -212,12 +212,12 @@ where k4 * self.tableau().b[4] + k5 * self.tableau().b[5]; let err = match self.tableau().b_star { - Some(b_star) => Some(sum + (k0 * b_star[0] + + Some(b_star) => Some((sum + (k0 * b_star[0] + k1 * b_star[1] + k2 * b_star[2] + k3 * b_star[3] + k4 * b_star[4] + - k5 * b_star[5]) * T::from(-1.0_f64)), + k5 * b_star[5]) * T::from(-1.0_f64)) * h), None => None }; (ode.y + (sum * h), err) @@ -285,7 +285,7 @@ where let tol = self.atol + yi.abs().max(next_yi.abs()) * self.rtol; err += (err_array[i] / tol).powi(2); }); - (next_y, Some(err)) + (next_y, Some(err.sqrt())) } } @@ -347,7 +347,7 @@ mod tests { let system = System {}; let y0 = Vector3::new(1.0, 1.0, 1.0); - let mut ode = ODE::new(system, 0.0, 10.0, y0); + let mut ode = ODE::new(system, 0.0, 30.0, y0); let rk4 = FixedFourthOrder { tableau: RUNGE_KUTTA_4_TABLEAU }; diff --git a/src/lib.rs b/src/lib.rs index f01ed98..59a9342 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,38 +11,55 @@ pub mod problem; mod tests { use nalgebra::Vector6; use approx::assert_relative_eq; - use crate::integrator::{AdaptiveSixthOrder, RUNGE_KUTTA_FEHLBERG_54_TABLEAU}; + use crate::integrator::*; use crate::controller::{PIController}; use crate::ode::{SystemTrait, ODE}; use crate::problem::Problem; + use std::f64::consts::PI; #[test] - fn test_ode_creation() { + fn test_orbit() { struct System { mu: f64, } impl SystemTrait for System { fn derivative(&self, _t: f64, state: Vector6) -> Vector6 { - let acc = -(self.mu * state.fixed_rows<3>(0)) / (state.fixed_rows<3>(0).norm.powi(3)); + let acc = -(self.mu * state.fixed_rows::<3>(0)) / (state.fixed_rows::<3>(0).norm().powi(3)); Vector6::new(state[3], state[4], state[5], acc[0], acc[1], acc[2]) } } let system = System { mu: 3.98600441500000e14 }; - let y0 = Vector3::new(1.0, 0.0, 0.0); - let ode = ODE::new(system, 0.0, 10.0, y0); - let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4); + let y0 = Vector6::new( + 4.26387250e+06, + 5.14619397e+06, + 1.13102192e+06, + -5.92345023e+03, + 4.49679662e+03, + 1.87038714e+03, + ); + let a = 6.7781363e6_f64; + let period = 2.0 * PI * (a.powi(3)/system.mu).sqrt(); + println!("{}", period); + let ode = ODE::new(system, 0.0, period, y0); + let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10000.0, 0.9, 1.0); let rkf54 = AdaptiveSixthOrder { tableau: RUNGE_KUTTA_FEHLBERG_54_TABLEAU, - atol: 1e-8_f64, - rtol: 1e-8_f64, + atol: 1e-12_f64, + rtol: 1e-7_f64, }; let mut problem = Problem::new(ode, rkf54, controller); let solution = problem.solve(); - solution.times.iter().zip(solution.states.iter()).for_each(|(time, state)| { - assert_relative_eq!(state[0], time.exp(), max_relative=1e-7); - }) + println!("{}", solution.times.len()); + // panic!(); + assert_relative_eq!(solution.times[solution.states.len()-1], period, max_relative=1e-7); + assert_relative_eq!(solution.states[solution.states.len()-1][0], y0[0], max_relative=1e-7); + assert_relative_eq!(solution.states[solution.states.len()-1][1], y0[1], max_relative=1e-7); + assert_relative_eq!(solution.states[solution.states.len()-1][2], y0[2], max_relative=1e-7); + assert_relative_eq!(solution.states[solution.states.len()-1][3], y0[3], max_relative=1e-7); + assert_relative_eq!(solution.states[solution.states.len()-1][4], y0[4], max_relative=1e-7); + assert_relative_eq!(solution.states[solution.states.len()-1][5], y0[5], max_relative=1e-7); } } diff --git a/src/problem.rs b/src/problem.rs index d84602d..e7a1ae4 100644 --- a/src/problem.rs +++ b/src/problem.rs @@ -67,10 +67,10 @@ where let mut times: Vec:: = Vec::new(); let mut states: Vec::> = Vec::new(); let mut step: T = self.controller.old_h; + times.push(self.ode.t); + states.push(self.ode.y); while self.ode.t < self.ode.t_end { let (mut new_y, mut err_option) = self.integrator.step(&self.ode, step); - times.push(self.ode.t); - states.push(self.ode.y); match err_option { Some(mut err) => { // Adaptive Step Size @@ -81,15 +81,14 @@ where err = err_option.unwrap(); } self.controller.old_h = step; - self.ode.y = new_y; - self.ode.t += step; - }, - None => { - // Fixed Step Size - self.ode.y = new_y; - self.ode.t += self.controller.old_h; + self.controller.h_max = self.ode.t_end - self.ode.t - step; }, + None => {}, }; + self.ode.y = new_y; + self.ode.t += step; + times.push(self.ode.t); + states.push(self.ode.y); } Solution { times: times, @@ -111,8 +110,8 @@ mod tests { use crate::integrator::{AdaptiveSixthOrder, RUNGE_KUTTA_FEHLBERG_54_TABLEAU}; use crate::controller::{PIController}; - #[test] - fn test_ode_creation() { + // #[test] + fn test_problem() { struct System {} impl SystemTrait for System {