uhd: math: Add linear_interp()

This lets you linearly interpolate between two points.

2 files changed, 30 insertions, 7 deletions

diff --git a/host/include/uhd/utils/math.hpp b/host/include/uhd/utils/math.hpp
index 60b34bef0..bb721d3af 100644
--- a/host/include/uhd/utils/math.hpp
+++ b/host/include/uhd/utils/math.hpp
@@ -1,14 +1,10 @@
-//
-// Copyright 2014-2015 Ettus Research LLC
-// Copyright 2018 Ettus Research, a National Instruments Company
-//
-// SPDX-License-Identifier: GPL-3.0-or-later
-//
+
 
 #ifndef INCLUDED_UHD_UTILS_MATH_HPP
 #define INCLUDED_UHD_UTILS_MATH_HPP
 
 #include <uhd/config.hpp>
+#include <uhd/exception.hpp>
 #include <stdint.h>
 #include <boost/numeric/conversion/bounds.hpp>
 #include <cmath>
@@ -250,6 +246,24 @@ inline IntegerType gcd(IntegerType x, IntegerType y)
     return _bmint::gcd<IntegerType>(x, y);
 }
 
+//! Linearly interpolate f(x) given f(x0) = y0 and f(x1) = y1
+//
+// This draws a line through the coordinates x0/y0 and x1/y1, and then returns
+// the y-value for the given x-value on said line.
+//
+// \throws uhd::runtime_error if x0 == x1, since that doesn't allow us to
+//         interpolate.
+template <typename InterpType>
+inline InterpType linear_interp(
+    InterpType x, InterpType x0, InterpType y0, InterpType x1, InterpType y1)
+{
+    if (x0 == x1) {
+        throw uhd::runtime_error("linear_interp(): x0 and x1 must differ!");
+    }
+    return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
+}
+
+
 } // namespace math
 } // namespace uhd
 
diff --git a/host/tests/math_test.cpp b/host/tests/math_test.cpp
index 4714f6625..bff6fc16f 100644
--- a/host/tests/math_test.cpp
+++ b/host/tests/math_test.cpp
@@ -6,7 +6,6 @@
 //
 
 #include <uhd/utils/math.hpp>
-#include <stdint.h>
 #include <boost/test/unit_test.hpp>
 
 BOOST_AUTO_TEST_CASE(test_lcm)
@@ -18,3 +17,13 @@ BOOST_AUTO_TEST_CASE(test_gcd)
 {
     BOOST_CHECK_EQUAL(uhd::math::gcd<int>(6, 15), 3);
 }
+
+BOOST_AUTO_TEST_CASE(test_interp)
+{
+    const double x0 = 1.0, x1 = 2.0;
+    const double y0 = 2.0, y1 = 4.0;
+    const double x     = 1.5;
+    const double y_exp = 3.0;
+
+    BOOST_CHECK_EQUAL(uhd::math::linear_interp<double>(x, x0, y0, x1, y1), y_exp);
+}