C++ generic, multi-dimensional linear interpolation template

Question

Use-case in a nutshell: Interpolate fixed-sized data (a lookup table) that provides output values for combinations of a (fixed) number of inputs. The appropriate scaling is done externally, each axis starts at 0 and increments by 1. The output is a floating point value regardless of the table data type, the idea being that the user of this class can determine if rounding/casting is needed.

This function attempts to generalise (bi|tri|etc)linear interpolation in a way that's efficient on a microcontroller (with hardware FPU).

A working example that can be copy-pasted:

#include <cmath>
#include <array>
#include <cstddef>
#include <cstdint>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace interpolate {
    /**
     * @brief Linear interpolation for N-dimensional data. Saturates / clamps search values to dataset range.
     * 
     * This function is mainly meant to interpolate singleton values -such as lookup tables-, not entire images.
     * 
     * @tparam DIMS     Size of each dimension in supplied data array, excluding the last. For example 7 for a table with 7 columns and 3 rows.
     * @tparam Td       Type of data
     * @tparam N        Count of data
     * @tparam Ts       Type of search values / indices
     * @param search... Search indices, the amount equal to the amount of dimensions.
     * @return Td       Interpolated value
     */
    template <std::size_t... DIMS,
              typename Td = decltype(0.0),
              std::size_t N,
              typename... Ts>
    constexpr auto linear(const Td(&data)[N], const Ts& ...search) {
        using index_t = std::size_t;
        using fp_t = decltype(0.0); // Use the system default literal floating point type
        using result_t = std::conditional_t<std::is_floating_point_v<Td> && (sizeof(Td) >= sizeof(fp_t)), Td, fp_t>; // Ensure we're always working in precise and reasonably performant floating point
        constexpr index_t D {sizeof...(Ts)};
        using search_set = std::array<std::common_type_t<Ts...>, D>;

        if constexpr (sizeof...(Ts) > 1) {
            constexpr index_t LAST_S { N / (DIMS * ...) };
            constexpr std::size_t SIZES[D] { DIMS..., LAST_S };
            constexpr std::size_t MAX[D] { (DIMS - 1u)..., LAST_S - 1 };

            constexpr std::size_t L = 0;
            constexpr std::size_t H = 1;
            
            static_assert(sizeof...(DIMS) + 1 == sizeof...(Ts), "Amount of dimension sizes doesn't match search parameter count");
            static_assert((DIMS * ...) != 0 && LAST_S != 0, "Can't interpolate singularities");
            static_assert(N == (LAST_S * (DIMS * ...)), "Data size doesn't match provided dimension sizes");

            index_t i[D][2] {{0}}; // High & low indices
            fp_t    w[D][2] {{0}}; // Associated weights

            result_t result {0};

            for (index_t j = 0; j < D; ++j) {
                const auto searchval = search_set{{search...}}[j];
                // Perform input clamping
                if (searchval <= 0) {
                    // Keeping the indices one apart should do better in vectorisation
                    i[j][L] = 0;
                    i[j][H] = 1;
                    w[j][L] = 1;
                    w[j][H] = 0;
                } else if (searchval >= MAX[j]) {
                    i[j][L] = MAX[j]-1;
                    i[j][H] = MAX[j];
                    w[j][L] = 0;
                    w[j][H] = 1;
                } else {
                    i[j][L] = static_cast<index_t>(searchval);
                    if constexpr (false) {
                        // Sinus curve based fitting. TODO: Find an elegant way to toggle this
                        i[j][H] = i[j][L] + ((search_set{{search...}}[j] - i[j][L]) > 0);
                        w[j][H] = std::sin((i[j][L] + 0.5 - searchval) * M_PI) / -2.0 + 0.5;
                    } else {
                        // Linear
                        w[j][H] = searchval - static_cast<index_t>(searchval);
                        i[j][H] = i[j][L] + 1;
                    }
                    w[j][L] = 1 - w[j][H];
                }
            }

            // Accumulate 2^D weighed results
            for (index_t j = 0; j < (0b1 << D); ++j) {
                fp_t    weight { w[0][j & 0b1] };
                index_t offset { 1 };
                index_t idx    { i[0][j & 0b1] };

                for (index_t k = 1; k < D; ++k) {
                    const bool n = (j >> k) & 0b1;
                    weight *= w[k][n];
                    offset *= SIZES[k-1];
                    idx += i[k][n] * offset;
                }
                result += weight * data[idx];

            }
            return result;

        } else {
            const auto searchval = search_set{{search...}}[0];
            if (searchval <= 0) {
                return static_cast<result_t>(data[0]);
            } else if (searchval >= N) {
                return static_cast<result_t>(data[N-1]);
            } else {
                const auto i = static_cast<index_t>(searchval);
                const auto w = searchval - i;
                if (w == 0) {
                    return static_cast<result_t>(data[i]);
                } else {
                    return static_cast<result_t>(data[i] * (1 - w) + data[i + 1] * w);
                }
            }
        }
    }

}

