Merge branch 'master' of ssh://doucillon/tmp_mnt/netapp/users_home7/lavaux/git_root/CosmoToolbox

src/dinterpolate.hpp

@@ -0,0 +1,90 @@
+#ifndef __COSMO_DINTERPOLATE_HPP
+#define __COSMO_DINTERPOLATE_HPP
+
+#include "config.hpp"
+#include "mykdtree.hpp"
+#include <gsl/gsl_eigen.h>
+
+namespace CosmoTool {
+ 
+  template<typename PType, typename IType, int N>
+  class DelaunayInterpolate {
+  public:
+    struct SimplexAccess {
+      int32_t *simplex_list;      
+    };
+ 
+    typedef KDTree<N, SimplexAccess, PType> QuickTree;
+    typedef typename QuickTree::Cell QuickCell;
+    typedef PType CoordType[N];
+    
+    QuickTree *quickAccess;
+    QuickCell *cells;
+    PType *all_preweight;
+    int32_t *point_to_simplex_list_base;    
+    IType *values;
+    CoordType *positions;    
+    uint32_t numPoints;
+    uint32_t numSimplex;
+    uint32_t *simplex_list;
+    gsl_eigen_symmv_workspace *eigen_work;
+    
+    /**
+     * This construct the interpolator. The construction is time consuming so
+     * please do it the less possible number of times, especially if you have
+     * a large number of points.
+     *
+     * @param positions list of the positions
+     * @param values list of the values taken at each position
+     * @param simplex_list list of points for each simplex. The packing
+     * is the following:
+     * [t(0,1),t(0,2),...,t(0,n+1),t(1,0),t(1,1),...,t(1,n+1),..],
+     * with t(i,j) the i-th simplex and j-th point of the simplex. The indexes
+     * refer to the previous list of points.
+     * @param numPoints the number of points
+     */
+    DelaunayInterpolate(CoordType *positions, IType *values, uint32_t *simplex_list,
+			uint32_t numPoints, uint32_t numSimplex)
+	throw (InvalidArgumentException)
+    {
+      this->positions = positions;
+      this->values = values;
+      this->simplex_list = simplex_list;
+      this->numPoints = numPoints;
+      this->numSimplex = numSimplex;
+      
+      buildPreweight();
+      buildQuickAccess();
+
+      eigen_work = gsl_eigen_symmv_alloc(N);
+
+    }
+    
+    ~DelaunayInterpolate()
+    {
+      delete[] cells;
+      delete quickAccess;
+      delete[] point_to_simplex_list_base;      
+      delete[] all_preweight;
+
+      gsl_eigen_symmv_free(eigen_work);
+    }
+
+    void buildPreweight()
+	throw (InvalidArgumentException);
+    void buildQuickAccess();
+    void buildHyperplane(const PType *v, CoordType& hyper);
+
+    bool checkPointInSimplex(const CoordType& pos, uint32_t simplex);
+    
+    uint32_t findSimplex(const CoordType& pos)
+      throw (InvalidArgumentException);
+
+    IType computeValue(const CoordType& pos)
+      throw (InvalidArgumentException);
+  };
+};
+
+#include "dinterpolate.tcc"
+
+#endif

src/dinterpolate.tcc

