From 8e9f3db316e2cc3fd645cb81727a5d610aa83983 Mon Sep 17 00:00:00 2001
From: Cory <cory@balaton.dev>
Date: Fri, 29 Sep 2023 14:36:53 +0200
Subject: [PATCH] Implement force_on_particle

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

diff --git a/src/PenningTrap.cpp b/src/PenningTrap.cpp
index b445f21..538bb4c 100644
--- a/src/PenningTrap.cpp
+++ b/src/PenningTrap.cpp
@@ -21,6 +21,8 @@
  * */
 
 #include "PenningTrap.hpp"
+#include <algorithm>
+#include <stdexcept>
 
 PenningTrap::PenningTrap(double B_0, double V_0, double d)
 {
@@ -59,7 +61,17 @@ arma::vec PenningTrap::external_B_field(arma::vec r)
 
 arma::vec PenningTrap::force_on_particle(int i, int j)
 {
+    // Calculate the difference between the particles' position
+    arma::vec::fixed<3> res = this->particles.at(i).r_vec 
+        - this->particles.at(j).r_vec;
 
+    // Get the distance between the particles
+    double norm = arma::norm(res);
+
+    // Multiply res with p_j's charge divided by the norm cubed
+    res *= this->particles.at(j).q/(norm*norm*norm);
+
+    return res;
 }
 
 arma::vec PenningTrap::total_force_external(int i)