// volatile
constexpr
float
// unsigned
data[] {
//         0      1      2      3  
/* 0 */  1000,  2000,  3000,  4000,
/* 1 */  5000,  6000,  7000,  8000,
/* 2 */  9000, 10000, 11000, 12000,
/* 3 */  1000,  2000,  3000,  4000,
/* 4 */  5000,  6000,  7000,  8000,
/* 5 */  9000, 10000, 11000, 12000,
/* 6 */  1000,  2000,  3000,  4000,
/* 7 */  5000,  6000,  7000,  8000,
/* 8 */  9000, 10000, 11000, 12000
};

int main() {
    volatile unsigned interpolated1 __attribute__((unused)) = interpolate::linear(data, 2.5);
    volatile unsigned interpolated2 __attribute__((unused)) = interpolate::linear<4>(data, 2.5, 1.5);
    volatile unsigned interpolated3 __attribute__((unused)) = interpolate::linear<4,3>(data, 2.25, 1.25, 0.5);
}

I'd love to hear comments & critiques!

Answer 1

First, the input type is rather strange const Td(&data)[N] - normally one would want a std::span or equivalent and I don't think that fixing values of all dimensions as a template parameter is helpful. Number of dimensions indeed should be a template as otherwise implementation would be somewhat more problematic with requirements of dynamic memory usage.

Second, const Ts& ...search you should wrap it as std::array<floating_type, dims> or something of the sort instead of a sequence of arbitrary parameters.

Perhaps, this is an oddness of programming for a micro controller, but generally I'd advice to wrap it all in a class and store important parameters like SIZES[D], MAX[D], and more importantly steps sizes as members of the class. And hopefully make the whole class constexpr. I am not certain which C++ version do you use and thus not entire certain that it is feasible.

Lastly, the interpolation algorithm is inefficient. What you do is for each vertex of the n dimensional cube compute product of weights and sum it all together. Which is inefficient as you make O(n) products for each vertex with a bunch of rather random memory access.

A faster approach is to interpolate according to a single dimension and get n-1 dimensional cube of values - and repeat. This way you'll make O(1) products for each vertex. I suppose it is confusing what I mean without example.

Let me give you one: you have 2x2x2 block {{{1,2}{3,4}},{{5,6}{7,8}}} and you want to compute interpolation at point (0.3, 0.4, 0.8). So first you interpolate along first dimension and get 2x2 block:

{{1.3, 3.3}, {5.3, 7.3}} // here 1*(1-0.3) + 2*0.3 = 1.3, 3*(1-0.3) + 4*0.3 = 3.3 etc...

Then you interpolate along second dimension (interp value = 0.4) to get block of size 2 {2.1, 6.1} and finally, along the last dimension (interp value = 0.8) and get 2.1*(1-0.8)+6.1*0.8 = 5.3.

Here, as you can see it is a rather simple double loop that just applies a[k] = a[2k]*(1-p)+a[2k+1]*p for the majority of operations.