@@ -0,0 +1,294 @@
+#include <cstdlib>
+#include <cassert>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_linalg.h>
+
+namespace CosmoTool {
+
+
+  template<typename PType, typename IType, int N>
+  void DelaunayInterpolate<PType,IType,N>::buildQuickAccess()
+  {
+    cells = new QuickCell[numPoints];
+
+    uint32_t point_to_simplex_size = 0;
+    uint32_t *numSimplex_by_point = new uint32_t[numPoints];
+    uint32_t *index_by_point = new uint32_t[numPoints];
+
+    // First count the number of simplex for each point
+    for (uint32_t i = 0; i < numPoints; i++)
+      index_by_point[i] = numSimplex_by_point[i] = 0;
+    for (uint32_t i = 0; i < (N+1)*numSimplex; i++)
+      numSimplex_by_point[simplex_list[i]]++;
+
+    // Compute the total number and the index for accessing lists.
+    for (uint32_t i = 0; i < numPoints; i++)
+      {
+	index_by_point[i] = point_to_simplex_size;
+	point_to_simplex_size += numSimplex_by_point[i]+1;
+      }
+
+    // Now compute the real list.
+    point_to_simplex_list_base = new int32_t[point_to_simplex_size];
+    for (uint32_t i = 0; i < numSimplex; i++)
+      {
+	for (int j = 0; j <= N; j++)
+	  {
+	    uint32_t p = simplex_list[(N+1)*i+j];
+	    point_to_simplex_list_base[index_by_point[p]] = i;
+	    ++index_by_point[p];
+	  }
+      }
+
+    // Finish the lists
+    for (uint32_t i = 0; i < numPoints; i++)
+      {
+	// check assertion
+	assert((i==0 && index_by_point[0]==numSimplex_by_point[0]) || ((index_by_point[i]-index_by_point[i-1]) == (numSimplex_by_point[i]+1)));
+	point_to_simplex_list_base[index_by_point[i]] = -1;
+      }
+    
+    uint32_t idx = 0;
+    for (uint32_t i = 0; i < numPoints; i++)
+      {
+	cells[i].active = true;
+	cells[i].val.simplex_list = &point_to_simplex_list_base[idx];
+	// We may have to cast here.
+	for (int j = 0; j < N; j++)
+	  cells[i].coord[j] = positions[i][j];
+
+	idx += numSimplex_by_point[i]+1;
+      }
+
+    // Free the memory allocated for temporary arrays.
+    delete[] numSimplex_by_point;
+    delete[] index_by_point;
+
+    // Build the kd tree now.
+    quickAccess = new QuickTree(cells, numPoints);
+  }
+
+  template<typename PType, typename IType, int N>
+  void DelaunayInterpolate<PType,IType,N>::buildPreweight()
+	throw(InvalidArgumentException)
+  {
+    double preweight[N*N];
+    double preweight_inverse[N*N];
+    gsl_permutation *p = gsl_permutation_alloc(N);
+
+    all_preweight = new PType[N*N*numSimplex];
+
+    for (uint32_t i = 0; i < numSimplex; i++)     
+      {
+	uint32_t base = i*(N+1);
+	uint32_t pref = simplex_list[base];
+	// Compute the forward matrix first.
+	for (int j = 0; j < N; j++)
+	  {
+	    PType xref = positions[pref][j];
+
+	    for (int k = 0; k < N; k++)
+	      {
+		preweight[j*N + k] = positions[simplex_list[k+base+1]][j] - xref;
+	      }
+	  }
+
+	gsl_matrix_view M = gsl_matrix_view_array(preweight, N, N);
+	gsl_matrix_view iM = gsl_matrix_view_array(preweight_inverse, N, N);
+	int signum;
+
+	gsl_linalg_LU_decomp(&M.matrix, p, &signum);
+	if (fabs(gsl_linalg_LU_det(&M.matrix, signum)) < 1e-10)
+	  throw InvalidArgumentException("Invalid tesselation. One simplex is coplanar.");
+	gsl_linalg_LU_invert(&M.matrix, p, &iM.matrix);
+
+	for (int j = 0; j < N*N; j++)
+	  all_preweight[N*N*i + j] = preweight_inverse[j];
+      }
+
+    gsl_permutation_free(p);
+  }
+
+  template<typename PType, typename IType, int N>
+  void DelaunayInterpolate<PType,IType,N>::buildHyperplane(const PType *v, CoordType& hyper)
+  {
+    double M[N][N], eVal[N], eVec[N][N];
+    gsl_matrix_view mM, evec;
+    gsl_vector_view eval;
+
+    // Construct the symmetric matrix
+    for (int k = 0; k < N; k++)
+      for (int l = k; l < N; l++)
+	{
+	  double val = 0;
+
+	  for (int i = 0; i < (N-1); i++)
+	    {
+	      val += v[i*N+l] * v[i*N+k];
+	    }
+	  M[l][k] = M[k][l] = val;
+	}
+
+    mM = gsl_matrix_view_array(&M[0][0], N, N);
+    evec = gsl_matrix_view_array(&eVec[0][0], N, N);
+    eval = gsl_vector_view_array(&eVal[0], N);
+
+    // Solve the eigensystem
+    gsl_eigen_symmv (&mM.matrix, &eval.vector, &evec.matrix, eigen_work);
+    
+    double minLambda = INFINITY;
+    uint32_t idx = N+1;
+
+    // Look for the smallest eigenvalue
+    for (int k = 0; k < N; k++)
+      {
+	if (minLambda > eVal[k])
+	  {
+	    minLambda = eVal[k];
+	    idx = k;
+	  }
+      }
+    assert(idx != (N+1));
+
+    // Copy the corresponding vector
+    for (int k = 0; k < N; k++)
+      {
+	hyper[k] = eVec[k][idx];
+      }
+  }
+
+  template<typename PType, typename IType, int N>
+  bool DelaunayInterpolate<PType,IType,N>::checkPointInSimplex(const CoordType& pos, uint32_t simplex)
+  {
+    uint32_t *desc_simplex = &simplex_list[simplex*(N+1)];
+    CoordType *p[N+1], v[N], hyper;
+
+    for (int k = 0; k <= N; k++)
+      p[k] = &positions[desc_simplex[k]];
+
+
+    for (int i = 0; i <= N; i++)
+      {
+	// Build vectors 
+	for (int k = 1; k <= N; k++)
+	  for (int l = 0; l < N; l++)
+	    v[k-1][l] = (*p[k])[l] - (*p[0])[l];
+
+	// Build hyperplane.
+	buildHyperplane(&v[0][0], hyper);
+       
+	// Compute the appropriate sign using the last point.
+	PType sign = 0;
+	for (int k = 0; k < N; k++)
+	  sign += hyper[k] * v[N-1][k];
+
+	// Now check the point has the same sign;
+	PType pnt_sign = 0;
+	for (int k = 0; k < N; k++)
+	  pnt_sign += hyper[k] * (pos[k] - (*p[0])[k]);
+
+	if (pnt_sign*sign < 0)
+	  return false;
+
+
+	// Rotate the points.
+	for (int k = 1; k <= N; k++)
+	  {
+	    p[k-1] = p[k];
+	  }
+	p[N] = &positions[desc_simplex[i]];
+      }
+
+    // We checked all possibilities. Return now.
+    return true;
+  }
+
+
+  template<typename PType, typename IType, int N>
+  uint32_t DelaunayInterpolate<PType,IType,N>::findSimplex(const CoordType& c)
+    throw (InvalidArgumentException)
+  {
+    uint32_t N_ngb = 1;
+    QuickCell **cell_Ngb = new QuickCell *[N_ngb];
+    typename QuickTree::coords kdc;
+
+    for (int i = 0; i < N; i++)
+       kdc[i] = c[i];
+
+    // It may happen that we are unlucky and have to iterate to farther
+    // neighbors. It should happen, especially on the boundaries.
+    do
+      {
+	uint32_t i;
+
+	quickAccess->getNearestNeighbours(kdc, N_ngb, cell_Ngb);
+	
+	for (i = 0; i < N_ngb && cell_Ngb[i] != 0; i++)
+	  {
+	    int32_t *simplex_list = cell_Ngb[i]->val.simplex_list;
+	    uint32_t j = 0;
+
+	    while (simplex_list[j] >= 0)
+	      {
+		if (checkPointInSimplex(c, simplex_list[j]))
+		  {
+		    delete[] cell_Ngb;
+		    return simplex_list[j];
+		  }
+	        ++j;
+	      }
+	  }	
+	delete[] cell_Ngb;
+
+	// The point does not belong to any simplex.
+	if (i != N_ngb)
+	  throw InvalidArgumentException("the given point does not belong to any simplex");
+
+	N_ngb *= 2;
+	cell_Ngb = new QuickCell *[N_ngb];
+      }
+    while (1);
+
+    // Point not reached.
+    abort();
+    return 0;
+  }
+
+  template<typename PType, typename IType, int N>
+  IType DelaunayInterpolate<PType,IType,N>::computeValue(const CoordType& c)
+    throw (InvalidArgumentException)
+  {
+    uint32_t simplex = findSimplex(c);
+    PType *preweight = &all_preweight[simplex*N*N];
+    PType weight[N+1];
+    PType p0[N];
+    PType sum_weight = 0;
+
+    for (int i = 0; i < N; i++)
+      p0[i] = positions[simplex_list[simplex*(N+1) + 0]][i];
+
+    // Now we use the preweight to compute the weight...
+    for (int i = 1; i <= N; i++)
+      {
+	weight[i] = 0;
+	for (int j = 0; j < N; j++)
+	  weight[i] += preweight[(i-1)*N+j]*(c[j]-p0[j]);
+
+	assert(weight[i] > -1e-7);
+	assert(weight[i] < 1+1e-7);
+	sum_weight += weight[i];
+      }   
+    weight[0] = 1-sum_weight;
+    assert(weight[0] > -1e-7);
+    assert(weight[0] < (1+1e-7));
+    
+    // We compute the final value by weighing the value at the N+1
+    // points by the proper weight.
+    IType final = 0;
+    for (int i = 0; i <= N; i++)
+      final += weight[i] * values[ simplex_list[simplex*(N+1) + i] ];
+
+    return final;
+  }
+  
+};

