UniformGrid3.h

#pragma once
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <stdexcept>
#include <vector>
 
namespace marenv::grid
{
template<class T>
class UniformGrid3
{
public:
    UniformGrid3() = default;
 
    UniformGrid3(const double lower[3], const double spacing[3], const int numCells[3], const T& initial = T {})
    {
        std::size_t size = 1;
        for (int a = 0; a < 3; ++a) {
            if (!(spacing[a] > 0.0))
                throw std::invalid_argument("UniformGrid3: the spacing must be positive");
            if (numCells[a] < 1)
                throw std::invalid_argument("UniformGrid3: the number of cells must be at least 1");
            m_lower[a]    = lower[a];
            m_spacing[a]  = spacing[a];
            m_numCells[a] = numCells[a];
            size *= static_cast<std::size_t>(numCells[a] + 1);
        }
        m_values.assign(size, initial);
    }
 
    int NumCells(int axis) const { return m_numCells[axis]; }
 
    double Spacing(int axis) const { return m_spacing[axis]; }
 
    double NodeCoordinate(int axis, int index) const { return m_lower[axis] + index * m_spacing[axis]; }
 
    T& At(int i, int j, int k) { return m_values[Index(i, j, k)]; }
 
    const T& At(int i, int j, int k) const { return m_values[Index(i, j, k)]; }
 
    bool Contains(const double pos[3]) const
    {
        if (m_values.empty())
            return false;
        for (int a = 0; a < 3; ++a) {
            const double scaled = (pos[a] - m_lower[a]) / m_spacing[a];
            if (!(scaled >= -boundarySlack && scaled <= m_numCells[a] + boundarySlack))
                return false;
        }
        return true;
    }
 
    bool Interpolate(const double pos[3], T& out) const
    {
        if (!Contains(pos)) {
            out = T {};
            return false;
        }
        int cell[3];
        double fraction[3];
        for (int a = 0; a < 3; ++a) {
            const double scaled = std::clamp((pos[a] - m_lower[a]) / m_spacing[a], 0.0, static_cast<double>(m_numCells[a]));
            cell[a]             = std::min(static_cast<int>(std::floor(scaled)), m_numCells[a] - 1);
            fraction[a]         = scaled - cell[a];
        }
        T sum {};
        for (int corner = 0; corner < 8; ++corner) {
            double weight = 1.0;
            int node[3];
            for (int a = 0; a < 3; ++a) {
                const bool upper = (corner >> a) & 1;
                node[a]          = cell[a] + (upper ? 1 : 0);
                weight *= upper ? fraction[a] : 1.0 - fraction[a];
            }
            sum = sum + weight * At(node[0], node[1], node[2]);
        }
        out = sum;
        return true;
    }
 
private:
    static constexpr double boundarySlack = 1e-9; 
 
    std::size_t Index(int i, int j, int k) const
    {
        const std::size_t numX = static_cast<std::size_t>(m_numCells[0] + 1);
        const std::size_t numY = static_cast<std::size_t>(m_numCells[1] + 1);
        return static_cast<std::size_t>(i) + numX * (static_cast<std::size_t>(j) + numY * static_cast<std::size_t>(k));
    }
 
    double m_lower[3]   = {0.0, 0.0, 0.0}; 
    double m_spacing[3] = {1.0, 1.0, 1.0}; 
    int m_numCells[3]   = {0, 0, 0}; 
    std::vector<T> m_values; 
};
} // namespace marenv::grid