diff --git a/src/dinterpolate.hpp b/src/dinterpolate.hpp new file mode 100644 index 0000000..5109ce9 --- /dev/null +++ b/src/dinterpolate.hpp @@ -0,0 +1,90 @@ +#ifndef __COSMO_DINTERPOLATE_HPP +#define __COSMO_DINTERPOLATE_HPP + +#include "config.hpp" +#include "mykdtree.hpp" +#include + +namespace CosmoTool { + + template + class DelaunayInterpolate { + public: + struct SimplexAccess { + int32_t *simplex_list; + }; + + typedef KDTree 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 diff --git a/src/dinterpolate.tcc b/src/dinterpolate.tcc new file mode 100644 index 0000000..d545788 --- /dev/null +++ b/src/dinterpolate.tcc @@ -0,0 +1,294 @@ +#include +#include +#include +#include + +namespace CosmoTool { + + + template + void DelaunayInterpolate::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 + void DelaunayInterpolate::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 + void DelaunayInterpolate::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 + bool DelaunayInterpolate::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 + uint32_t DelaunayInterpolate::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 + IType DelaunayInterpolate::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; + } + +}; diff --git a/src/field.hpp b/src/field.hpp new file mode 100644 index 0000000..25ec447 --- /dev/null +++ b/src/field.hpp @@ -0,0 +1,83 @@ +#ifndef __COSMOTOOL_FIELD +#define __COSMOTOOL_FIELD + +#include "config.hpp" +#include +#include + +namespace CosmoTool { + + template + struct ScalarField + { + BaseType value; + }; + + template + 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 + VectorField operator*(BaseType s, const VectorField& a) + { + VectorField v; + + for (int i = 0; i < N; i++) + v.vec[i] = a.vec[i]*s; + + return v; + } + + template + VectorField operator+(const VectorField& a, const VectorField& b) + { + VectorField v; + + for (int i = 0; i < N; i++) + v.vec[i] = a.vec[i]+b.vec[i]; + + return v; + } + + template + VectorField& operator+=(VectorField& a, const VectorField& b) + { + for (int i = 0; i < N; i++) + a.vec[i]+=b.vec[i]; + + return a; + } + +}; + +template +std::ostream& operator<<(std::ostream& s, const CosmoTool::VectorField& a) +{ + for (int i = 0; i < N; i++) + s << a.vec[i] << " " ; + return s; +} + +#endif diff --git a/src/fixArray.hpp b/src/fixArray.hpp new file mode 100644 index 0000000..d1516e4 --- /dev/null +++ b/src/fixArray.hpp @@ -0,0 +1,40 @@ +#ifndef __FIX_ARRAY_HPP +#define __FIX_ARRAY_HPP + +namespace CosmoTool +{ + + template 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 T &operator[] (T2 n) { + return d[n]; + } + + /*! Returns a constant reference to element \a #n */ + template const T &operator[] (T2 n) const { + return d[n]; + } + + template void importArray(T2 *indata) { + for (int i = 0; i < sz; i++) + d[i] = indata[i]; + } + + template void exportArray(T2 *outdata) { + for (int i = 0; i < sz; i++) + outdata[i] = d[i]; + } + + }; + +}; + +#endif diff --git a/src/interpolate3d.hpp b/src/interpolate3d.hpp new file mode 100644 index 0000000..8c165ff --- /dev/null +++ b/src/interpolate3d.hpp @@ -0,0 +1,105 @@ +#ifndef __COSMO_INTERPOLATE3D_HPP +#define __COSMO_INTERPOLATE3D_HPP + +#include "config.hpp" +#include "field.hpp" +#include + +namespace CosmoTool +{ + + template + 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 + 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