feat: use Runge Kutta ODE solver

3 files changed, 38 insertions, 19 deletions

diff --git a/physics/src/solver/midpoint.rs b/physics/src/solver/midpoint.rs
deleted file mode 100644
index 2d71758..0000000
--- a/physics/src/solver/midpoint.rs
+++ /dev/null
@@ -1,18 +0,0 @@
-use crate::{algebra::Scalar, particle_system::ParticleSystem};
-use super::{PhaseSpace, Solver};
-
-impl Solver for ParticleSystem {
-    fn step(&mut self, dt: Scalar) {
-        let mut state = self.collect_phase_space();
-
-        // Shift to the midpoint
-        self.scatter_phase_space(&PhaseSpace {
-            0: state.0.clone() + self.compute_derivative().0 * dt / 2.0,
-        });
-
-        state.0 += self.compute_derivative().0 * dt;
-        self.scatter_phase_space(&state);
-
-        self.t += dt;
-    }
-}
diff --git a/physics/src/solver/mod.rs b/physics/src/solver/mod.rs
index 726dcae..527bb06 100644
--- a/physics/src/solver/mod.rs
+++ b/physics/src/solver/mod.rs
@@ -2,7 +2,7 @@ use crate::particle_system::ParticleSystem;
 use crate::algebra::{Point, Scalar, Vector, N};
 use nalgebra::{Const, DVector, Dyn, Matrix, ViewStorage};
 
-mod midpoint;
+mod rk4;
 
 /// A vector of concatenated position and velocity components of each particle
 #[derive(Debug, Clone)]
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;
+    }
+}