Skip to main content

qualia_core_db/solvers/calculus/
ode_adaptive.rs

1//! Caller-buffered adaptive Dormand-Prince ODE integration.
2
3#[derive(Debug, Clone, Copy, PartialEq)]
4pub enum OdeError {
5    InvalidDomain,
6    DimensionMismatch,
7    WorkspaceTooSmall { required: usize, available: usize },
8    NonFiniteDerivative,
9    StepUnderflow,
10    StepLimitExceeded,
11}
12
13#[repr(C)]
14#[derive(Debug, Clone, Copy, PartialEq)]
15pub struct AdaptiveOdeConfig {
16    pub absolute_tolerance: f64,
17    pub relative_tolerance: f64,
18    pub initial_step: f64,
19    pub minimum_step: f64,
20    pub maximum_step: f64,
21    pub max_steps: u32,
22}
23
24impl Default for AdaptiveOdeConfig {
25    fn default() -> Self {
26        Self {
27            absolute_tolerance: 1e-9,
28            relative_tolerance: 1e-7,
29            initial_step: 1e-3,
30            minimum_step: 1e-14,
31            maximum_step: 1.0,
32            max_steps: 100_000,
33        }
34    }
35}
36
37#[repr(C)]
38#[derive(Debug, Clone, Copy, PartialEq)]
39pub struct AdaptiveOdeResult {
40    pub final_time: f64,
41    pub accepted_steps: u32,
42    pub rejected_steps: u32,
43    pub derivative_evaluations: u32,
44    pub last_error_norm: f64,
45    pub last_step: f64,
46}
47
48/// `Default::dopri5_workspace_len`.
49pub const fn dopri5_workspace_len(dimension: usize) -> Option<usize> {
50    dimension.checked_mul(8)
51}
52
53/// Dormand-Prince 5 adaptive integration.
54pub fn integrate_dopri5<F>(
55    derivative: F,
56    state: &mut [f64],
57    t0: f64,
58    t_final: f64,
59    config: AdaptiveOdeConfig,
60    workspace: &mut [f64],
61) -> Result<AdaptiveOdeResult, OdeError>
62where
63    F: Fn(f64, &[f64], &mut [f64]) -> Result<(), OdeError>,
64{
65    let dimension = state.len();
66    let required = dopri5_workspace_len(dimension).ok_or(OdeError::InvalidDomain)?;
67    if dimension == 0
68        || !t0.is_finite()
69        || !t_final.is_finite()
70        || t_final < t0
71        || state.iter().any(|value| !value.is_finite())
72        || !valid_config(config)
73    {
74        return Err(OdeError::InvalidDomain);
75    }
76    if workspace.len() < required {
77        return Err(OdeError::WorkspaceTooSmall {
78            required,
79            available: workspace.len(),
80        });
81    }
82    if t_final == t0 {
83        return Ok(AdaptiveOdeResult {
84            final_time: t0,
85            accepted_steps: 0,
86            rejected_steps: 0,
87            derivative_evaluations: 0,
88            last_error_norm: 0.0,
89            last_step: 0.0,
90        });
91    }
92
93    let (k1, rest) = workspace.split_at_mut(dimension);
94    let (k2, rest) = rest.split_at_mut(dimension);
95    let (k3, rest) = rest.split_at_mut(dimension);
96    let (k4, rest) = rest.split_at_mut(dimension);
97    let (k5, rest) = rest.split_at_mut(dimension);
98    let (k6, rest) = rest.split_at_mut(dimension);
99    let (k7, rest) = rest.split_at_mut(dimension);
100    let temp = &mut rest[..dimension];
101
102    let mut time = t0;
103    let mut step = config.initial_step.min(config.maximum_step);
104    let mut accepted = 0;
105    let mut rejected = 0;
106    let mut evaluations = 0;
107    let mut last_error = f64::INFINITY;
108    let mut last_step = 0.0;
109
110    for _ in 0..config.max_steps {
111        if time >= t_final {
112            return Ok(AdaptiveOdeResult {
113                final_time: time,
114                accepted_steps: accepted,
115                rejected_steps: rejected,
116                derivative_evaluations: evaluations,
117                last_error_norm: last_error,
118                last_step,
119            });
120        }
121        step = step.min(t_final - time).min(config.maximum_step);
122        if step < config.minimum_step || time + step == time {
123            return Err(OdeError::StepUnderflow);
124        }
125
126        eval(&derivative, time, state, k1)?;
127        stage(state, temp, step, &[(1.0 / 5.0, k1)]);
128        eval(&derivative, time + step / 5.0, temp, k2)?;
129
130        stage(state, temp, step, &[(3.0 / 40.0, k1), (9.0 / 40.0, k2)]);
131        eval(&derivative, time + 3.0 * step / 10.0, temp, k3)?;
132
133        stage(
134            state,
135            temp,
136            step,
137            &[(44.0 / 45.0, k1), (-56.0 / 15.0, k2), (32.0 / 9.0, k3)],
138        );
139        eval(&derivative, time + 4.0 * step / 5.0, temp, k4)?;
140
141        stage(
142            state,
143            temp,
144            step,
145            &[
146                (19372.0 / 6561.0, k1),
147                (-25360.0 / 2187.0, k2),
148                (64448.0 / 6561.0, k3),
149                (-212.0 / 729.0, k4),
150            ],
151        );
152        eval(&derivative, time + 8.0 * step / 9.0, temp, k5)?;
153
154        stage(
155            state,
156            temp,
157            step,
158            &[
159                (9017.0 / 3168.0, k1),
160                (-355.0 / 33.0, k2),
161                (46732.0 / 5247.0, k3),
162                (49.0 / 176.0, k4),
163                (-5103.0 / 18656.0, k5),
164            ],
165        );
166        eval(&derivative, time + step, temp, k6)?;
167
168        stage(
169            state,
170            temp,
171            step,
172            &[
173                (35.0 / 384.0, k1),
174                (500.0 / 1113.0, k3),
175                (125.0 / 192.0, k4),
176                (-2187.0 / 6784.0, k5),
177                (11.0 / 84.0, k6),
178            ],
179        );
180        eval(&derivative, time + step, temp, k7)?;
181        evaluations += 7;
182
183        let mut error_norm = 0.0_f64;
184        for index in 0..dimension {
185            let fifth = state[index]
186                + step
187                    * (35.0 / 384.0 * k1[index]
188                        + 500.0 / 1113.0 * k3[index]
189                        + 125.0 / 192.0 * k4[index]
190                        - 2187.0 / 6784.0 * k5[index]
191                        + 11.0 / 84.0 * k6[index]);
192            let fourth = state[index]
193                + step
194                    * (5179.0 / 57600.0 * k1[index]
195                        + 7571.0 / 16695.0 * k3[index]
196                        + 393.0 / 640.0 * k4[index]
197                        - 92097.0 / 339200.0 * k5[index]
198                        + 187.0 / 2100.0 * k6[index]
199                        + 1.0 / 40.0 * k7[index]);
200            let scale = config.absolute_tolerance
201                + config.relative_tolerance * state[index].abs().max(fifth.abs());
202            error_norm = error_norm.max((fifth - fourth).abs() / scale);
203            temp[index] = fifth;
204        }
205        if !error_norm.is_finite() || temp.iter().any(|value| !value.is_finite()) {
206            return Err(OdeError::NonFiniteDerivative);
207        }
208
209        last_error = error_norm;
210        let factor = if error_norm == 0.0 {
211            5.0
212        } else {
213            (0.9 * error_norm.powf(-0.2)).clamp(0.2, 5.0)
214        };
215        if error_norm <= 1.0 {
216            state.copy_from_slice(temp);
217            time += step;
218            last_step = step;
219            accepted += 1;
220        } else {
221            rejected += 1;
222        }
223        step = (step * factor).clamp(config.minimum_step, config.maximum_step);
224    }
225
226    Err(OdeError::StepLimitExceeded)
227}
228
229fn valid_config(config: AdaptiveOdeConfig) -> bool {
230    config.absolute_tolerance.is_finite()
231        && config.absolute_tolerance > 0.0
232        && config.relative_tolerance.is_finite()
233        && config.relative_tolerance >= 0.0
234        && config.initial_step.is_finite()
235        && config.initial_step > 0.0
236        && config.minimum_step.is_finite()
237        && config.minimum_step > 0.0
238        && config.maximum_step.is_finite()
239        && config.maximum_step >= config.minimum_step
240        && config.max_steps > 0
241}
242
243fn eval<F>(derivative: &F, time: f64, state: &[f64], output: &mut [f64]) -> Result<(), OdeError>
244where
245    F: Fn(f64, &[f64], &mut [f64]) -> Result<(), OdeError>,
246{
247    derivative(time, state, output)?;
248    if output.len() != state.len() {
249        return Err(OdeError::DimensionMismatch);
250    }
251    if output.iter().any(|value| !value.is_finite()) {
252        return Err(OdeError::NonFiniteDerivative);
253    }
254    Ok(())
255}
256
257fn stage(state: &[f64], output: &mut [f64], step: f64, terms: &[(f64, &[f64])]) {
258    for index in 0..state.len() {
259        let mut value = state[index];
260        for (coefficient, derivative) in terms {
261            value += step * coefficient * derivative[index];
262        }
263        output[index] = value;
264    }
265}
266
267#[cfg(test)]
268mod tests {
269    use super::*;
270
271    fn decay(_time: f64, state: &[f64], output: &mut [f64]) -> Result<(), OdeError> {
272        if state.len() != output.len() {
273            return Err(OdeError::DimensionMismatch);
274        }
275        for (out, value) in output.iter_mut().zip(state) {
276            *out = -*value;
277        }
278        Ok(())
279    }
280
281    #[test]
282    fn dopri5_lands_exactly_at_final_time_with_tolerance_control() {
283        let mut state = [1.0, 2.0, 3.0];
284        let mut workspace = [0.0; 24];
285        let report = integrate_dopri5(
286            decay,
287            &mut state,
288            0.0,
289            2.0,
290            AdaptiveOdeConfig::default(),
291            &mut workspace,
292        )
293        .unwrap();
294        assert_eq!(report.final_time, 2.0);
295        for (index, value) in state.iter().enumerate() {
296            let expected = (index + 1) as f64 * (-2.0_f64).exp();
297            assert!((value - expected).abs() < 3e-8);
298        }
299        assert!(report.accepted_steps > 0);
300    }
301
302    #[test]
303    fn dopri5_rejects_workspace_and_non_finite_derivatives() {
304        let mut state = [1.0, 2.0];
305        let mut too_small = [0.0; 15];
306        assert_eq!(
307            integrate_dopri5(
308                decay,
309                &mut state,
310                0.0,
311                1.0,
312                AdaptiveOdeConfig::default(),
313                &mut too_small,
314            ),
315            Err(OdeError::WorkspaceTooSmall {
316                required: 16,
317                available: 15
318            })
319        );
320
321        let mut workspace = [0.0; 16];
322        assert_eq!(
323            integrate_dopri5(
324                |_t, _y, out| {
325                    out.fill(f64::NAN);
326                    Ok(())
327                },
328                &mut state,
329                0.0,
330                1.0,
331                AdaptiveOdeConfig::default(),
332                &mut workspace,
333            ),
334            Err(OdeError::NonFiniteDerivative)
335        );
336    }
337}