fmi_sim/sim/solver/
euler.rs

1use super::{Model, Solver, SolverError};
2
3pub struct Euler {
4    /// Current time
5    time: f64,
6    /// Continuous states
7    x: Vec<f64>,
8    /// Derivatives of continuous states
9    dx: Vec<f64>,
10    /// Event indicators
11    z: Vec<f64>,
12    /// Previous event indicators
13    prez: Vec<f64>,
14}
15
16impl<M: Model> Solver<M> for Euler {
17    type Params = ();
18
19    fn new(start_time: f64, _tol: f64, nx: usize, nz: usize, _solver_params: Self::Params) -> Self {
20        Self {
21            time: start_time,
22            x: vec![0.0; nx],
23            dx: vec![0.0; nx],
24            z: vec![0.0; nz],
25            prez: vec![0.0; nz],
26        }
27    }
28
29    fn step(&mut self, model: &mut M, next_time: f64) -> Result<(f64, bool), SolverError> {
30        let dt = next_time - self.time;
31
32        if !self.x.is_empty() {
33            model.get_continuous_states(&mut self.x);
34            model.get_continuous_state_derivatives(&mut self.dx);
35
36            for i in 0..self.x.len() {
37                self.x[i] += self.dx[i] * dt;
38            }
39
40            model.set_continuous_states(&self.x);
41        }
42
43        let mut state_event = false;
44
45        if !self.z.is_empty() {
46            model.get_event_indicators(&mut self.z);
47
48            self.z
49                .iter()
50                .zip(self.prez.iter_mut())
51                .for_each(|(z, prez)| {
52                    // crossed zero going positive
53                    let cross_pos = *prez <= 0.0 && *z > 0.0;
54                    // crossed zero going negative
55                    let cross_neg = *prez > 0.0 && *z <= 0.0;
56                    state_event = cross_pos || cross_neg || state_event;
57                    *prez = *z;
58                });
59        }
60        self.time = next_time;
61
62        Ok((self.time, state_event))
63    }
64
65    fn reset(&mut self, model: &mut M, _time: f64) -> Result<(), SolverError> {
66        if !self.z.is_empty() {
67            model.get_event_indicators(&mut self.z);
68        }
69        Ok(())
70    }
71}
72
73#[cfg(test)]
74mod tests {
75    use super::*;
76
77    struct SimpleModel;
78
79    impl Model for SimpleModel {
80        fn get_continuous_states(&mut self, x: &mut [f64]) {
81            x[0] = 0.0;
82        }
83
84        fn set_continuous_states(&mut self, states: &[f64]) {
85            assert_eq!(states[0], 1.0);
86        }
87
88        fn get_continuous_state_derivatives(&mut self, dx: &mut [f64]) {
89            dx[0] = 1.0;
90        }
91
92        fn get_event_indicators(&mut self, z: &mut [f64]) {
93            z[0] = 0.0;
94        }
95    }
96
97    #[test]
98    fn test_euler() {
99        let mut euler = <Euler as Solver<SimpleModel>>::new(0.0, 1e-6, 1, 1, ());
100        let (time, state_event) = euler.step(&mut SimpleModel, 1.0).unwrap();
101        assert_eq!(time, 1.0);
102        assert!(!state_event);
103    }
104}
fmi_sim/sim/solver/euler.rs

fmi_sim/sim/solver/
euler.rs
