Adaptive stepsize seems to be working

src/controller.rs

@@ -0,0 +1,90 @@
+use super::ode::SystemTrait;
+use num_traits::Float;
+
+pub trait Controller<T,F, const D: usize>
+where
+    f64: From<T>,
+    F: SystemTrait<T,D>,
+    T: Copy + From<f64>,
+{
+    fn determine_step(&mut self, h: T, err: T) -> (bool, T);
+}
+
+#[derive(Debug, Clone, Copy)]
+pub struct PIController<T> where f64: From<T> {
+    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,
+}
+
+impl<T,F,const D:usize> Controller<T,F,D> for PIController<T>
+where
+    f64: From<T>,
+    F: SystemTrait<T,D>,
+    T: Copy + From<f64> + Float,
+{
+    /// 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) {
+        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() {
+            // Accept the stepsize
+            self.factor_old = err.max(1.0e-4.into());
+            if h_new.abs() > self.h_max {
+                // If the step is too big
+                h_new = self.h_max.copysign(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));
+            (false, h_new)
+        }
+    }
+}
+
+impl<T> PIController<T>
+where
+    f64: From<T>,
+    T: Copy + From<f64> + Float,
+{
+    pub fn new(alpha:T, beta:T, max_factor: T, min_factor: T, h_max: T, safety_factor: T, initial_h: T) -> Self {
+        Self {
+            alpha: alpha,
+            beta: beta,
+            factor_c1: <T as From<T>>::from(1.0.into()) / min_factor,
+            factor_c2: <T as From<T>>::from(1.0.into()) / max_factor,
+            factor_old: 1.0e-4.into(),
+            h_max: h_max.abs(),
+            safety_factor: safety_factor,
+            old_h: initial_h,
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_controller_creation() {
+        let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4);
+
+        assert!(controller.alpha == 0.17);
+        assert!(controller.beta == 0.04);
+        assert!(controller.factor_c1 == 1.0/0.2);
+        assert!(controller.factor_c2 == 1.0/10.0);
+        assert!(controller.factor_old == 1.0e-4);
+        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));
+    }
+}