3d linear interpolation

src/interpolate3d.hpp

@@ -0,0 +1,105 @@
+#ifndef __COSMO_INTERPOLATE3D_HPP
+#define __COSMO_INTERPOLATE3D_HPP
+
+#include "config.hpp"
+#include "field.hpp"
+#include <cmath>
+
+namespace CosmoTool
+{
+
+  template<typename IType>
+  class GridSampler
+  {
+  public:
+    typedef IType result_type;
+
+    GridSampler(IType *array_, int Nx_, int Ny_, int Nz_, int stride_)
+      : array(array_), Nx(Nx_), Ny(Ny_), Nz(Nz_), stride(stride_)
+    {
+    }
+
+    ~GridSampler()
+    {
+    }
+
+    IType& operator()(int x, int y, int z)
+      throw ()
+    {
+      while (x < 0)
+	x += Nx;
+      x %= Nx;
+      while (y < 0)
+	y += Ny;
+      y %= Ny;
+      while (z < 0)
+	z += Nz;
+      z %= Nz;
+      
+      uint32_t idx = x + Nx * (y + Ny * z);
+
+      return array[idx*stride];
+    }
+
+  private:
+    IType *array;
+    int Nx, Ny, Nz, stride;
+  };
+
+
+  // IType is the quantity to interpolate, 
+  template<typename SampledFunction, typename PosType = float>
+  class Interpolate3D
+  {
+  public:
+    typedef typename SampledFunction::result_type IType;
+
+    Interpolate3D(SampledFunction& f)
+      : sampler(f)
+    {
+    };
+
+    ~Interpolate3D()
+    {
+    };
+    
+    IType get(PosType x, PosType y, PosType z)
+      throw (InvalidArgumentException)
+    {
+      int ix = (int)std::floor(x);
+      int iy = (int)std::floor(y);
+      int iz = (int)std::floor(z);
+
+      PosType rx = x-ix;
+      PosType ry = y-iy;
+      PosType rz = z-iz;
+
+      IType v000 = sampler(ix,iy,iz);
+      IType v001 = sampler(ix,iy,iz+1);
+      IType v010 = sampler(ix,iy+1,iz);
+      IType v011 = sampler(ix,iy+1,iz+1);
+
+      IType v100 = sampler(ix+1,iy,iz);
+      IType v101 = sampler(ix+1,iy,iz+1);
+      IType v110 = sampler(ix+1,iy+1,iz);
+      IType v111 = sampler(ix+1,iy+1,iz+1);
+
+      return
+	((1-rx) * (1-ry) * (1-rz)) * v000 +
+	((1-rx) * (1-ry) *    rz)  * v001 +
+	((1-rx) *    ry  * (1-rz)) * v010 +
+	((1-rx) *    ry  *    rz)  * v011 +
+	(   rx  * (1-ry) * (1-rz)) * v100 +
+	(   rx  * (1-ry) *    rz)  * v101 +
+	(   rx  *    ry  * (1-rz)) * v110 +
+	(   rx  *    ry  *    rz)  * v111;
+    }
+
+  private:
+    SampledFunction& sampler;    
+    int Nx, Ny, Nz;
+  };
+
+};
+
+#endif