Added 3D boxKDE

math/boxkde/Grid3D.h

@@ -0,0 +1,182 @@
+#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>;