src/field.hpp

@@ -0,0 +1,83 @@
+#ifndef __COSMOTOOL_FIELD
+#define __COSMOTOOL_FIELD
+
+#include "config.hpp"
+#include <iostream>
+#include <cassert>
+
+namespace CosmoTool {
+
+  template<typename BaseType>
+  struct ScalarField
+  {
+    BaseType value;    
+  };
+
+  template<typename BaseType, int N>
+  struct VectorField
+  {
+    BaseType vec[N];
+    
+    VectorField& operator=(const VectorField& a)
+    {
+      for (int i = 0; i < N; i++)
+	vec[i] = a.vec[i];
+      return *this;
+    }
+
+    VectorField()
+    {
+      for (int i = 0; i < N; i++)
+	vec[i] = 0;      
+    }
+
+    VectorField(double a)
+    {
+      assert(a == 0);
+      for (int i = 0; i < N; i++)
+	vec[i] = 0;      
+    }
+  };
+
+  template<typename BaseType, int N>
+  VectorField<BaseType,N> operator*(BaseType s, const VectorField<BaseType,N>& a)
+  {
+    VectorField<BaseType,N> v;
+
+    for (int i = 0; i < N; i++)
+      v.vec[i] = a.vec[i]*s;
+    
+    return v;
+  }  
+  
+  template<typename BaseType, int N>
+  VectorField<BaseType,N> operator+(const VectorField<BaseType,N>& a, const VectorField<BaseType,N>& b)
+  {
+    VectorField<BaseType,N> v;
+
+    for (int i = 0; i < N; i++)
+      v.vec[i] = a.vec[i]+b.vec[i];
+    
+    return v;
+  }  
+
+  template<typename BaseType, int N>
+  VectorField<BaseType,N>& operator+=(VectorField<BaseType,N>& a, const VectorField<BaseType,N>& b)
+  {
+    for (int i = 0; i < N; i++)
+      a.vec[i]+=b.vec[i];
+    
+    return a;
+  }  
+
+};
+
+template<typename BaseType, int N>
+std::ostream& operator<<(std::ostream& s, const CosmoTool::VectorField<BaseType,N>& a)
+{
+  for (int i = 0; i < N; i++)
+    s << a.vec[i] << " " ;
+  return s;
+}
+
+#endif

src/fixArray.hpp

@@ -0,0 +1,40 @@
+#ifndef __FIX_ARRAY_HPP
+#define __FIX_ARRAY_HPP
+
+namespace CosmoTool
+{
+
+  template <typename T, unsigned int sz> class fixArray
+  {
+  private:
+    T d[sz];
+    
+  public:
+    /*! Returns the size of the array. */
+    long size() const { return sz; }
+
+    /*! Returns a reference to element \a #n */
+    template<typename T2> T &operator[] (T2 n) {
+      return d[n];
+    }
+
+    /*! Returns a constant reference to element \a #n */
+    template<typename T2> const T &operator[] (T2 n) const {
+      return d[n];
+    }
+
+    template<typename T2> void importArray(T2 *indata) {
+      for (int i = 0; i < sz; i++)
+	d[i] = indata[i];
+    }
+
+    template<typename T2> void exportArray(T2 *outdata) {
+      for (int i = 0; i < sz; i++)
+	outdata[i] = d[i];
+    }
+
+  };
+
+};
+
+#endif

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