use crate::{
error::Error,
traits::{Real, State},
};
pub trait Interpolation<T, Y>
where
T: Real,
Y: State<T>,
{
fn interpolate(&mut self, t_interp: T) -> Result<Y, Error<T, Y>>;
}
pub fn cubic_hermite_interpolate<T: Real, Y: State<T>>(
t0: T,
t1: T,
y0: &Y,
y1: &Y,
k0: &Y,
k1: &Y,
t: T,
) -> Y {
let two = T::from_f64(2.0).unwrap();
let three = T::from_f64(3.0).unwrap();
let h = t1 - t0;
let s = (t - t0) / h;
let h00 = two * s.powi(3) - three * s.powi(2) + T::one();
let h10 = s.powi(3) - two * s.powi(2) + s;
let h01 = -two * s.powi(3) + three * s.powi(2);
let h11 = s.powi(3) - s.powi(2);
*y0 * h00 + *k0 * h10 * h + *y1 * h01 + *k1 * h11 * h
}
pub fn linear_interpolate<T: Real, Y: State<T>>(t0: T, t1: T, y0: &Y, y1: &Y, t: T) -> Y {
let s = (t - t0) / (t1 - t0);
*y0 * (T::one() - s) + *y1 * s
}