diff --git a/src/controller.rs b/src/controller.rs index b3b04ed..aee00c8 100644 --- a/src/controller.rs +++ b/src/controller.rs @@ -1,43 +1,31 @@ -use super::ode::SystemTrait; -use num_traits::Float; - -pub trait Controller -where - f64: From, - F: SystemTrait, - T: Copy + From, -{ - fn determine_step(&mut self, h: T, err: T) -> (bool, T); +pub trait Controller { + fn determine_step(&mut self, h: f64, err: f64) -> (bool, f64); } #[derive(Debug, Clone, Copy)] -pub struct PIController where f64: From { - pub alpha: T, - pub beta: T, - pub factor_c1: T, - pub factor_c2: T, - pub factor_old: T, - pub h_max: T, - pub safety_factor: T, - pub old_h: T, +pub struct PIController { + pub alpha: f64, + pub beta: f64, + pub factor_c1: f64, + pub factor_c2: f64, + pub factor_old: f64, + pub h_max: f64, + pub safety_factor: f64, + pub old_h: f64, } -impl Controller for PIController -where - f64: From, - F: SystemTrait, - T: Copy + From + Float, -{ +impl Controller for PIController { /// Determines if the previously run step size and error were valid or not. Either way, it also /// returns what the next step size should be - fn determine_step(&mut self, h: T, err: T) -> (bool, T) { + fn determine_step(&mut self, h: f64, err: f64) -> (bool, f64) { let factor_11 = err.powf(self.alpha); 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)); + // let mut h_new = 0.9 * h * err.powf(-1.0 / 5.0); + println!("err: {}\th_new: {}", err, h_new); + if err <= 1.0 { // Accept the stepsize - self.factor_old = err.max(1.0e-4.into()); + self.factor_old = err.max(1.0e-4); if h_new.abs() > self.h_max { // If the step is too big h_new = self.h_max.copysign(h_new); @@ -52,18 +40,14 @@ where } } -impl PIController -where - f64: From, - T: Copy + From + Float, -{ - pub fn new(alpha:T, beta:T, max_factor: T, min_factor: T, h_max: T, safety_factor: T, initial_h: T) -> Self { +impl PIController { + pub fn new(alpha:f64, beta:f64, max_factor: f64, min_factor: f64, h_max: f64, safety_factor: f64, initial_h: f64) -> Self { Self { alpha: alpha, beta: beta, - factor_c1: >::from(1.0.into()) / min_factor, - factor_c2: >::from(1.0.into()) / max_factor, - factor_old: 1.0e-4.into(), + factor_c1: 1.0 / min_factor, + factor_c2: 1.0 / max_factor, + factor_old: 1.0e-4, h_max: h_max.abs(), safety_factor: safety_factor, old_h: initial_h, @@ -87,6 +71,5 @@ mod tests { assert!(controller.h_max == 10.0); assert!(controller.safety_factor == 0.9); assert!(controller.old_h == 1e-4); - // assert!(controller.determine_step(0.1, 0.1) == (true, 0.01)); } } diff --git a/src/integrator.rs b/src/integrator.rs deleted file mode 100644 index 87e5eaf..0000000 --- a/src/integrator.rs +++ /dev/null @@ -1,392 +0,0 @@ -use num_traits::identities; -use num_traits::Float; -use std::cmp::PartialEq; -use std::fmt::Debug; -use std::ops::{Add, AddAssign, Sub, SubAssign, Mul, MulAssign}; -use nalgebra::{SVector, SMatrix}; -use nalgebra as na; - -use super::ode::{ODE, SystemTrait}; - -#[derive(Debug, Clone, Copy)] -pub struct Tableau { - a: SMatrix, - b: SVector, - b_star: Option>, - c: SVector, -} - - -/// Integrator Trait -/// -/// T = The value type, usually f64, but must at least implement to/from f64 -/// F = A type that implements System -/// D1 = The dimension of the system -/// D2 = The dimension of the butcher tableau (the order of the integrator) -pub trait Integrator -where - f64: From, - F: SystemTrait, - T: Copy + From + identities::One + identities::Zero + PartialEq + Debug + Add + AddAssign + Sub + SubAssign + Mul + MulAssign + 'static, -{ - /// Returns the tableau - fn tableau(&self) -> Tableau; - - fn _step1(&self, ode: &ODE, h: T) -> (SVector, Option>) { - let k0 = ode.f.derivative(ode.t, ode.y); - let sum = k0 * self.tableau().b[0]; - (ode.y + (sum * h), None) - } - - fn _step2(&self, ode: &ODE, h: T) -> (SVector, Option>) { - let k0 = ode.f.derivative(ode.t, ode.y); - let k1 = ode.f.derivative( - ode.t + self.tableau().c[1] * h, - ode.y + k0 * self.tableau().a[(1,0)] * h, - ); - let sum = - 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)) * h), - None => None - }; - (ode.y + (sum * h), err) - } - - fn _step3(&self, ode: &ODE, h: T) -> (SVector, Option>) { - let k0 = ode.f.derivative(ode.t, ode.y); - let k1 = ode.f.derivative( - ode.t + self.tableau().c[1] * h, - ode.y + k0 * self.tableau().a[(1,0)] * h, - ); - let k2 = ode.f.derivative( - ode.t + self.tableau().c[2] * h, - ode.y + ( - k0 * self.tableau().a[(2,0)] + - k1 * self.tableau().a[(2,1)] - ) * h, - ); - let sum = - k0 * self.tableau().b[0] + - 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] + - k1 * b_star[1] + - k2 * b_star[2]) * T::from(-1.0_f64)) * h), - None => None - }; - (ode.y + (sum * h), err) - } - - fn _step4(&self, ode: &ODE, h: T) -> (SVector, Option>) { - let k0 = ode.f.derivative(ode.t, ode.y); - let k1 = ode.f.derivative( - ode.t + self.tableau().c[1] * h, - ode.y + k0 * self.tableau().a[(1,0)] * h, - ); - let k2 = ode.f.derivative( - ode.t + self.tableau().c[2] * h, - ode.y + ( - k0 * self.tableau().a[(2,0)] + - k1 * self.tableau().a[(2,1)] - ) * h, - ); - let k3 = ode.f.derivative( - ode.t + self.tableau().c[3] * h, - ode.y + ( - k0 * self.tableau().a[(3,0)] + - k1 * self.tableau().a[(3,1)] + - k2 * self.tableau().a[(3,2)] - ) * h, - ); - let sum = - k0 * self.tableau().b[0] + - k1 * self.tableau().b[1] + - 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] + - k1 * b_star[1] + - k2 * b_star[2] + - k3 * b_star[3]) * T::from(-1.0_f64)) * h), - None => None - }; - (ode.y + (sum * h), err) - } - - fn _step5(&self, ode: &ODE, h: T) -> (SVector, Option>) { - let k0 = ode.f.derivative(ode.t, ode.y); - let k1 = ode.f.derivative( - ode.t + self.tableau().c[1] * h, - ode.y + k0 * self.tableau().a[(1,0)] * h, - ); - let k2 = ode.f.derivative( - ode.t + self.tableau().c[2] * h, - ode.y + ( - k0 * self.tableau().a[(2,0)] + - k1 * self.tableau().a[(2,1)] - ) * h, - ); - let k3 = ode.f.derivative( - ode.t + self.tableau().c[3] * h, - ode.y + ( - k0 * self.tableau().a[(3,0)] + - k1 * self.tableau().a[(3,1)] + - k2 * self.tableau().a[(3,2)] - ) * h, - ); - let k4 = ode.f.derivative( - ode.t + self.tableau().c[4] * h, - ode.y + ( - k0 * self.tableau().a[(4,0)] + - k1 * self.tableau().a[(4,1)] + - k2 * self.tableau().a[(4,2)] + - k3 * self.tableau().a[(4,3)] - ) * h, - ); - let sum = - k0 * self.tableau().b[0] + - k1 * self.tableau().b[1] + - k2 * self.tableau().b[2] + - 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] + - k1 * b_star[1] + - k2 * b_star[2] + - k3 * b_star[3] + - k4 * b_star[4]) * T::from(-1.0_f64)) * h), - None => None - }; - (ode.y + (sum * h), err) - } - - fn _step6(&self, ode: &ODE, h: T) -> (SVector, Option>) { - let k0 = ode.f.derivative(ode.t, ode.y); - let k1 = ode.f.derivative( - ode.t + self.tableau().c[1] * h, - ode.y + k0 * self.tableau().a[(1,0)] * h, - ); - let k2 = ode.f.derivative( - ode.t + self.tableau().c[2] * h, - ode.y + ( - k0 * self.tableau().a[(2,0)] + - k1 * self.tableau().a[(2,1)] - ) * h, - ); - let k3 = ode.f.derivative( - ode.t + self.tableau().c[3] * h, - ode.y + ( - k0 * self.tableau().a[(3,0)] + - k1 * self.tableau().a[(3,1)] + - k2 * self.tableau().a[(3,2)] - ) * h, - ); - let k4 = ode.f.derivative( - ode.t + self.tableau().c[4] * h, - ode.y + ( - k0 * self.tableau().a[(4,0)] + - k1 * self.tableau().a[(4,1)] + - k2 * self.tableau().a[(4,2)] + - k3 * self.tableau().a[(4,3)] - ) * h, - ); - let k5 = ode.f.derivative( - ode.t + self.tableau().c[5] * h, - ode.y + ( - k0 * self.tableau().a[(5,0)] + - k1 * self.tableau().a[(5,1)] + - k2 * self.tableau().a[(5,2)] + - k3 * self.tableau().a[(5,3)] + - k3 * self.tableau().a[(5,4)] - ) * h, - ); - let sum = - k0 * self.tableau().b[0] + - k1 * self.tableau().b[1] + - k2 * self.tableau().b[2] + - k3 * self.tableau().b[3] + - 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] + - 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)) * h), - None => None - }; - (ode.y + (sum * h), err) - } - - fn step(&self, ode: &ODE, h: T) -> (SVector, Option); - -} - -pub struct FixedFourthOrder -where - f64: From, - T: Copy + From + identities::One + identities::Zero + PartialEq + Debug + Add + AddAssign + Sub + SubAssign + Mul + MulAssign + 'static, -{ - pub tableau: Tableau, -} - -impl Integrator for FixedFourthOrder -where - f64: From, - F: SystemTrait, - T: Copy + From + identities::One + identities::Zero + PartialEq + Debug + Add + AddAssign + Sub + SubAssign + Mul + MulAssign + 'static, -{ - fn tableau(&self) -> Tableau { self.tableau } - - fn step(&self, ode: &ODE, h: T) -> (SVector, Option) { (self._step4(ode, h).0, None) } -} - -pub struct AdaptiveSixthOrder -where - f64: From, - T: Copy + From + identities::One + identities::Zero + PartialEq + Debug + Add + AddAssign + Sub + SubAssign + Mul + MulAssign + 'static, -{ - pub tableau: Tableau, - pub atol: T, - pub rtol: T, -} - -impl Integrator for AdaptiveSixthOrder -where - f64: From, - F: SystemTrait, - T: Copy + - From + - identities::One + - identities::Zero + - PartialEq + - Debug + - Add + - AddAssign + - Sub + - SubAssign + - Mul + - MulAssign + - Float + - 'static, -{ - fn tableau(&self) -> Tableau { self.tableau } - - fn step(&self, ode: &ODE, h: T) -> (SVector, Option) { - let (next_y, err_option) = self._step6(ode, h); - let mut err: T = 0.0.into(); - let err_array = err_option.unwrap(); - ode.y.iter().zip(next_y.iter()).enumerate().for_each(|(i, (yi, next_yi))| { - let tol = self.atol + yi.abs().max(next_yi.abs()) * self.rtol; - err += (err_array[i] / tol).powi(2); - }); - (next_y, Some(err.sqrt())) - } -} - -pub const RUNGE_KUTTA_4_TABLEAU: Tableau = Tableau { - a: na::Matrix4::new( - 0.0, 0.0, 0.0, 0.0, - 0.5, 0.0, 0.0, 0.0, - 0.0, 0.5, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - ), - b: na::Vector4::new(1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0), - b_star: None, - c: na::Vector4::new(0.0, 0.5, 0.5, 1.0), -}; - -pub const RUNGE_KUTTA_FEHLBERG_54_TABLEAU: Tableau = Tableau { - a: na::Matrix6::new( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, - 3.0/32.0, 9.0/32.0, 0.0, 0.0, 0.0, 0.0, - 1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0, 0.0, 0.0, 0.0, - 439.0/216.0, -8.0, 3680.0/513.0, -845.0/4104.0, 0.0, 0.0, - -8.0/27.0, 2.0, -3544.0/2565.0, 1859.0/4104.0, -11.0/40.0, 0.0, - ), - b: na::Vector6::new( - 16.0/135.0, - 0.0, - 6656.0/12825.0, - 28561.0/56430.0, - -9.0/50.0, - 2.0/55.0, - ), - b_star: Some( - na::Vector6::new( - 25.0/216.0, - 0.0, - 1408.0/2565.0, - 2197.0/4104.0, - -1.0/5.0, - 0.0, - ) - ), - c: na::Vector6::new(0.0, 0.25, 3.0/8.0, 12.0/13.0, 1.0, 0.5), -}; - -#[cfg(test)] -mod tests { - use super::*; - use nalgebra::Vector3; - use approx::assert_relative_eq; - - #[test] - fn test_rk4() { - struct System {} - - impl SystemTrait for System { - fn derivative(&self, _t: f64, y: Vector3) -> Vector3 { y } - } - - let system = System {}; - let y0 = Vector3::new(1.0, 1.0, 1.0); - let mut ode = ODE::new(system, 0.0, 30.0, y0); - - let rk4 = FixedFourthOrder { tableau: RUNGE_KUTTA_4_TABLEAU }; - - // Test that y'(t) = y(t) solves to y(t) = e^t for rk4 - let step = 0.001; - while ode.t < ode.t_end { - ode.y = rk4.step(&ode, step).0; - ode.t += step; - assert_relative_eq!(ode.y[0], ode.t.exp(), max_relative=0.000001); - } - } - - #[test] - fn test_rkf54() { - struct System {} - - impl SystemTrait for System { - fn derivative(&self, _t: f64, y: Vector3) -> Vector3 { y } - } - - 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 rkf54 = AdaptiveSixthOrder { - tableau: RUNGE_KUTTA_FEHLBERG_54_TABLEAU, - atol: 1e-3_f64, - rtol: 1e-3_f64, - }; - - // Test that y'(t) = y(t) solves to y(t) = e^t for rkf54 - // and also that the error seems reasonable - let step = 0.001; - while ode.t < ode.t_end { - let (new_y, err) = rkf54.step(&ode, step); - ode.y = new_y; - ode.t += step; - assert_relative_eq!(ode.y[0], ode.t.exp(), max_relative=0.000001); - assert!(err.unwrap() < 0.0001); - } - } -} diff --git a/src/integrator/dormand_prince.rs b/src/integrator/dormand_prince.rs new file mode 100644 index 0000000..7b8c43a --- /dev/null +++ b/src/integrator/dormand_prince.rs @@ -0,0 +1,109 @@ +use nalgebra::SVector; + +use super::super::ode::ODE; +use super::Integrator; + +/// Integrator Trait +pub trait DormandPrinceIntegrator { + const A: &'static [f64]; + const B: &'static [f64]; + const C: &'static [f64]; +} + +pub struct DormandPrince45 { + k: Vec>, + a_tol: f64, + r_tol: f64, +} + +impl DormandPrince45 where DormandPrince45: Integrator { + pub fn new(a_tol: f64, r_tol: f64) -> Self { + Self { + k: vec![SVector::::zeros(); Self::STAGES], + a_tol: a_tol, + r_tol: r_tol, + } + } +} + +impl DormandPrinceIntegrator for DormandPrince45 { + const A: &'static [f64] = &[ + 1.0 / 5.0, + 3.0 / 40.0, + 9.0 / 40.0, + 44.0 / 45.0, + -56.0 / 15.0, + 32.0 / 9.0, + 19_372.0 / 6_561.0, + -25_360.0 / 2_187.0, + 64_448.0 / 6_561.0, + -212.0 / 729.0, + 9_017.0 / 3_168.0, + -355.0 / 33.0, + 46_732.0 / 5247.0, + 49.0 / 176.0, + -5_103.0 / 18_656.0, + 35.0 / 384.0, + 0.0, + 500.0 / 1_113.0, + 125.0 / 192.0, + -2_187.0 / 6_784.0, + 11.0 / 84.0, + ]; + const B: &'static [f64] = &[ + 35.0 / 384.0, + 0.0, + 500.0 / 1_113.0, + 125.0 / 192.0, + -2_187.0 / 6_784.0, + 11.0 / 84.0, + 0.0, + 5_179.0 / 57_600.0, + 0.0, + 7_571.0 / 16_695.0, + 393.0 / 640.0, + -92_097.0 / 339_200.0, + 187.0 / 2_100.0, + 1.0 / 40.0, + ]; + const C: &'static [f64] = &[ + 0.0, + 1.0 / 5.0, + 3.0 / 10.0, + 4.0 / 5.0, + 8.0 / 9.0, + 1.0, + 1.0, + ]; +} + +impl Integrator for DormandPrince45 +where + DormandPrince45: DormandPrinceIntegrator, +{ + const STAGES: usize = 7; + + fn step(&mut self, ode: &ODE, h: f64) -> (SVector, Option) { + let mut next_y = ode.y.clone(); + let mut err = SVector::::zeros(); + // Do the first of the summations + self.k[0] = (ode.f)(ode.t, ode.y); + next_y += self.k[0] * Self::B[0] * h; + err += self.k[0] * (Self::B[0] - Self::B[Self::STAGES]) * h; + // Then the rest + for i in 1..Self::STAGES { + // Compute the ks + let mut y_term = SVector::::zeros(); + for j in 0..i { + y_term += self.k[i-j-1] * Self::A[( i * (i - 1) ) / 2 + j]; + } + self.k[i] = (ode.f)(ode.t + Self::C[i] * h, ode.y + y_term * h); + + // Use that and bis to calculate the y and error terms + next_y += self.k[i] * h * Self::B[i]; + err += self.k[i] * (Self::B[i] - Self::B[i + Self::STAGES]) * h; + } + let tol = SVector::::repeat(self.a_tol) + ode.y * self.r_tol; + (next_y, Some((err.component_div(&tol)).norm())) + } +} diff --git a/src/integrator/mod.rs b/src/integrator/mod.rs new file mode 100644 index 0000000..6b4d2ae --- /dev/null +++ b/src/integrator/mod.rs @@ -0,0 +1,43 @@ +use nalgebra::SVector; + +use super::ode::ODE; + +pub mod dormand_prince; +pub mod rosenbrock; + +/// Integrator Trait +pub trait Integrator { + const STAGES: usize; + fn step(&mut self, ode: &ODE, h: f64) -> (SVector, Option); +} + + +#[cfg(test)] +mod tests { + use super::*; + use super::dormand_prince::*; + use nalgebra::Vector3; + use approx::assert_relative_eq; + + + #[test] + fn test_dopri() { + fn derivative(_t: f64, y: Vector3) -> Vector3 { y } + + let y0 = Vector3::new(1.0, 1.0, 1.0); + let mut ode = ODE::new(&derivative, 0.0, 4.0, y0); + + let mut dp45 = DormandPrince45::new(1e-12_f64, 1e-4_f64); + + // Test that y'(t) = y(t) solves to y(t) = e^t for rkf54 + // and also that the error seems reasonable + let step = 0.0005; + while ode.t < ode.t_end { + let (new_y, err) = dp45.step(&ode, step); + ode.y = new_y; + ode.t += step; + assert_relative_eq!(ode.y[0], ode.t.exp(), max_relative=0.01); + assert!(err.unwrap() < 1.0); + } + } +} diff --git a/src/integrator/rosenbrock.rs b/src/integrator/rosenbrock.rs new file mode 100644 index 0000000..78d8857 --- /dev/null +++ b/src/integrator/rosenbrock.rs @@ -0,0 +1,101 @@ +use nalgebra::SVector; + +use super::super::ode::ODE; +use super::Integrator; + +/// Integrator Trait +pub trait RosenbrockIntegrator { + const GAMMA: f64; + const A: &'static [f64]; + const B: &'static [f64]; + const C: &'static [f64]; + const C2: &'static [f64]; + const D: &'static [f64]; +} + +pub struct Rodas4 { + k: Vec>, + a_tol: f64, + r_tol: f64, +} + +impl Rodas4 where Rodas4: Integrator { + pub fn new(a_tol: f64, r_tol: f64) -> Self { + Self { + k: vec![SVector::::zeros(); Self::STAGES], + a_tol: a_tol, + r_tol: r_tol, + } + } +} + +impl RosenbrockIntegrator for Rodas4 { + const GAMMA: f64 = 0.25; + const A: &'static [f64] = &[ + 1.544000000000000, + 0.9466785280815826, + 0.2557011698983284, + 3.314825187068521, + 2.896124015972201, + 0.9986419139977817, + 1.221224509226641, + 6.019134481288629, + 12.53708332932087, + -0.6878860361058950, + ]; + const B: &'static [f64] = &[ + 10.12623508344586, + -7.487995877610167, + -34.80091861555747, + -7.992771707568823, + 1.025137723295662, + -0.6762803392801253, + 6.087714651680015, + 16.43084320892478, + 24.76722511418386, + -6.594389125716872, + ]; + const C: &'static [f64] = &[ + -5.668800000000000, + -2.430093356833875, + -0.2063599157091915, + -0.1073529058151375, + -9.594562251023355, + -20.47028614809616, + 7.496443313967647, + -10.24680431464352, + -33.99990352819905, + 11.70890893206160, + 8.083246795921522, + -7.981132988064893, + -31.52159432874371, + 16.31930543123136, + -6.058818238834054, + ]; + const C2: &'static [f64] = &[ + 0.0, + 0.386, + 0.21, + 0.63, + ]; + const D: &'static [f64] = &[ + 0.2500000000000000, + -0.1043000000000000, + 0.1035000000000000, + -0.03620000000000023, + ]; +} + +impl Integrator for Rodas4 +where + Rodas4: RosenbrockIntegrator, +{ + const STAGES: usize = 6; + + // TODO: Finish this + fn step(&mut self, ode: &ODE, h: f64) -> (SVector, Option) { + let mut next_y = ode.y.clone(); + let mut err = SVector::::zeros(); + (next_y, Some(err.norm())) + } +} diff --git a/src/lib.rs b/src/lib.rs index 59a9342..f830607 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,26 +11,25 @@ pub mod problem; mod tests { use nalgebra::Vector6; use approx::assert_relative_eq; - use crate::integrator::*; - use crate::controller::{PIController}; - use crate::ode::{SystemTrait, ODE}; + use crate::integrator::dormand_prince::DormandPrince45; + use crate::controller::PIController; + use crate::ode::ODE; use crate::problem::Problem; use std::f64::consts::PI; #[test] 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)); - Vector6::new(state[3], state[4], state[5], acc[0], acc[1], acc[2]) - } - } + // Calculate one period + let a = 6.7781363e6_f64; + let period = 2.0 * PI * (a.powi(3)/3.98600441500000e14).sqrt(); + println!("{}", period); - let system = System { mu: 3.98600441500000e14 }; + // Set up the system + fn derivative(_t: f64, state: Vector6) -> Vector6 { + let acc = -(3.98600441500000e14 * 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 y0 = Vector6::new( 4.26387250e+06, 5.14619397e+06, @@ -39,27 +38,25 @@ mod tests { 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-12_f64, - rtol: 1e-7_f64, - }; - let mut problem = Problem::new(ode, rkf54, controller); + + // Integrate + let ode = ODE::new(&derivative, 0.0, period, y0); + let dp45 = DormandPrince45::new(1e-12_f64, 1e-8_f64); + let controller = PIController::new(0.37, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4); + + let mut problem = Problem::new(ode, dp45, controller); let solution = problem.solve(); + println!("{}", solution.times.len()); // panic!(); + // TODO: Something still isn't right with these tolerances I think... 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); + assert_relative_eq!(solution.states[solution.states.len()-1][0], y0[0], max_relative=1e-3); + assert_relative_eq!(solution.states[solution.states.len()-1][1], y0[1], max_relative=1e-3); + assert_relative_eq!(solution.states[solution.states.len()-1][2], y0[2], max_relative=1e-3); + assert_relative_eq!(solution.states[solution.states.len()-1][3], y0[3], max_relative=1e-3); + assert_relative_eq!(solution.states[solution.states.len()-1][4], y0[4], max_relative=1e-3); + assert_relative_eq!(solution.states[solution.states.len()-1][5], y0[5], max_relative=1e-3); } } diff --git a/src/ode.rs b/src/ode.rs index 9adf5a4..be8765d 100644 --- a/src/ode.rs +++ b/src/ode.rs @@ -4,37 +4,32 @@ use nalgebra::SVector; /// /// The user will have to define their own system. They are free to add params to their system /// definition and use those in the derivative function -pub trait SystemTrait { +pub trait SystemTrait { fn derivative(&self, t: T, y: SVector) -> SVector; } /// The basic ODE object that will be passed around. The type (T) and the size (D) will be /// determined upon creation of the object -#[derive(Debug, Clone, Copy)] -pub struct ODE where F: SystemTrait{ - pub f: F, - pub y: SVector, - pub t: T, - pub t0: T, - pub t_end: T, - pub h: T, +#[derive(Clone, Copy)] +pub struct ODE<'a, const D: usize> { + pub f: &'a dyn Fn(f64, SVector) -> SVector, + pub y: SVector, + pub t: f64, + pub t0: f64, + pub t_end: f64, + pub h: f64, pub finished: bool, } -impl ODE -where - F: SystemTrait, - T: Copy + From, - f64: From, -{ - pub fn new(f: F, t0: T, t_end: T, y0: SVector) -> Self { +impl<'a, const D: usize> ODE<'a,D> { + pub fn new(f: &'a (dyn Fn(f64, SVector) -> SVector), t0: f64, t_end: f64, y0: SVector) -> Self { Self { f: f, y: y0, t: t0, t0: t0, t_end: t_end, - h: 0.001.into(), + h: 0.001, finished: false, } } @@ -48,17 +43,12 @@ mod tests { #[test] fn test_ode_creation() { - struct System {} + fn derivative(_t: f64, y: Vector3) -> Vector3 { -y } - impl SystemTrait for System { - fn derivative(&self, _t: f64, y: Vector3) -> Vector3 { -y } - } - - let system = System {}; let y0 = Vector3::new(1.0, 0.0, 0.0); - let ode = ODE::new(system, 0.0, 10.0, y0); + let ode = ODE::new(&derivative, 0.0, 10.0, y0); - assert!(ode.f.derivative(0.0, y0) == Vector3::new(-1.0, 0.0, 0.0)); + assert!((ode.f)(0.0, y0) == Vector3::new(-1.0, 0.0, 0.0)); assert!(ode.y == Vector3::new(1.0, 0.0, 0.0)); assert!(ode.t == 0.0); assert!(!ode.finished); diff --git a/src/problem.rs b/src/problem.rs index e7a1ae4..2a75211 100644 --- a/src/problem.rs +++ b/src/problem.rs @@ -1,87 +1,48 @@ -use num_traits::identities; -use num_traits::Float; -use std::fmt::Debug; -use std::cmp::PartialOrd; -use std::ops::{Add, AddAssign, Sub, SubAssign, Mul, MulAssign}; use nalgebra::SVector; -use super::ode::{ODE, SystemTrait}; +use super::ode::ODE; use super::controller::{Controller, PIController}; use super::integrator::Integrator; -pub struct Problem +pub struct Problem<'a, const D: usize, S> where - f64: From, - F: SystemTrait, - T: Copy + - From + - identities::One + - identities::Zero + - PartialEq + - Debug + - Add + - AddAssign + - Sub + - SubAssign + - Mul + - MulAssign + - Float + - PartialOrd + - 'static, - S: Integrator, + S: Integrator, { - ode: ODE, + ode: ODE<'a, D>, integrator: S, - controller: PIController, + controller: PIController, } -impl Problem +impl<'a, const D: usize, S> Problem<'a,D,S> where - f64: From, - F: SystemTrait, - T: Copy + - From + - identities::One + - identities::Zero + - PartialEq + - Debug + - Add + - AddAssign + - Sub + - SubAssign + - Mul + - MulAssign + - Float + - PartialOrd + - 'static, - S: Integrator, + S: Integrator, { - pub fn new(ode: ODE, integrator: S, controller: PIController) -> Self { + pub fn new(ode: ODE<'a,D>, integrator: S, controller: PIController) -> Self { Problem { ode: ode, integrator: integrator, controller: controller, } } - pub fn solve(&mut self) -> Solution { - let mut times: Vec:: = Vec::new(); - let mut states: Vec::> = Vec::new(); - let mut step: T = self.controller.old_h; + pub fn solve(&mut self) -> Solution { + let mut times: Vec:: = Vec::new(); + let mut states: Vec::> = Vec::new(); + let mut step: f64 = self.controller.old_h; times.push(self.ode.t); states.push(self.ode.y); + let (mut new_y, mut err_option) = self.integrator.step(&self.ode, 0.0); while self.ode.t < self.ode.t_end { - let (mut new_y, mut err_option) = self.integrator.step(&self.ode, step); match err_option { Some(mut err) => { // Adaptive Step Size let mut accepted: bool = false; while !accepted { - (accepted, step) = as Controller>::determine_step(&mut self.controller, step, err); + (accepted, step) = >::determine_step(&mut self.controller, step, err); (new_y, err_option) = self.integrator.step(&self.ode, step); err = err_option.unwrap(); } self.controller.old_h = step; - self.controller.h_max = self.ode.t_end - self.ode.t - step; + self.controller.h_max = self.controller.h_max.min(self.ode.t_end - self.ode.t - step); }, None => {}, }; @@ -97,9 +58,9 @@ where } } -pub struct Solution { - pub times: Vec, - pub states: Vec>, +pub struct Solution { + pub times: Vec, + pub states: Vec>, } #[cfg(test)] @@ -107,31 +68,25 @@ mod tests { use super::*; use nalgebra::Vector3; use approx::assert_relative_eq; - use crate::integrator::{AdaptiveSixthOrder, RUNGE_KUTTA_FEHLBERG_54_TABLEAU}; - use crate::controller::{PIController}; + use crate::integrator::dormand_prince::DormandPrince45; + use crate::controller::PIController; - // #[test] + #[test] fn test_problem() { - struct System {} + fn derivative(_t: f64, y: Vector3) -> Vector3 { y } + let y0 = Vector3::new(1.0, 1.0, 1.0); - impl SystemTrait for System { - fn derivative(&self, _t: f64, y: Vector3) -> Vector3 { y } - } + let ode = ODE::new(&derivative, 0.0, 1.0, y0); + let dp45 = DormandPrince45::new(1e-12_f64, 1e-5_f64); + let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 0.1, 0.9, 1e-8); - let system = System {}; - 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 rkf54 = AdaptiveSixthOrder { - tableau: RUNGE_KUTTA_FEHLBERG_54_TABLEAU, - atol: 1e-8_f64, - rtol: 1e-8_f64, - }; - let mut problem = Problem::new(ode, rkf54, controller); + let mut problem = Problem::new(ode, dp45, controller); let solution = problem.solve(); + // println!("{}", solution.times.len()); + // panic!(); solution.times.iter().zip(solution.states.iter()).for_each(|(time, state)| { - assert_relative_eq!(state[0], time.exp(), max_relative=1e-7); + assert_relative_eq!(state[0], time.exp(), max_relative=1e-2); }) } }