1 files changed, 25 insertions, 0 deletions
diff --git a/src/Math/Interpolation.cpp b/src/Math/Interpolation.cpp
new file mode 100644
index 0000000..a1e6b69
--- /dev/null
+++ b/src/Math/Interpolation.cpp
@@ -0,0 +1,25 @@
+#include "Interpolation.hpp"
+#include <functional>
+#include "Common.hpp"
+
+namespace Math {
+
+float linear_interpolation(Vector<2> val, float left, float right, float pos) {
+    return val.x() + (pos - left) * (val.y() - val.x()) / (right - left);
+}
+
+float bilinear_interpolation(Matrix<2, 2> val, GridCellBoundaries cell, Vector<2> pos) {
+    auto r1 = linear_interpolation(val.row(0), cell.x1, cell.x2, pos.x());
+    auto r2 = linear_interpolation(val.row(1), cell.x1, cell.x2, pos.x());
+
+    return linear_interpolation({r1, r2}, cell.y1, cell.y2, pos.y());
+}
+
+float trilinear_interpolation(Matrix<2, 2> val_front, Matrix<2, 2> val_back, CubeCellBoundaries cell, Vector<3> pos) {
+    auto r1 = bilinear_interpolation(val_front, cell.grid_cell(), {pos.x(), pos.y()});
+    auto r2 = bilinear_interpolation(val_back, cell.grid_cell(), {pos.x(), pos.y()});
+
+    return linear_interpolation({r1, r2}, cell.z1, cell.z2, pos.z());
+}
+
+}
\ No newline at end of file