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
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
use structure::matrix::*;
use structure::vector::*;
use structure::dual::*;
use numerical::utils::jacobian;
use util::non_macro::{concat, eye, zeros, cat};
const SQRT3: f64 = 1.7320508075688772;
const GL4: [[f64; 3]; 2] = [
[0.5 - SQRT3/6f64, 0.25, 0.25 - SQRT3/6f64],
[0.5 + SQRT3/6f64, 0.25 + SQRT3/6f64, 0.25]
];
pub fn gl4<F>(f: F, t_init: f64, y_init: Vec<f64>, h: f64, rtol: f64, num: usize) -> Matrix
where F: Fn(Dual, Vec<Dual>) -> Vec<Dual> + Copy
{
let mut t = t_init;
let mut y_curr = y_init.clone();
let mut records = zeros(num + 1, y_curr.len() + 1);
records.subs_row(0, cat(t, y_curr.clone()));
for i in 0 .. num {
let (k1, k2) = k_newton(f, t, y_curr.clone(), h, rtol);
y_curr = y_curr.add(&k1.fmap(|x| 0.5 * x * h).add(&k2.fmap(|x| 0.5 * x * h)));
t += h;
records.subs_row(i+1, cat(t, y_curr.clone()))
}
records
}
#[allow(non_snake_case)]
pub fn k_newton<F>(f: F, t: f64, y: Vec<f64>, h: f64, rtol: f64) -> (Vec<f64>, Vec<f64>)
where F: Fn(Dual, Vec<Dual>) -> Vec<Dual> + Copy,
{
let t1 = dual(t + GL4[0][0] * h, 0.);
let t2 = dual(t + GL4[1][0] * h, 0.);
let tn = dual(t, 0.);
let yn = y.conv_dual();
let n = y.len();
let k1_init = f(tn, yn.clone()).values();
let k2_init = f(tn, yn.clone()).values();
let mut k_curr = concat(k1_init.clone(), k2_init.clone());
let mut err = 1f64;
let g = |k: Vec<Dual>| -> Vec<Dual> {
let k1 = k.take(n);
let k2 = k.skip(n);
concat(
f(t1, yn.add(&k1.fmap(|x| x * GL4[0][1] * h).add(&k2.fmap(|x| x * GL4[0][2] * h)))),
f(t2, yn.add(&k1.fmap(|x| x * GL4[1][1] * h).add(&k2.fmap(|x| x * GL4[1][2] * h))))
)
};
let I = eye(2*n);
let mut Dg = jacobian(k_curr.clone(), g.clone());
let mut DG = I.clone() - Dg.clone();
let mut DG_inv = DG.inv().unwrap();
let mut G = k_curr.sub(&g.clone()(k_curr.conv_dual()).values());
let mut num_iter: usize = 0;
while err >= rtol && num_iter <= 10 {
let k_prev = k_curr.clone();
let DGG = DG_inv.clone() % G.clone();
k_curr = k_curr.sub(&DGG.col(0));
Dg = jacobian(k_curr.clone(), g.clone());
DG = I.clone() - Dg.clone();
DG_inv = DG.inv().unwrap();
G = k_curr.sub(&g.clone()(k_curr.conv_dual()).values());
err = k_curr.sub(&k_prev).norm();
num_iter += 1;
}
(k_curr.take(n), k_curr.skip(n))
}