40 lines
876 B
Rust
40 lines
876 B
Rust
use crate::model::{Model, Settings};
|
|
|
|
use nalgebra::{base::*, ComplexField};
|
|
|
|
/// Matrix `<Type, Rows, Columns>`
|
|
pub type Mat<T, const R: usize, const C: usize> =
|
|
Matrix<T, Const<R>, Const<C>, ArrayStorage<T, R, C>>;
|
|
/// Vector `<Type, Dimension>` (column matrix)
|
|
pub type Vect<T, const R: usize> = Matrix<T, Const<R>, U1, ArrayStorage<T, R, 1>>;
|
|
|
|
pub fn newton<
|
|
T: Copy + Scalar + ComplexField<RealField = T> + PartialOrd,
|
|
S: Settings,
|
|
const D: usize,
|
|
>(
|
|
model: &impl Model<T, S, D>,
|
|
x0: Vect<T, D>,
|
|
dt: T,
|
|
tol: T,
|
|
niters: usize,
|
|
) -> Vect<T, D>
|
|
where
|
|
Const<D>: ToTypenum + DimMin<Const<D>, Output = Const<D>>,
|
|
{
|
|
let mut x = x0;
|
|
|
|
for _ in 0..niters {
|
|
if let Some(m) = (Mat::<T, D, D>::identity() - model.df(x) * dt).try_inverse() {
|
|
let dx = m * (x - x0 - model.f(x) * dt);
|
|
if dx.norm() < tol {
|
|
break;
|
|
}
|
|
x -= dx;
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
x
|
|
}
|