diff --git a/src/integrator/dormand_prince.rs b/src/integrator/dormand_prince.rs index 9940111..6953000 100644 --- a/src/integrator/dormand_prince.rs +++ b/src/integrator/dormand_prince.rs @@ -4,11 +4,11 @@ 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]; - const D: &'static [f64]; +pub trait DormandPrinceIntegrator<'a> { + const A: &'a [f64]; + const B: &'a [f64]; + const C: &'a [f64]; + const D: &'a [f64]; } #[derive(Debug, Clone, Copy)] @@ -20,14 +20,14 @@ pub struct DormandPrince45 { impl DormandPrince45 where DormandPrince45: Integrator { pub fn new(a_tol: f64, r_tol: f64) -> Self { Self { - a_tol: a_tol, - r_tol: r_tol, + a_tol, + r_tol, } } } -impl DormandPrinceIntegrator for DormandPrince45 { - const A: &'static [f64] = &[ +impl<'a, const D: usize> DormandPrinceIntegrator<'a> for DormandPrince45 { + const A: &'a [f64] = &[ 1.0 / 5.0, 3.0 / 40.0, 9.0 / 40.0, @@ -50,7 +50,7 @@ impl DormandPrinceIntegrator for DormandPrince45 { -2_187.0 / 6_784.0, 11.0 / 84.0, ]; - const B: &'static [f64] = &[ + const B: &'a [f64] = &[ 35.0 / 384.0, 0.0, 500.0 / 1_113.0, @@ -66,7 +66,7 @@ impl DormandPrinceIntegrator for DormandPrince45 { 187.0 / 2_100.0, 1.0 / 40.0, ]; - const C: &'static [f64] = &[ + const C: &'a [f64] = &[ 0.0, 1.0 / 5.0, 3.0 / 10.0, @@ -75,7 +75,7 @@ impl DormandPrinceIntegrator for DormandPrince45 { 1.0, 1.0, ]; - const D: &'static [f64] = &[ + const D: &'a [f64] = &[ -12715105075.0 / 11282082432.0, 0.0, 87487479700.0 / 32700410799.0, @@ -86,9 +86,9 @@ impl DormandPrinceIntegrator for DormandPrince45 { ]; } -impl Integrator for DormandPrince45 +impl<'a, const D: usize> Integrator for DormandPrince45 where - DormandPrince45: DormandPrinceIntegrator, + DormandPrince45: DormandPrinceIntegrator<'a>, { const ORDER: usize = 5; const STAGES: usize = 7; diff --git a/src/integrator/rosenbrock.rs b/src/integrator/rosenbrock.rs index bdf320c..7311fa4 100644 --- a/src/integrator/rosenbrock.rs +++ b/src/integrator/rosenbrock.rs @@ -4,13 +4,13 @@ use super::super::ode::ODE; use super::Integrator; /// Integrator Trait -pub trait RosenbrockIntegrator { +pub trait RosenbrockIntegrator<'a> { const GAMMA: f64; - const A: &'static [f64]; - const B: &'static [f64]; - const C: &'static [f64]; - const C2: &'static [f64]; - const D: &'static [f64]; + const A: &'a [f64]; + const B: &'a [f64]; + const C: &'a [f64]; + const C2: &'a [f64]; + const D: &'a [f64]; } pub struct Rodas4 { @@ -23,15 +23,15 @@ 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, + a_tol, + r_tol, } } } -impl RosenbrockIntegrator for Rodas4 { +impl<'a, const D: usize> RosenbrockIntegrator<'a> for Rodas4 { const GAMMA: f64 = 0.25; - const A: &'static [f64] = &[ + const A: &'a [f64] = &[ 1.544000000000000, 0.9466785280815826, 0.2557011698983284, @@ -43,7 +43,7 @@ impl RosenbrockIntegrator for Rodas4 { 12.53708332932087, -0.6878860361058950, ]; - const B: &'static [f64] = &[ + const B: &'a [f64] = &[ 10.12623508344586, -7.487995877610167, -34.80091861555747, @@ -55,7 +55,7 @@ impl RosenbrockIntegrator for Rodas4 { 24.76722511418386, -6.594389125716872, ]; - const C: &'static [f64] = &[ + const C: &'a [f64] = &[ -5.668800000000000, -2.430093356833875, -0.2063599157091915, @@ -72,13 +72,13 @@ impl RosenbrockIntegrator for Rodas4 { 16.31930543123136, -6.058818238834054, ]; - const C2: &'static [f64] = &[ + const C2: &'a [f64] = &[ 0.0, 0.386, 0.21, 0.63, ]; - const D: &'static [f64] = &[ + const D: &'a [f64] = &[ 0.2500000000000000, -0.1043000000000000, 0.1035000000000000, diff --git a/src/integrator/tsit.rs b/src/integrator/tsit.rs new file mode 100644 index 0000000..6563ced --- /dev/null +++ b/src/integrator/tsit.rs @@ -0,0 +1,131 @@ +use nalgebra::SVector; + +use super::super::ode::ODE; +use super::Integrator; + +/// Integrator Trait +pub trait TsitIntegrator<'a> { + const A: &'a [f64]; + const B: &'a [f64]; + const C: &'a [f64]; +} + +#[derive(Debug, Clone, Copy)] +pub struct Tsit5 { + a_tol: f64, + r_tol: f64, +} + +impl Tsit5 where Tsit5: Integrator { + pub fn new(a_tol: f64, r_tol: f64) -> Self { + Self { + a_tol, + r_tol, + } + } +} + +impl<'a, const D: usize> TsitIntegrator<'a> for Tsit5 { + const A: &'a [f64] = &[ + 0.161, + -0.008480655492356989, + 0.335480655492357, + 2.8971530571054935, + -6.359448489975075, + 4.3622954328695815, + 5.325864828439257, + -11.748883564062828, + 7.4955393428898365, + -0.09249506636175525, + 5.86145544294642, + -12.92096931784711, + 8.159367898576159, + -0.071584973281401, + -0.028269050394068383, + 0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774, + ]; + const B: &'a [f64] = &[ + 0.09646076681806523, + 0.01, + 0.4798896504144996, + 1.379008574103742, + -3.290069515436081, + 2.324710524099774, + 0.0, + + -0.001780011052226, + -0.000816434459657, + 0.007880878010262, + -0.144711007173263, + 0.582357165452555, + -0.458082105929187, + 1.0 / 66.0, + ]; + const C: &'a [f64] = &[ + 0.0, + 0.161, + 0.327, + 0.9, + 0.9800255409045097, + 1.0, + 1.0, + ]; +} + +impl Integrator for Tsit5 +where + Tsit5: TsitIntegrator, +{ + const ORDER: usize = 5; + const STAGES: usize = 7; + const ADAPTIVE: bool = true; + const DENSE: bool = true; + + fn step

