From b5acec572ffb15cdaf0db1c45c85355d2f8c98ac Mon Sep 17 00:00:00 2001
From: Cory <cory@balaton.dev>
Date: Sun, 8 Oct 2023 00:35:52 +0200
Subject: [PATCH] Implement RK4 method

---
 src/PenningTrap.cpp | 59 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/src/PenningTrap.cpp b/src/PenningTrap.cpp
index b4b2dd8..5d9a16b 100644
--- a/src/PenningTrap.cpp
+++ b/src/PenningTrap.cpp
@@ -103,6 +103,65 @@ arma::vec PenningTrap::total_force(int i)
 
 void PenningTrap::evolve_RK4(double dt)
 {
+    std::vector<Particle> tmp_particles = this->particles;
+    
+    arma::vec::fixed<3> *k_v = new arma::vec::fixed<3>[this->particles.size()*4];
+    arma::vec::fixed<3> *k_r = new arma::vec::fixed<3>[this->particles.size()*4];
+
+    int size = this->particles.size();
+
+    for (int i=0; i<size; i++) {
+        k_v[i] = this->total_force(i)/this->particles.at(i).m;
+        k_r[i] = this->particles.at(i).v_vec;
+    }
+
+    for (int i=0; i<size; i++) {
+        Particle *p = &this->particles.at(i);
+
+        p->v_vec = tmp_particles.at(i).v_vec + (dt/2)*k_v[i];
+        p->r_vec = tmp_particles.at(i).r_vec + (dt/2)*k_r[i];
+    }
+
+
+    for (int i=0; i<size; i++) {
+        k_v[1*size + i] = this->total_force(i)/this->particles.at(i).m;
+        k_r[1*size + i] = this->particles.at(i).v_vec;
+    }
+
+    for (int i=0; i<size; i++) {
+        Particle *p = &this->particles.at(i);
+
+        p->v_vec = tmp_particles.at(i).v_vec + (dt/2)*k_v[1*size + i];
+        p->r_vec = tmp_particles.at(i).r_vec + (dt/2)*k_r[1*size + i];
+    }
+
+    for (int i=0; i<size; i++) {
+        k_v[2*size + i] = this->total_force(i)/this->particles.at(i).m;
+        k_r[2*size + i] = this->particles.at(i).v_vec;
+    }
+
+    for (int i=0; i<size; i++) {
+        Particle *p = &this->particles.at(i);
+
+        p->v_vec = tmp_particles.at(i).v_vec + dt*k_v[2*size + i];
+        p->r_vec = tmp_particles.at(i).r_vec + dt*k_r[2*size + i];
+    }
+
+
+    for (int i=0; i<size; i++) {
+        k_v[3*size + i] = this->total_force(i)/this->particles.at(i).m;
+        k_r[3*size + i] = this->particles.at(i).v_vec;
+    }
+
+    for (int i=0; i<size; i++) {
+        Particle *p = &this->particles.at(i);
+
+        p->v_vec = tmp_particles.at(i).v_vec + dt*(k_v[i] + k_v[size + i] + k_v[2*size + i] + k_v[3*size + i])/6;
+        p->r_vec = tmp_particles.at(i).r_vec + dt*(k_r[i] + k_r[size + i] + k_r[2*size + i] + k_r[3*size + i])/6;
+    }
+
+    delete [] k_v;
+    delete [] k_r;
 }
 
 void PenningTrap::evolve_forward_euler(double dt)