Line data Source code
1 : #ifndef CRPROPA_GRID_H
2 : #define CRPROPA_GRID_H
3 :
4 : #include "crpropa/Referenced.h"
5 : #include "crpropa/Vector3.h"
6 :
7 : #include "kiss/string.h"
8 : #include "kiss/logger.h"
9 :
10 : #include <vector>
11 : #include <type_traits>
12 : #if HAVE_SIMD
13 : #include <immintrin.h>
14 : #include <smmintrin.h>
15 : #endif // HAVE_SIMD
16 :
17 : namespace crpropa {
18 :
19 : /** If set to TRILINEAR, use trilinear interpolation (standard)
20 : If set to TRICUBIC, use tricubic interpolation instead of trilinear interpolation
21 : If set to NEAREST_NEIGHBOUR , use nearest neighbour interpolation instead of trilinear interpolation */
22 : enum interpolationType {
23 : TRILINEAR = 0,
24 : TRICUBIC,
25 : NEAREST_NEIGHBOUR
26 : };
27 :
28 : /** Lower and upper neighbour in a periodically continued unit grid */
29 : inline void periodicClamp(double x, int n, int &lo, int &hi) {
30 100051 : lo = ((int(floor(x)) % (n)) + (n)) % (n);
31 10 : hi = (lo + 1) % (n);
32 : }
33 :
34 : /** grid index in a reflective continued unit grid */
35 : inline int reflectiveBoundary(int index, int n) {
36 1963 : while ((index < -0.5) or (index > (n-0.5)))
37 806 : index = 2 * n * (index > (n-0.5)) - index-1;
38 : return index;
39 : }
40 :
41 : /** grid index in a periodically continued unit grid */
42 : inline int periodicBoundary(int index, int n) {
43 768 : return ((index % (n)) + (n)) % (n);
44 : }
45 :
46 : /** Lower and upper neighbour in a reflectively repeated unit grid */
47 20 : inline void reflectiveClamp(double x, int n, int &lo, int &hi, double &res) {
48 36 : while ((x < -0.5) or (x > (n-0.5)))
49 16 : x = 2 * n * (x > (n-0.5)) -x-1;
50 20 : res = x;
51 20 : lo = floor(x);
52 20 : hi = lo + (lo < n-1);
53 20 : if (x<0) {
54 10 : lo=0;
55 10 : hi=0;
56 : }
57 20 : }
58 :
59 : /** Symmetrical round */
60 51 : inline double round(double r) {
61 51 : return (r > 0.0) ? floor(r + 0.5) : ceil(r - 0.5);
62 : }
63 :
64 : /**
65 : * \addtogroup Core
66 : * @{
67 : */
68 :
69 : /**
70 : @class GridProperties
71 : @brief Combines parameters that uniquely define Grid class
72 : */
73 : class GridProperties: public Referenced {
74 : public:
75 : size_t Nx, Ny, Nz; // Number of grid points
76 : Vector3d origin; // Position of the lower left front corner of the volume
77 : Vector3d spacing; // Spacing vector between gridpoints
78 : bool reflective; // using reflective repetition of the grid instead of periodic
79 : interpolationType ipol; // Interpolation type used between grid points
80 : bool clipVolume; // Set grid values to 0 outside the volume if true
81 :
82 : /** Constructor for cubic grid
83 : @param origin Position of the lower left front corner of the volume
84 : @param N Number of grid points in one direction
85 : @param spacing Spacing between grid points
86 : */
87 6 : GridProperties(Vector3d origin, size_t N, double spacing) :
88 6 : origin(origin), Nx(N), Ny(N), Nz(N), spacing(Vector3d(spacing)), reflective(false), ipol(TRILINEAR), clipVolume(false) {
89 : }
90 :
91 : /** Constructor for non-cubic grid
92 : @param origin Position of the lower left front corner of the volume
93 : @param Nx Number of grid points in x-direction
94 : @param Ny Number of grid points in y-direction
95 : @param Nz Number of grid points in z-direction
96 : @param spacing Spacing between grid points
97 : */
98 0 : GridProperties(Vector3d origin, size_t Nx, size_t Ny, size_t Nz, double spacing) :
99 0 : origin(origin), Nx(Nx), Ny(Ny), Nz(Nz), spacing(Vector3d(spacing)), reflective(false), ipol(TRILINEAR), clipVolume(false) {
100 : }
101 :
102 : /** Constructor for non-cubic grid with spacing vector
103 : @param origin Position of the lower left front corner of the volume
104 : @param Nx Number of grid points in x-direction
105 : @param Ny Number of grid points in y-direction
106 : @param Nz Number of grid points in z-direction
107 : @param spacing Spacing vector between grid points
108 : */
109 1 : GridProperties(Vector3d origin, size_t Nx, size_t Ny, size_t Nz, Vector3d spacing) :
110 1 : origin(origin), Nx(Nx), Ny(Ny), Nz(Nz), spacing(spacing), reflective(false), ipol(TRILINEAR), clipVolume(false) {
111 : }
112 :
113 2 : virtual ~GridProperties() {
114 4 : }
115 :
116 : /** If True, the repetition of the grid is refletive instead of periodic. */
117 : void setReflective(bool b) {
118 0 : reflective = b;
119 : }
120 :
121 : /** set the type of interpolation between grid points.
122 : * @param i: interpolationType (TRILINEAR, TRICUBIC, NEAREST_NEIGHBOUR) */
123 : void setInterpolationType(interpolationType i) {
124 0 : ipol = i;
125 : }
126 :
127 : /** If True, the grid is set to zero outside of the volume. */
128 : void setClipVolume(bool b) {
129 0 : clipVolume = b;
130 : }
131 : };
132 :
133 : /**
134 : @class Grid
135 : @brief Template class for fields on a periodic grid with trilinear interpolation
136 :
137 : The grid spacing is constant with diffrent resolution along all three axes.
138 : Values are calculated by trilinear interpolation of the surrounding 8 grid points.
139 : The grid is periodically (default) or reflectively extended.
140 : The grid sample positions are at 1/2 * size/N, 3/2 * size/N ... (2N-1)/2 * size/N.
141 : */
142 : template<typename T>
143 17 : class Grid: public Referenced {
144 : std::vector<T> grid;
145 : size_t Nx, Ny, Nz; /**< Number of grid points */
146 : Vector3d origin; /**< Origin of the volume that is represented by the grid. */
147 : Vector3d gridOrigin; /**< Grid origin */
148 : Vector3d spacing; /**< Distance between grid points, determines the extension of the grid */
149 : bool clipVolume; /**< If set to true, all values outside of the grid will be 0*/
150 : bool reflective; /**< If set to true, the grid is repeated reflectively instead of periodically */
151 : interpolationType ipolType; /**< Type of interpolation between the grid points */
152 :
153 : public:
154 : /** Constructor for cubic grid
155 : @param origin Position of the lower left front corner of the volume
156 : @param N Number of grid points in one direction
157 : @param spacing Spacing between grid points
158 : */
159 11 : Grid(Vector3d origin, size_t N, double spacing) {
160 : setOrigin(origin);
161 11 : setGridSize(N, N, N);
162 : setSpacing(Vector3d(spacing));
163 : setReflective(false);
164 : setClipVolume(false);
165 : setInterpolationType(TRILINEAR);
166 11 : }
167 :
168 : /** Constructor for non-cubic grid
169 : @param origin Position of the lower left front corner of the volume
170 : @param Nx Number of grid points in x-direction
171 : @param Ny Number of grid points in y-direction
172 : @param Nz Number of grid points in z-direction
173 : @param spacing Spacing between grid points
174 : */
175 7 : Grid(Vector3d origin, size_t Nx, size_t Ny, size_t Nz, double spacing) {
176 : setOrigin(origin);
177 7 : setGridSize(Nx, Ny, Nz);
178 : setSpacing(Vector3d(spacing));
179 : setReflective(false);
180 : setClipVolume(false);
181 : setInterpolationType(TRILINEAR);
182 7 : }
183 :
184 : /** Constructor for non-cubic grid with spacing vector
185 : @param origin Position of the lower left front corner of the volume
186 : @param Nx Number of grid points in x-direction
187 : @param Ny Number of grid points in y-direction
188 : @param Nz Number of grid points in z-direction
189 : @param spacing Spacing vector between grid points
190 : */
191 1 : Grid(Vector3d origin, size_t Nx, size_t Ny, size_t Nz, Vector3d spacing) {
192 : setOrigin(origin);
193 1 : setGridSize(Nx, Ny, Nz);
194 : setSpacing(spacing);
195 : setReflective(false);
196 : setClipVolume(false);
197 : setInterpolationType(TRILINEAR);
198 1 : }
199 :
200 : /** Constructor for GridProperties
201 : @param p GridProperties instance
202 : */
203 9 : Grid(const GridProperties &p) :
204 9 : origin(p.origin), spacing(p.spacing), reflective(p.reflective), ipolType(p.ipol) {
205 9 : setGridSize(p.Nx, p.Ny, p.Nz);
206 9 : setClipVolume(p.clipVolume);
207 9 : }
208 :
209 : void setOrigin(Vector3d origin) {
210 : this->origin = origin;
211 : this->gridOrigin = origin + spacing/2;
212 : }
213 :
214 : /** Resize grid, also enlarges the volume as the spacing stays constant */
215 28 : void setGridSize(size_t Nx, size_t Ny, size_t Nz) {
216 28 : this->Nx = Nx;
217 28 : this->Ny = Ny;
218 28 : this->Nz = Nz;
219 28 : grid.resize(Nx * Ny * Nz);
220 : setOrigin(origin);
221 28 : }
222 :
223 : void setSpacing(Vector3d spacing) {
224 : this->spacing = spacing;
225 : setOrigin(origin);
226 : }
227 :
228 : void setReflective(bool b) {
229 8 : reflective = b;
230 : }
231 :
232 : // If set to true, all values outside of the grid will be 0.
233 : void setClipVolume(bool b) {
234 21 : clipVolume = b;
235 : }
236 :
237 : /** Change the interpolation type to the routine specified by the user. Check if this routine is
238 : contained in the enum interpolationType and thus supported by CRPropa.*/
239 0 : void setInterpolationType(interpolationType ipolType) {
240 0 : if (ipolType == TRILINEAR || ipolType == TRICUBIC || ipolType == NEAREST_NEIGHBOUR) {
241 30 : this->ipolType = ipolType;
242 0 : if ((ipolType == TRICUBIC) && (std::is_same<T, Vector3d>::value)) {
243 0 : KISS_LOG_WARNING << "Tricubic interpolation on Grid3d works only with float-precision, doubles will be downcasted";
244 : }
245 : } else {
246 0 : throw std::runtime_error("InterpolationType: unknown interpolation type");
247 : }
248 0 : }
249 :
250 : /** returns the position of the lower left front corner of the volume */
251 : Vector3d getOrigin() const {
252 : return origin;
253 : }
254 :
255 : bool getClipVolume() const {
256 0 : return clipVolume;
257 : }
258 :
259 : size_t getNx() const {
260 728 : return Nx;
261 : }
262 :
263 : size_t getNy() const {
264 41792 : return Ny;
265 : }
266 :
267 : size_t getNz() const {
268 2663721 : return Nz;
269 : }
270 :
271 : /** Calculates the total size of the grid in bytes */
272 : size_t getSizeOf() const {
273 0 : return sizeof(grid) + (sizeof(grid[0]) * grid.size());
274 : }
275 :
276 : Vector3d getSpacing() const {
277 : return spacing;
278 : }
279 :
280 : bool isReflective() const {
281 0 : return reflective;
282 : }
283 :
284 : /** Choose the interpolation algorithm based on the set interpolation type.
285 : By default this it the trilinear interpolation. The user can change the
286 : routine with the setInterpolationType function.*/
287 100091 : T interpolate(const Vector3d &position) {
288 : // check for volume
289 100091 : if (clipVolume) {
290 5 : Vector3d edge = origin + Vector3d(Nx, Ny, Nz) * spacing;
291 5 : bool isInVolume = (position.x >= origin.x) && (position.x <= edge.x);
292 5 : isInVolume &= (position.y >= origin.y) && (position.y <= edge.y);
293 5 : isInVolume &= (position.z >= origin.z) && (position.z <= edge.z);
294 5 : if (!isInVolume)
295 : return T(0.);
296 : }
297 :
298 100086 : if (ipolType == TRICUBIC)
299 18 : return tricubicInterpolate(T(), position);
300 100068 : else if (ipolType == NEAREST_NEIGHBOUR)
301 13 : return closestValue(position);
302 : else
303 100055 : return trilinearInterpolate(position);
304 : }
305 :
306 : /** Inspector & Mutator */
307 : T &get(size_t ix, size_t iy, size_t iz) {
308 8391022 : return grid[ix * Ny * Nz + iy * Nz + iz];
309 : }
310 :
311 : /** Inspector */
312 : const T &get(size_t ix, size_t iy, size_t iz) const {
313 100072 : return grid[ix * Ny * Nz + iy * Nz + iz];
314 : }
315 :
316 : const T &periodicGet(size_t ix, size_t iy, size_t iz) const {
317 192 : ix = periodicBoundary(ix, Nx);
318 192 : iy = periodicBoundary(iy, Ny);
319 192 : iz = periodicBoundary(iz, Nz);
320 192 : return grid[ix * Ny * Nz + iy * Nz + iz];
321 : }
322 :
323 0 : const T &reflectiveGet(size_t ix, size_t iy, size_t iz) const {
324 0 : ix = reflectiveBoundary(ix, Nx);
325 0 : iy = reflectiveBoundary(iy, Ny);
326 0 : iz = reflectiveBoundary(iz, Nz);
327 0 : return grid[ix * Ny * Nz + iy * Nz + iz];
328 : }
329 :
330 : T getValue(size_t ix, size_t iy, size_t iz) {
331 0 : return grid[ix * Ny * Nz + iy * Nz + iz];
332 : }
333 :
334 : void setValue(size_t ix, size_t iy, size_t iz, T value) {
335 0 : grid[ix * Ny * Nz + iy * Nz + iz] = value;
336 : }
337 :
338 : /** Return a reference to the grid values */
339 : std::vector<T> &getGrid() {
340 10102 : return grid;
341 : }
342 :
343 : /** Position of the grid point of a given index */
344 10103 : Vector3d positionFromIndex(int index) const {
345 10103 : int ix = index / (Ny * Nz);
346 10103 : int iy = (index / Nz) % Ny;
347 10103 : int iz = index % Nz;
348 10103 : return Vector3d(ix, iy, iz) * spacing + gridOrigin;
349 : }
350 :
351 : /** Value of a grid point that is closest to a given position / nearest neighbour interpolation */
352 17 : T closestValue(const Vector3d &position) const {
353 : Vector3d r = (position - gridOrigin) / spacing;
354 : int ix, iy, iz;
355 17 : if (reflective) {
356 6 : ix = round(r.x);
357 6 : iy = round(r.y);
358 6 : iz = round(r.z);
359 9 : while ((ix < -0.5) or (ix > (Nx-0.5)))
360 3 : ix = 2 * Nx * (ix > (Nx-0.5)) - ix-1;
361 11 : while ((iy < -0.5) or (iy > (Ny-0.5)))
362 5 : iy = 2 * Ny * (iy > (Ny-0.5)) - iy-1;
363 9 : while ((iz < -0.5) or (iz > (Nz-0.5)))
364 3 : iz = 2 * Nz * (iz > (Nz-0.5)) - iz-1;
365 : } else {
366 11 : ix = round(fmod(r.x, Nx));
367 11 : iy = round(fmod(r.y, Ny));
368 11 : iz = round(fmod(r.z, Nz));
369 11 : ix = (ix + Nx * (ix < 0)) % Nx;
370 11 : iy = (iy + Ny * (iy < 0)) % Ny;
371 11 : iz = (iz + Nz * (iz < 0)) % Nz;
372 : }
373 17 : return get(ix, iy, iz);
374 : }
375 :
376 : private:
377 : #ifdef HAVE_SIMD
378 : __m128 simdperiodicGet(size_t ix, size_t iy, size_t iz) const {
379 576 : ix = periodicBoundary(ix, Nx);
380 576 : iy = periodicBoundary(iy, Ny);
381 576 : iz = periodicBoundary(iz, Nz);
382 576 : return convertVector3fToSimd(grid[ix * Ny * Nz + iy * Nz + iz]);
383 : }
384 :
385 384 : __m128 simdreflectiveGet(size_t ix, size_t iy, size_t iz) const {
386 384 : ix = reflectiveBoundary(ix, Nx);
387 384 : iy = reflectiveBoundary(iy, Ny);
388 384 : iz = reflectiveBoundary(iz, Nz);
389 384 : return convertVector3fToSimd(grid[ix * Ny * Nz + iy * Nz + iz]);
390 : }
391 :
392 : __m128 convertVector3fToSimd(const Vector3f v) const {
393 : __m128 simdVar = _mm_set_ps(0,v.z,v.y,v.x);
394 : return simdVar;
395 : }
396 :
397 : Vector3f convertSimdToVector3f(__m128 res) const {
398 : float vec[4];
399 : _mm_store_ps(&vec[0], res);
400 : Vector3f result = Vector3f(vec[0], vec[1], vec[2]);
401 : return result;
402 : }
403 :
404 : /** Vectorized cubic Interpolator in 1D */
405 : __m128 CubicInterpolate(__m128 p0,__m128 p1,__m128 p2,__m128 p3,double position) const {
406 : __m128 c1 = _mm_set1_ps (1/2.);
407 : __m128 c2 = _mm_set1_ps (3/2.);
408 : __m128 c3 = _mm_set1_ps (2.);
409 : __m128 c4 = _mm_set1_ps (5/2.);
410 :
411 315 : __m128 pos = _mm_set1_ps (position);
412 315 : __m128 pos2 = _mm_set1_ps (position*position);
413 300 : __m128 pos3 = _mm_set1_ps (position*position*position);
414 :
415 : /** SIMD optimized routine to calculate 'res = ((-0.5*p0+3/2.*p1-3/2.*p2+0.5*p3)*pos*pos*pos+(p0-5/2.*p1+p2*2-0.5*p3)*pos*pos+(-0.5*p0+0.5*p2)*pos+p1);'
416 : where terms are used as:
417 : term = (-0.5*p0+0.5*p2)*pos
418 : term2 = (p0-5/2.*p1+p2*2-0.5*p3)*pos*pos;
419 : term3 = (-0.5*p0+3/2.*p1-3/2.*p2+0.5*p3)*pos*pos*pos; */
420 : __m128 term = _mm_mul_ps(_mm_sub_ps(_mm_mul_ps(c1,p2),_mm_mul_ps(c1,p0)),pos);
421 : __m128 term2 = _mm_mul_ps(_mm_sub_ps(_mm_add_ps(p0,_mm_mul_ps(c3,p2)),_mm_add_ps(_mm_mul_ps(c4,p1),_mm_mul_ps(c1,p3))),pos2);
422 : __m128 term3 = _mm_mul_ps(_mm_sub_ps(_mm_add_ps(_mm_mul_ps(c2,p1),_mm_mul_ps(c1,p3)),_mm_add_ps(_mm_mul_ps(c1,p0),_mm_mul_ps(c2,p2))),pos3);
423 : __m128 res = _mm_add_ps(_mm_add_ps(_mm_add_ps(term3,term2),term),p1);
424 : return res;
425 : }
426 : #endif // HAVE_SIMD
427 : /** Interpolate the grid tricubic at a given position (see https://www.paulinternet.nl/?page=bicubic, http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf) */
428 15 : Vector3f tricubicInterpolate(Vector3f, const Vector3d &position) const {
429 : #ifdef HAVE_SIMD
430 : // position on a unit grid
431 : Vector3d r = (position - gridOrigin) / spacing;
432 :
433 : int iX0, iY0, iZ0;
434 15 : iX0 = floor(r.x);
435 15 : iY0 = floor(r.y);
436 15 : iZ0 = floor(r.z);
437 :
438 : double fX, fY, fZ;
439 15 : fX = r.x - iX0;
440 15 : fY = r.y - iY0;
441 15 : fZ = r.z - iZ0;
442 :
443 : int nrCubicInterpolations = 4;
444 15 : __m128 interpolateVaryX[nrCubicInterpolations];
445 15 : __m128 interpolateVaryY[nrCubicInterpolations];
446 15 : __m128 interpolateVaryZ[nrCubicInterpolations];
447 : /** Perform 1D interpolations while iterating in each for loop over the index of another direction */
448 75 : for (int iLoopX = -1; iLoopX < nrCubicInterpolations-1; iLoopX++) {
449 300 : for (int iLoopY = -1; iLoopY < nrCubicInterpolations-1; iLoopY++) {
450 1200 : for (int iLoopZ = -1; iLoopZ < nrCubicInterpolations-1; iLoopZ++) {
451 960 : if (reflective)
452 384 : interpolateVaryZ[iLoopZ+1] = simdreflectiveGet(iX0+iLoopX, iY0+iLoopY, iZ0+iLoopZ);
453 : else
454 576 : interpolateVaryZ[iLoopZ+1] = simdperiodicGet(iX0+iLoopX, iY0+iLoopY, iZ0+iLoopZ);
455 : }
456 240 : interpolateVaryY[iLoopY+1] = CubicInterpolate(interpolateVaryZ[0], interpolateVaryZ[1], interpolateVaryZ[2], interpolateVaryZ[3], fZ);
457 : }
458 60 : interpolateVaryX[iLoopX+1] = CubicInterpolate(interpolateVaryY[0], interpolateVaryY[1], interpolateVaryY[2], interpolateVaryY[3], fY);
459 : }
460 15 : __m128 result = CubicInterpolate(interpolateVaryX[0], interpolateVaryX[1], interpolateVaryX[2], interpolateVaryX[3], fX);
461 15 : return convertSimdToVector3f(result);
462 : #else // HAVE_SIMD
463 : throw std::runtime_error( "Tried to use tricubic Interpolation without SIMD_EXTENSION. SIMD Optimization is necessary for tricubic interpolation of vector grids.\n");
464 : #endif // HAVE_SIMD
465 15 : }
466 :
467 : /** Vectorized cubic Interpolator in 1D that returns a scalar (see https://www.paulinternet.nl/?page=bicubic, http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf) */
468 63 : double CubicInterpolateScalar(double p0,double p1,double p2,double p3,double pos) const {
469 63 : return((-0.5*p0+3/2.*p1-3/2.*p2+0.5*p3)*pos*pos*pos+(p0-5/2.*p1+p2*2-0.5*p3)*pos*pos+(-0.5*p0+0.5*p2)*pos+p1);
470 : }
471 :
472 : /** Interpolate the grid tricubic at a given position (see https://www.paulinternet.nl/?page=bicubic, http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf) */
473 3 : double tricubicInterpolate(double, const Vector3d &position) const {
474 : /** position on a unit grid */
475 : Vector3d r = (position - gridOrigin) / spacing;
476 :
477 : int iX0, iY0, iZ0;
478 3 : iX0 = floor(r.x);
479 3 : iY0 = floor(r.y);
480 3 : iZ0 = floor(r.z);
481 :
482 : double fX, fY, fZ;
483 3 : fX = r.x - iX0;
484 3 : fY = r.y - iY0;
485 3 : fZ = r.z - iZ0;
486 :
487 : int nrCubicInterpolations = 4;
488 3 : double interpolateVaryX[nrCubicInterpolations];
489 3 : double interpolateVaryY[nrCubicInterpolations];
490 3 : double interpolateVaryZ[nrCubicInterpolations];
491 : /** Perform 1D interpolations while iterating in each for loop over the index of another direction */
492 15 : for (int iLoopX = -1; iLoopX < nrCubicInterpolations-1; iLoopX++) {
493 60 : for (int iLoopY = -1; iLoopY < nrCubicInterpolations-1; iLoopY++) {
494 240 : for (int iLoopZ = -1; iLoopZ < nrCubicInterpolations-1; iLoopZ++) {
495 192 : if (reflective)
496 0 : interpolateVaryZ[iLoopZ+1] = reflectiveGet(iX0+iLoopX, iY0+iLoopY, iZ0+iLoopZ);
497 : else
498 192 : interpolateVaryZ[iLoopZ+1] = periodicGet(iX0+iLoopX, iY0+iLoopY, iZ0+iLoopZ);
499 : }
500 48 : interpolateVaryY[iLoopY+1] = CubicInterpolateScalar(interpolateVaryZ[0], interpolateVaryZ[1], interpolateVaryZ[2], interpolateVaryZ[3], fZ);
501 : }
502 12 : interpolateVaryX[iLoopX+1] = CubicInterpolateScalar(interpolateVaryY[0], interpolateVaryY[1], interpolateVaryY[2], interpolateVaryY[3], fY);
503 : }
504 3 : double result = CubicInterpolateScalar(interpolateVaryX[0], interpolateVaryX[1], interpolateVaryX[2], interpolateVaryX[3], fX);
505 3 : return result;
506 3 : }
507 :
508 : /** Interpolate the grid trilinear at a given position */
509 100055 : T trilinearInterpolate(const Vector3d &position) const {
510 : /** position on a unit grid */
511 : Vector3d r = (position - gridOrigin) / spacing;
512 :
513 : /** indices of lower (0) and upper (1) neighbours. The neighbours span a grid
514 : with the origin at [iX0, iY0, iZ0] and the most distant corner [iX1, iY1, iZ1]. */
515 : int iX0, iX1, iY0, iY1, iZ0, iZ1;
516 : double resX, resY, resZ, fX0, fY0, fZ0;
517 :
518 100055 : if (reflective) {
519 6 : reflectiveClamp(r.x, Nx, iX0, iX1, resX);
520 6 : reflectiveClamp(r.y, Ny, iY0, iY1, resY);
521 6 : reflectiveClamp(r.z, Nz, iZ0, iZ1, resZ);
522 6 : fX0 = resX - floor(resX);
523 6 : fY0 = resY - floor(resY);
524 6 : fZ0 = resZ - floor(resZ);
525 : } else {
526 100049 : periodicClamp(r.x, Nx, iX0, iX1);
527 100049 : periodicClamp(r.y, Ny, iY0, iY1);
528 100049 : periodicClamp(r.z, Nz, iZ0, iZ1);
529 100049 : fX0 = r.x - floor(r.x);
530 100049 : fY0 = r.y - floor(r.y);
531 100049 : fZ0 = r.z - floor(r.z);
532 : }
533 :
534 : /** linear fraction to upper neighbours based on lower neighbours calculated above */
535 100055 : double fX1 = 1 - fX0;
536 100055 : double fY1 = 1 - fY0;
537 100055 : double fZ1 = 1 - fZ0;
538 :
539 : /** trilinear interpolation (see http://paulbourke.net/miscellaneous/interpolation) */
540 : T b(0.);
541 100055 : b += get(iX0, iY0, iZ0) * fX1 * fY1 * fZ1;
542 100055 : b += get(iX1, iY0, iZ0) * fX0 * fY1 * fZ1;
543 100055 : b += get(iX0, iY1, iZ0) * fX1 * fY0 * fZ1;
544 100055 : b += get(iX0, iY0, iZ1) * fX1 * fY1 * fZ0;
545 9 : b += get(iX1, iY0, iZ1) * fX0 * fY1 * fZ0;
546 9 : b += get(iX0, iY1, iZ1) * fX1 * fY0 * fZ0;
547 9 : b += get(iX1, iY1, iZ0) * fX0 * fY0 * fZ1;
548 9 : b += get(iX1, iY1, iZ1) * fX0 * fY0 * fZ0;
549 :
550 100055 : return b;
551 : }
552 :
553 : }; // class Grid
554 :
555 : typedef Grid<double> Grid1d;
556 : typedef Grid<float> Grid1f;
557 : typedef Grid<Vector3f> Grid3f;
558 : typedef Grid<Vector3d> Grid3d;
559 :
560 : /** @}*/
561 :
562 : } // namespace crpropa
563 :
564 : #endif // CRPROPA_GRID_H
|
