Adaptive stepsize seems to be working

src/problem.rs

@@ -0,0 +1,138 @@
+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::controller::{Controller, PIController};
+use super::integrator::Integrator;
+
+pub struct Problem<T, F, const D1: usize, const D2: usize, S>
+where
+    f64: From<T>,
+    F: SystemTrait<T,D1>,
+    T: Copy +
+       From<f64> +
+       identities::One +
+       identities::Zero +
+       PartialEq +
+       Debug +
+       Add +
+       AddAssign +
+       Sub +
+       SubAssign +
+       Mul +
+       MulAssign +
+       Float +
+       PartialOrd +
+       'static,
+    S: Integrator<T, F, D1, D2>,
+{
+    ode: ODE<T,F,D1>,
+    integrator: S,
+    controller: PIController<T>,
+}
+
+impl<T, F, const D1: usize, const D2: usize, S> Problem<T,F,D1,D2,S>
+where
+    f64: From<T>,
+    F: SystemTrait<T,D1>,
+    T: Copy +
+       From<f64> +
+       identities::One +
+       identities::Zero +
+       PartialEq +
+       Debug +
+       Add +
+       AddAssign +
+       Sub +
+       SubAssign +
+       Mul +
+       MulAssign +
+       Float +
+       PartialOrd +
+       'static,
+    S: Integrator<T, F, D1, D2>,
+{
+    pub fn new(ode: ODE<T,F,D1>, integrator: S, controller: PIController<T>) -> Self {
+        Problem {
+            ode: ode,
+            integrator: integrator,
+            controller: controller,
+        }
+    }
+    pub fn solve(&mut self) -> Solution<T, D1> {
+        let mut times: Vec::<T> = Vec::new();
+        let mut states: Vec::<SVector<T,D1>> = Vec::new();
+        let mut step: T = self.controller.old_h;
+        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
+                    let mut accepted: bool = false;
+                    while !accepted {
+                        (accepted, step) = <PIController<T> as Controller<T, F, D1>>::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.ode.y = new_y;
+                    self.ode.t += step;
+                },
+                None => {
+                    // Fixed Step Size
+                    self.ode.y = new_y;
+                    self.ode.t += self.controller.old_h;
+                },
+            };
+        }
+        Solution {
+            times: times,
+            states: states,
+        }
+    }
+}
+
+pub struct Solution<T,const D: usize> {
+    pub times: Vec<T>,
+    pub states: Vec<SVector<T,D>>,
+}
+
+#[cfg(test)]
+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};
+
+    #[test]
+    fn test_ode_creation() {
+        struct System {}
+
+        impl SystemTrait<f64,3> for System {
+            fn derivative(&self, _t: f64, y: Vector3<f64>) -> Vector3<f64> { 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 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 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);
+        })
+    }
+}