(&self, ode: &ODE, h: f64) -> (SVector, Option, Option>>) { + let mut k: Vec> = vec![SVector::::zeros(); Self::STAGES]; + let mut next_y = ode.y.clone(); + let mut err = SVector::::zeros(); + // Do the first of the summations + k[0] = (ode.f)(ode.t, ode.y, &ode.params); + next_y += k[0] * Self::B[0] * h; + err += k[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 += k[j] * Self::A[( i * (i - 1) ) / 2 + j]; + } + k[i] = (ode.f)(ode.t + Self::C[i] * h, ode.y + y_term * h, &ode.params); + + // Use that and bis to calculate the y and error terms + next_y += k[i] * h * Self::B[i]; + err += k[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()), Some(k)) + } + fn interpolate(&self, t_start: f64, t_end: f64, dense: &Vec>, t: f64) -> SVector { + let s = (t - t_start)/(t_end - t_start); + let hn = t_end - t_start; + let b = vec![ + -1.0530884977290216 * s * (s - 1.3299890189751412) * (s * s - 1.4364028541716351 * s + 0.7139816917074209), + 0.1017 * s * s * (s * s - 2.1966568338249754 * s + 1.2949852507374631), + 2.490627285651252793 * s * s * (s * s - 2.38535645472061657 * s + 1.57803468208092486), + -16.54810288924490272 * (s - 1.21712927295533244) * (s - 0.61620406037800089) * s * s, + 47.37952196281928122 * (s - 1.203071208372362603) * (s - 0.658047292653547382) * s * s, + -34.87065786149660974 * (s - 1.2) * (s - 0.666666666666666667) * s * s, + 2.5 * (s - 1.0) * (s - 0.6) * s * s, + ]; + let mut sum = SVector::::zeros(); + b.into_iter().zip(dense).for_each(|(bi, fi)| { + sum += fi * bi; + }); + dense[0] + sum * hn + } +}