conspire/math/integrate/field/hermite/
mod.rs

1#[cfg(test)]
2mod test;
3
4use super::{Integrable, reconstruct_or_err};
5use crate::math::{Quantity, TensorVec, integrate::IntegrationError};
6use crate::units::Time;
7
8/// Cubic Hermite dense output over one accepted step, built in the field's Lie
9/// algebra rather than on the state itself.
10///
11/// The flat interpolant combines `y_{n}`, `y_{n+1}` and the two end rates
12/// affinely. That is meaningless for a group-valued state: the two states are
13/// different group elements and the two rates live in different tangent spaces,
14/// so the combination leaves the manifold (`det F_p` drifts) exactly the way the
15/// additive march did.
16///
17/// Anchor everything at the left endpoint instead. The step already produced
18/// `sigma` with `reconstruct(base, sigma) = y_{n+1}`, and the algebra is a flat
19/// vector space — so the Hermite polynomial is built *there*,
20///
21/// ```math
22/// \sigma(\theta) = h_{10}(\theta)\,\dot\sigma_0 + h_{01}(\theta)\,\sigma
23///                + h_{11}(\theta)\,\dot\sigma_1
24/// ,\qquad
25/// \mathbf{y}(\theta) = \mathrm{reconstruct}(\mathbf{y}_n, \sigma(\theta))
26/// ```
27///
28/// with `σ(0) = 0` and `σ(1) = sigma`, so both endpoints are reproduced exactly
29/// and every interior point is an `expm` of a trace-free element — on the group
30/// by construction. `slope_0` is the raw stage-0 slope (`dexpinv` at zero
31/// displacement is the identity); `slope_1` is the endpoint rate pulled back
32/// through [`Integrable::dexpinv`] at `sigma`, both already scaled by the
33/// step. On a [`super::Flat`] field `reconstruct` adds and `dexpinv` is the
34/// identity, and `h_{00} + h_{01} = 1` collapses this to the usual flat formula.
35pub struct HermiteSegment<Field: Integrable, T = Time> {
36    t_0: Quantity<T>,
37    h: Quantity<T>,
38    base: Field::Point,
39    sigma: Field::Increment,
40    slope_0: Field::Increment,
41    slope_1: Field::Increment,
42}
43
44impl<Field, T> HermiteSegment<Field, T>
45where
46    Field: Integrable,
47{
48    /// A segment of the accepted step `[t_0, t_0 + h]` from `base`, the algebra
49    /// displacement `sigma` over it, and the step-scaled algebra rates at its
50    /// two ends (`slope_1` already pulled back through
51    /// [`Integrable::dexpinv`] at `sigma`).
52    pub fn new(
53        t_0: Quantity<T>,
54        h: Quantity<T>,
55        base: Field::Point,
56        sigma: Field::Increment,
57        slope_0: Field::Increment,
58        slope_1: Field::Increment,
59    ) -> Self {
60        Self {
61            t_0,
62            h,
63            base,
64            sigma,
65            slope_0,
66            slope_1,
67        }
68    }
69    /// The state at `time`, reconstructed from the algebra Hermite polynomial.
70    pub fn evaluate(&self, time: Quantity<T>) -> Result<Field::Point, IntegrationError> {
71        let theta = (time - self.t_0).value() / self.h.value();
72        let theta_2 = theta * theta;
73        let theta_3 = theta_2 * theta;
74        let mut increment = self.slope_0.clone() * (theta_3 - 2.0 * theta_2 + theta);
75        increment += self.sigma.clone() * (3.0 * theta_2 - 2.0 * theta_3);
76        increment += self.slope_1.clone() * (theta_3 - theta_2);
77        reconstruct_or_err::<Field>(&self.base, &increment)
78    }
79}
80
81/// Evaluates `segments` at `time_k`, in the segment that contains it (the last
82/// one for a time past the final accepted step).
83pub(super) fn hermite_at<Field, T>(
84    segments: &[HermiteSegment<Field, T>],
85    time_k: Quantity<T>,
86) -> Result<Field::Point, IntegrationError>
87where
88    Field: Integrable,
89{
90    segments
91        .iter()
92        .find(|segment| time_k <= segment.t_0 + segment.h)
93        .unwrap_or(&segments[segments.len() - 1])
94        .evaluate(time_k)
95}
96
97/// Finds the containing segment and evaluates it there, over a whole grid of
98/// requested times.
99pub fn interpolate_hermite<Field, U, T>(
100    segments: &[HermiteSegment<Field, T>],
101    time: &[Quantity<T>],
102) -> Result<U, IntegrationError>
103where
104    Field: Integrable,
105    U: TensorVec<Item = Field::Point>,
106{
107    let mut points = U::new();
108    for time_k in time {
109        points.push(hermite_at::<Field, T>(segments, *time_k)?);
110    }
111    Ok(points)
112}
conspire/math/integrate/field/hermite/mod.rs

conspire/math/integrate/field/hermite/
mod.rs
