1 files changed, 37 insertions, 0 deletions
diff --git a/physics/src/solver/rk4.rs b/physics/src/solver/rk4.rs
new file mode 100644
index 0000000..224bec5
--- /dev/null
+++ b/physics/src/solver/rk4.rs
@@ -0,0 +1,37 @@
+use crate::{algebra::Scalar, particle_system::ParticleSystem};
+use super::{PhaseSpace, Solver};
+
+// Runge-Kutta 4th order
+impl Solver for ParticleSystem {
+    fn step(&mut self, dt: Scalar) {
+        let state = self.collect_phase_space();
+
+        // k1 = f(t, y)
+        let k1 = self.compute_derivative();
+
+        // k2 = f(t + dt/2, y + dt * k1 / 2)
+        self.scatter_phase_space(&PhaseSpace {
+            0: state.0.clone() + k1.0.clone() * dt / 2.0,
+        });
+        let k2 = self.compute_derivative();
+
+
+        // k3 = f(t + dt/2, y + dt * k2 / 2)
+        self.scatter_phase_space(&PhaseSpace {
+            0: state.0.clone() + k2.0.clone() * dt / 2.0,
+        });
+        let k3 = self.compute_derivative();
+
+        // k4 = f(t + dt, y + dt * k3)
+        self.scatter_phase_space(&PhaseSpace {
+            0: state.0.clone() + k3.0.clone() * dt
+        });
+        let k4 = self.compute_derivative();
+
+        self.scatter_phase_space(&PhaseSpace {
+            0: state.0.clone() + (k1.0 + 2.0 * k2.0 + 2.0 * k3.0 + k4.0) * dt / 6.0
+        });
+
+        self.t += dt;
+    }
+}