From 48a9ac9a2cb9df43d391f3a0dbe6948f26536afb Mon Sep 17 00:00:00 2001
From: Patrick Shriwise <pshriwise@gmail.com>
Date: Sat, 6 Aug 2022 06:48:47 -0500
Subject: [PATCH] Adding initial lagrangian interpolation.

---
 include/openmc/interpolate.h | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/include/openmc/interpolate.h b/include/openmc/interpolate.h
index b88ab2f2e7..0683140339 100644
--- a/include/openmc/interpolate.h
+++ b/include/openmc/interpolate.h
@@ -60,6 +60,31 @@ inline double interpolate(const std::vector<double>& xs,
   return interpolate(xs, ys, idx, x, i);
 }
 
+inline double interpolate_lagrangian(
+  const std::vector<double>& xs, const std::vector<double>& ys, double x)
+{
+  int idx = lower_bound_index(xs.begin(), xs.end(), x);
+
+  std::vector<double> coeffs;
+
+  int order = 3;
+
+  for (int i = 0; i < order + 1; i++) {
+    double numerator {1.0};
+    double denominator {1.0};
+    for (int j = 0; j < order; j++) {
+      if (i == j)
+        continue;
+      numerator *= (x - xs[idx + j]);
+      denominator *= (xs[idx + i] - xs[idx + j]);
+    }
+    coeffs.push_back(numerator / denominator);
+  }
+
+  return std::inner_product(
+    coeffs.begin(), coeffs.end(), ys.begin() + idx, 0.0);
+}
+
 } // namespace openmc
 
 #endif
\ No newline at end of file