forked from Axect/Peroxide
-
Notifications
You must be signed in to change notification settings - Fork 0
/
newton.rs
35 lines (30 loc) · 1004 Bytes
/
newton.rs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
use crate::numerical::utils::jacobian;
use crate::structure::matrix::*;
use crate::structure::ad::*;
use crate::traits::{
math::{Norm, Normed, Vector},
mutable::MutFP,
};
/// Newton-Raphson Method
pub fn newton<F: Fn(&Vec<AD>) -> Vec<AD> + Copy>(init_cond: Vec<f64>, f: F, rtol: f64) -> Vec<f64>
{
let mut x_next = init_cond;
let mut x = x_next.clone();
update(&mut x_next, f);
let mut err = x_next.sub_vec(&x).norm(Norm::L2);
while err >= rtol {
x = x_next.clone();
update(&mut x_next, f);
err = x_next.sub_vec(&x).norm(Norm::L2);
}
x_next
}
fn update<F: Fn(&Vec<AD>) -> Vec<AD> + Copy>(xs: &mut Vec<f64>, f: F)
{
let j = jacobian(f, &xs);
let pinv_j = j.pseudo_inv();
//let fx = f(NumberVector::from_f64_vec(xs.clone())).to_f64_vec();
let xs_ad: Vec<AD> = xs.iter().map(|&t| AD::from(t)).collect();
let fx: Vec<f64> = f(&xs_ad).iter().map(|&t| t.into()).collect();
xs.mut_zip_with(|x, y| x - y, &(pinv_j * fx))
}