sciforge_lib/maths/vector/
integrators.rs

1use super::types::Vec3;
2
3pub fn euler_step(pos: &Vec3, vel: &Vec3, accel: &Vec3, dt: f64) -> (Vec3, Vec3) {
4    let new_vel = &vel.clone() + &accel.scale(dt);
5    let new_pos = &pos.clone() + &new_vel.scale(dt);
6    (new_pos, new_vel)
7}
8
9pub fn verlet_step(
10    pos: &Vec3,
11    vel: &Vec3,
12    accel_fn: impl Fn(&Vec3) -> Vec3,
13    dt: f64,
14) -> (Vec3, Vec3) {
15    let a = accel_fn(pos);
16    let new_pos = &(&pos.clone() + &vel.scale(dt)) + &a.scale(0.5 * dt * dt);
17    let a_new = accel_fn(&new_pos);
18    let new_vel = &vel.clone() + &(&a + &a_new).scale(0.5 * dt);
19    (new_pos, new_vel)
20}
21
22pub fn rk4_step(y: &Vec3, dydt: impl Fn(&Vec3) -> Vec3, dt: f64) -> Vec3 {
23    let k1 = dydt(y);
24    let k2 = dydt(&(&y.clone() + &k1.scale(dt * 0.5)));
25    let k3 = dydt(&(&y.clone() + &k2.scale(dt * 0.5)));
26    let k4 = dydt(&(&y.clone() + &k3.scale(dt)));
27    let sum = &(&(&k1 + &k2.scale(2.0)) + &k3.scale(2.0)) + &k4;
28    &y.clone() + &sum.scale(dt / 6.0)
29}
30
31pub fn leapfrog_step(
32    pos: &Vec3,
33    vel_half: &Vec3,
34    accel_fn: impl Fn(&Vec3) -> Vec3,
35    dt: f64,
36) -> (Vec3, Vec3) {
37    let new_pos = &pos.clone() + &vel_half.scale(dt);
38    let a = accel_fn(&new_pos);
39    let new_vel_half = &vel_half.clone() + &a.scale(dt);
40    (new_pos, new_vel_half)
41}
42
43pub fn symplectic_euler_step(
44    pos: &Vec3,
45    vel: &Vec3,
46    accel_fn: impl Fn(&Vec3) -> Vec3,
47    dt: f64,
48) -> (Vec3, Vec3) {
49    let a = accel_fn(pos);
50    let new_vel = &vel.clone() + &a.scale(dt);
51    let new_pos = &pos.clone() + &new_vel.scale(dt);
52    (new_pos, new_vel)
53}
54
55pub fn yoshida4_step(
56    pos: &Vec3,
57    vel: &Vec3,
58    accel_fn: impl Fn(&Vec3) -> Vec3,
59    dt: f64,
60) -> (Vec3, Vec3) {
61    let w1 = 1.0 / (2.0 - 2.0_f64.powf(1.0 / 3.0));
62    let w0 = -2.0_f64.powf(1.0 / 3.0) * w1;
63    let c = [w1 / 2.0, (w0 + w1) / 2.0, (w0 + w1) / 2.0, w1 / 2.0];
64    let d = [w1, w0, w1];
65    let mut p = pos.clone();
66    let mut v = vel.clone();
67    p = &p + &v.scale(c[0] * dt);
68    let a0 = accel_fn(&p);
69    v = &v + &a0.scale(d[0] * dt);
70    p = &p + &v.scale(c[1] * dt);
71    let a1 = accel_fn(&p);
72    v = &v + &a1.scale(d[1] * dt);
73    p = &p + &v.scale(c[2] * dt);
74    let a2 = accel_fn(&p);
75    v = &v + &a2.scale(d[2] * dt);
76    p = &p + &v.scale(c[3] * dt);
77    (p, v)
78}
79
80pub fn rk4_vec3_step(
81    pos: &Vec3,
82    vel: &Vec3,
83    accel_fn: impl Fn(&Vec3, &Vec3) -> Vec3,
84    dt: f64,
85) -> (Vec3, Vec3) {
86    let k1v = accel_fn(pos, vel);
87    let k1x = vel.clone();
88
89    let p2 = &pos.clone() + &k1x.scale(0.5 * dt);
90    let v2 = &vel.clone() + &k1v.scale(0.5 * dt);
91    let k2v = accel_fn(&p2, &v2);
92    let k2x = v2.clone();
93
94    let p3 = &pos.clone() + &k2x.scale(0.5 * dt);
95    let v3 = &vel.clone() + &k2v.scale(0.5 * dt);
96    let k3v = accel_fn(&p3, &v3);
97    let k3x = v3.clone();
98
99    let p4 = &pos.clone() + &k3x.scale(dt);
100    let v4 = &vel.clone() + &k3v.scale(dt);
101    let k4v = accel_fn(&p4, &v4);
102    let k4x = v4;
103
104    let dx = &(&(&k1x + &k2x.scale(2.0)) + &k3x.scale(2.0)) + &k4x;
105    let dv = &(&(&k1v + &k2v.scale(2.0)) + &k3v.scale(2.0)) + &k4v;
106
107    (
108        &pos.clone() + &dx.scale(dt / 6.0),
109        &vel.clone() + &dv.scale(dt / 6.0),
110    )
111}
112
113pub fn stormer_verlet_step(
114    pos: &Vec3,
115    pos_prev: &Vec3,
116    accel_fn: impl Fn(&Vec3) -> Vec3,
117    dt: f64,
118) -> Vec3 {
119    let a = accel_fn(pos);
120    &(&pos.scale(2.0) - pos_prev) + &a.scale(dt * dt)
121}
122
123pub fn forest_ruth_step(
124    pos: &Vec3,
125    vel: &Vec3,
126    accel_fn: impl Fn(&Vec3) -> Vec3,
127    dt: f64,
128) -> (Vec3, Vec3) {
129    let theta = 1.0 / (2.0 - 2.0_f64.powf(1.0 / 3.0));
130    let mut p = &pos.clone() + &vel.scale(theta * dt / 2.0);
131    let a1 = accel_fn(&p);
132    let mut v = &vel.clone() + &a1.scale(theta * dt);
133    p = &p + &v.scale((1.0 - theta) * dt / 2.0);
134    let a2 = accel_fn(&p);
135    v = &v + &a2.scale((1.0 - 2.0 * theta) * dt);
136    p = &p + &v.scale((1.0 - theta) * dt / 2.0);
137    let a3 = accel_fn(&p);
138    v = &v + &a3.scale(theta * dt);
139    p = &p + &v.scale(theta * dt / 2.0);
140    (p, v)
141}
142
143pub fn adaptive_verlet(
144    pos: &Vec3,
145    vel: &Vec3,
146    accel_fn: impl Fn(&Vec3) -> Vec3,
147    dt: f64,
148    tol: f64,
149) -> (Vec3, Vec3, f64) {
150    let (p1, v1) = verlet_step(pos, vel, &accel_fn, dt);
151    let half_dt = dt * 0.5;
152    let (p_mid, v_mid) = verlet_step(pos, vel, &accel_fn, half_dt);
153    let (p2, v2) = verlet_step(&p_mid, &v_mid, &accel_fn, half_dt);
154    let err = (&p2 - &p1).magnitude();
155    let new_dt = if err > 1e-30 {
156        dt * (tol / err).powf(1.0 / 3.0) * 0.9
157    } else {
158        dt * 2.0
159    };
160    if err < tol {
161        (p2, v2, new_dt)
162    } else {
163        (p1, v1, new_dt)
164    }
165}
sciforge_lib/maths/vector/integrators.rs

sciforge_lib/maths/vector/
integrators.rs
