From 64372eb9dc8e9b78064e9fa458196c60af85faed Mon Sep 17 00:00:00 2001
From: Matt Spraggs <matthew.spraggs@gmail.com>
Date: Tue, 10 Nov 2015 12:23:29 +0000
Subject: [PATCH] TabFunction: Implemented quadratic interpolation

---
 lib/TabFunction.cpp | 63 +++++++++++++++++++++++++++++++++++----------
 lib/TabFunction.hpp |  2 ++
 2 files changed, 51 insertions(+), 14 deletions(-)

diff --git a/lib/TabFunction.cpp b/lib/TabFunction.cpp
index 44dd546..41a0b32 100644
--- a/lib/TabFunction.cpp
+++ b/lib/TabFunction.cpp
@@ -72,14 +72,14 @@ double TabFunction::operator()(const double *arg) const
                            + strFrom(value_.rbegin()->first) + "])");
     }
 
+    auto i = value_.equal_range(x);
+    auto low = (x == i.first->first) ? i.first : prev(i.first);
+    auto high = i.second;
+
     switch (interpType_) {
         case InterpType::LINEAR: {
             double x_a, x_b, y_a, y_b;
 
-            auto i = value_.equal_range(x);
-            auto low = (x == i.first->first) ? i.first : prev(i.first);
-            auto high = i.second;
-
             x_a = low->first;
             x_b = high->first;
             y_a = low->second;
@@ -88,18 +88,37 @@ double TabFunction::operator()(const double *arg) const
             break;
         }
         case InterpType::NEAREST: {
-            auto it = value_.equal_range(x);
-            auto lower = (x == it.first->first) ? it.first : prev(it.first);
-            auto upper = it.second;
-            if (fabs(upper->first - x) < fabs(lower->first - x)) {
-                result = upper->second;
-            }
-            else {
-                result = lower->second;
-            }
+            result = nearest(x)->second;
+            break;
+        }
+        case InterpType::QUADRATIC: {
+            double xs[3], ys[3], as[3];
+            auto it = nearest(x);
+            if (it == value_.begin()) {
+                it = next(it);
+            }
+            else if (it == value_.end()) {
+                it = prev(it);
+            }
+            xs[0] = prev(it)->first;
+            ys[0] = prev(it)->second;
+            xs[1] = it->first;
+            ys[1] = it->second;
+            xs[2] = next(it)->first;
+            ys[2] = next(it)->second;
+
+            as[0]
+                = (x - xs[1]) / (xs[0] - xs[1])
+                  * (x - xs[2]) / (xs[0] - xs[2]);
+            as[1]
+                = (x - xs[0]) / (xs[1] - xs[0])
+                  * (x - xs[2]) / (xs[1] - xs[2]);
+            as[2]
+                = (x - xs[0]) / (xs[2] - xs[0])
+                * (x - xs[1]) / (xs[2] - xs[1]);
+            result = as[0] * ys[0] + as[1] * ys[1] + as[2] * ys[2];
             break;
         }
-        case InterpType::QUADRATIC:
         default:
             int intType = static_cast<int>(interpType_);
             LATAN_ERROR(Implementation, "unsupported interpolation type in "
@@ -140,3 +159,19 @@ DoubleFunction Latan::interpolate(const XYStatData &data, const Index i,
 {
     return TabFunction(data, i, j, interpType).makeFunction();
 }
+
+map<double, double>::const_iterator TabFunction::nearest(const double x) const
+{
+    map<double, double>::const_iterator ret;
+    auto i = value_.equal_range(x);
+    auto low = (x == i.first->first) ? i.first : prev(i.first);
+    auto high = i.second;
+    if (fabs(high->first - x) < fabs(low->first - x)) {
+        ret = high;
+    }
+    else {
+        ret = low;
+    }
+
+    return ret;
+};
diff --git a/lib/TabFunction.hpp b/lib/TabFunction.hpp
index 82f9a67..d5623b7 100644
--- a/lib/TabFunction.hpp
+++ b/lib/TabFunction.hpp
@@ -59,6 +59,8 @@ public:
     // factory
     virtual DoubleFunction makeFunction(const bool makeHardCopy = true) const;
 private:
+    std::map<double, double>::const_iterator nearest(const double x) const;
+
     std::map<double, double> value_;
     InterpType interpType_;
 };