Indoor/math/boxkde/Grid3D.h

#pragma once

//#include "DataStructures.h"
#include "Image3D.h"

#include "../../geo/Point3.h"
#include "../../geo/BBox3.h"

template<class T>
struct Grid3D
{
    static_assert(std::is_floating_point<T>::value, "Grid3D only supports float values");

public:
    const _BBox3<T> bb;
    const size_t numBinsX, numBinsY, numBinsZ;
    const T binSizeX, binSizeY, binSizeZ;
private:
    Image3D<T> data;
    bool empty;

public:
    Grid3D(_BBox3<T> bb, size_t numBinsAll)
        : Grid3D(bb, numBinsAll, numBinsAll, numBinsAll)
    {}

    Grid3D(_BBox3<T> bb, size_t numBinsX, size_t numBinsY, size_t numBinsZ)
        : bb(bb),
          numBinsX(numBinsX),
          numBinsY(numBinsY),
          numBinsZ(numBinsZ),
          binSizeX((bb.getSize().x  + 1) / numBinsX),
          binSizeY((bb.getSize().y + 1) / numBinsY),
          binSizeZ((bb.getSize().z  + 1) / numBinsZ),
          data(numBinsX, numBinsY, numBinsZ),
          empty(false)
    {
    }

          Image3D<T>& image()       { return data; }
    const Image3D<T>& image() const { return data; }

          T& operator() (size_t x, size_t y, size_t z)       { return data(x, y, z); }
    const T& operator() (size_t x, size_t y, size_t z) const { return data(x, y, z); }

	void clear() { data.clear(); }

    void fill(const std::vector<_Point3<T>>& samples)
    {
        assertMsg(!samples.empty(), "Samples must be non-empty");

        if (!empty)
            data.clear();       

        const T weight = T(1.0) / samples.size();
        for (const auto& pt : samples)
        {
            add(pt.x, pt.y, pt.z, weight);
        }

        empty = false;
    }

    void fill(const std::vector<_Point3<T>>& samples, const std::vector<T>& weights)
    {
        assertMsg(!samples.empty(), "Samples must be non-empty");
        assertMsg(weights.size() == samples.size(), "Weights must have the same size as samples");

        if (!empty)
            data.clear();

        for (size_t i = 0; i < samples.size(); i++)
        {
            add(samples[i].x, samples[i].y, samples[i].z, weights[i]);
        }

        empty = false;
    }

	_Point3<size_t> add(_Point3<T> pt, T w)
	{
		return add(pt.x, pt.y, pt.z, w);
	}

	_Point3<size_t> add(T x, T y, T z, T w)
    {
        if (assertCond(bb.contains(_Point3<T>(x, y, z))))
        {
            std::stringstream ss;
            ss << "Point " << "(" << x << "; " << y << "; " << z << ")" << " is out of bounds: "
			   << "(X: " << bb.getMin().x << " - " << bb.getMax().x << ";"
			   << " Y: " << bb.getMin().y << " - " << bb.getMax().y << ";"
			   << " Z: " << bb.getMin().z << " - " << bb.getMax().z << ")";
            assertThrow(ss.str());
        }
        return add_simple_bin(x, y, z, w);
    }

    T fetch(T x, T y, T z) const
    {
        size_t bin_x = (size_t)((x - bb.getMin().x) / binSizeX);
		size_t bin_y = (size_t)((y - bb.getMin().y) / binSizeY);
		size_t bin_z = (size_t)((z - bb.getMin().z) / binSizeZ);

        return data(bin_x, bin_y, bin_z);
    }

	T maximum(_Point3<T>& pt) const
	{
		_Point3<size_t> gridPt;

		T maxValue = image().maximum(gridPt);
		pt = asInputSpace(gridPt);
		return maxValue;
	}


 //   // Takes a point in input space and converts it to grid space coordinates
	//_Point3<size_t> asGridSpace(T x, T y, T z) const
 //   {
 //       size_t bin_x = (size_t)((x - bb.MinX) / binSizeX);
 //       size_t bin_y = (size_t)((y - bb.MinY) / binSizeY);
 //       size_t bin_z = (size_t)((z - bb.MinZ) / binSizeZ);

 //       if (bin_x == data.width)  bin_x--;
 //       if (bin_y == data.height) bin_y--;
 //       if (bin_z == data.depth) bin_z--;

 //       return Point3D<size_t>(bin_x, bin_y, bin_z);
 //   }

 //   // Takes a Size2D in input space and converts it to grid space coordiantes
	//_Point3<size_t> asGridSpace(Size3D<T> sz) const
 //   {
 //       return _Point3<size_t>(sz.sX / binSizeX, sz.sY / binSizeY, sz.sZ / binSizeZ);
 //   }

 //   // Takes a Range2D in input space and converts it to grid space coordinates
 //   Range3D<size_t> asGridSpace(Range3D<T> range) const
 //   {
 //       Point3D<size_t> startPt = asGridSpace(range.X.Start, range.Y.Start, range.Z.Start);

 //       size_t lengthX = range.X.Length / binSizeX;
 //       size_t lengthY = range.Y.Length / binSizeY;
 //       size_t lengthZ = range.Z.Length / binSizeZ;

 //       return Range3D<size_t>(startPt.X, lengthX, startPt.Y, lengthY, startPt.Z, lengthZ);
 //   }

    // Takes a point in grid space and converts it to input space coordinates
    _Point3<T> asInputSpace(_Point3<size_t> pt) const
    {
        return asInputSpace(pt.x, pt.y, pt.z);
    }

    // Takes a point in grid space and converts it to input space coordinates
	_Point3<T> asInputSpace(size_t x, size_t y, size_t z) const
    {
        return _Point3<T>(x * binSizeX + bb.getMin().x + T(0.5)*binSizeX,
                          y * binSizeY + bb.getMin().y + T(0.5)*binSizeY,
                          z * binSizeZ + bb.getMin().z + T(0.5)*binSizeZ);
    }

private:
	_Point3<size_t> add_simple_bin(T x, T y, T z, T w)
    {
        size_t bin_x = (size_t)((x - bb.getMin().x) / binSizeX);
		size_t bin_y = (size_t)((y - bb.getMin().y) / binSizeY);
		size_t bin_z = (size_t)((z - bb.getMin().z) / binSizeZ);

        //if (bin_x == data.width)  bin_x--;
        //if (bin_y == data.height) bin_y--;
        //if (bin_z == data.depth)  bin_z--;

        data(bin_x, bin_y, bin_z) += w;

        return _Point3<size_t>(bin_x, bin_y, bin_z);
    }
};

template struct Grid3D<float>;
template struct Grid3D<double>;