use super::Func1;
use parry2d_f64::na::DMatrix;
pub type Quadratic = Polynomial<3>;
pub type Cubic = Polynomial<4>;
pub type Quartic = Polynomial<5>;
pub type Quintic = Polynomial<6>;
#[derive(Debug, Clone)]
pub struct Polynomial<const K: usize> {
pub c: [f64; K],
}
impl<const K: usize> Polynomial<K> {
pub fn new(c: [f64; K]) -> Self {
Self { c }
}
pub fn least_squares(xs: &[f64], ys: &[f64], weights: Option<&[f64]>) -> Option<Self> {
assert_eq!(xs.len(), ys.len());
let w = Weights::new(weights);
let mut sums = vec![0.0; 2 * K + 1];
let mut rhs: DMatrix<f64> = DMatrix::zeros(K, 1);
for i in 0..xs.len() {
let w = w.get(i);
for k in 0..K {
let wxk = w * xs[i].powi(k as i32);
rhs[(k, 0)] += wxk * ys[i];
sums[k] += wxk;
}
for (k, sums_k) in sums.iter_mut().enumerate().take(2 * K + 1).skip(K + 1) {
*sums_k += w * xs[i].powi(k as i32);
}
}
let mut matrix = DMatrix::zeros(K, K);
for r in 0..K {
for c in 0..K {
matrix[(r, c)] = sums[r + c];
}
}
let p = matrix.try_inverse()? * rhs;
let mut c = [0.0; K];
for i in 0..K {
c[i] = p[(i, 0)];
}
Some(Self::new(c))
}
}
impl<const K: usize> Func1 for Polynomial<K> {
fn f(&self, x: f64) -> f64 {
let mut y = 0.0;
for i in 0..K {
y += self.c[i] * x.powi(i as i32);
}
y
}
}
struct Weights<'a> {
values: Option<&'a [f64]>,
}
impl<'a> Weights<'a> {
fn new(values: Option<&'a [f64]>) -> Self {
Self { values }
}
fn get(&self, i: usize) -> f64 {
if let Some(values) = self.values {
values[i]
} else {
1.0
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::DiscreteDomain;
use approx::assert_relative_eq;
#[test]
fn least_squares_quadratic() {
let origin = Quadratic::new([0.0, 0.0, 1.0]);
let x = DiscreteDomain::linear(-1.0, 1.0, 5);
let y = origin.fs(&x);
let fit = Quadratic::least_squares(&x, &y, None).unwrap();
assert_relative_eq!(origin.c[0], fit.c[0], epsilon = 1e-10);
assert_relative_eq!(origin.c[1], fit.c[1], epsilon = 1e-10);
assert_relative_eq!(origin.c[2], fit.c[2], epsilon = 1e-10);
}
#[test]
fn test_quartic() {
let origin = Quartic::new([5.0, -2.0, 1.0, 4.0, -3.0]);
let x = DiscreteDomain::linear(-1.0, 1.0, 1000);
let y = origin.fs(&x);
let fit = Quartic::least_squares(&x, &y, None).unwrap();
assert_relative_eq!(origin.c[0], fit.c[0], epsilon = 1e-10);
assert_relative_eq!(origin.c[1], fit.c[1], epsilon = 1e-10);
assert_relative_eq!(origin.c[2], fit.c[2], epsilon = 1e-10);
assert_relative_eq!(origin.c[3], fit.c[3], epsilon = 1e-10);
assert_relative_eq!(origin.c[4], fit.c[4], epsilon = 1e-10);
}
}