USML
data_grid_svp.h
1 
6 #ifndef USML_TYPES_DATA_GRID_SVP_H
7 #define USML_TYPES_DATA_GRID_SVP_H
8 
9 #include <usml/types/data_grid.h>
10 
11 //#define FAST_GRID_DEBUG
12 //#define FAST_PCHIP_GRID_DEBUG
13 //#define FAST_NaN_GRID_DEBUG
14 
15 namespace usml {
16 namespace types {
19 
38 class USML_DECLSPEC data_grid_svp: public data_grid<double, 3> {
39 
40 public:
41 
51  data_grid_svp(const data_grid<double, 3>& grid, bool copy_data = true)
52  : data_grid<double, 3>(grid, copy_data),
53  _kzmax(_axis[0]->size() - 1u),
54  _kxmax(_axis[1]->size() - 1u),
55  _kymax(_axis[2]->size() - 1u)
56  {
57  double result = 0.0;
58 
59  //pchip variables
60  double inc1, inc2;
61  double w1, w2;
62  double slope_1, slope_2;
63 
64 
65  if (interp_type(0) != GRID_INTERP_PCHIP) {
66  interp_type(0, GRID_INTERP_PCHIP);
67  }
68  if ((interp_type(1) != GRID_INTERP_LINEAR)
69  || (interp_type(1) != GRID_INTERP_LINEAR)) {
70  interp_type(1, GRID_INTERP_LINEAR);
71  interp_type(2, GRID_INTERP_LINEAR);
72  }
73  derv_z = new double**[_kzmax + 1u];
74  for (int i = 0; i < _kzmax + 1u; ++i) {
75  derv_z[i] = new double*[_kxmax + 1u];
76  for (int j = 0; j < _kxmax + 1u; ++j) {
77  derv_z[i][j] = new double[_kymax + 1u];
78  }
79  }
80  for (int i = 0; i < _kzmax + 1u; ++i) {
81  for (int j = 0; j < _kxmax + 1u; ++j) {
82  for (int k = 0; k < _kymax + 1u; ++k) {
83  if (i == 0) {
84  inc1 = _axis[0]->increment(i);
85  inc2 = _axis[0]->increment(i + 1);
86  slope_1 = (data_3d(i + 1, j, k) - data_3d(i, j, k))
87  / inc1;
88  slope_2 = (data_3d(i + 2, j, k) - data_3d(i + 1, j, k))
89  / inc2;
90  result =
91  ((2.0 * inc1 + inc2) * slope_1 - inc1 * slope_2)
92  / (inc1 + inc2);
93  derv_z[i][j][k] = result;
94  if (result * slope_1 <= 0.0) {
95  derv_z[i][j][k] = 0.0;
96  } else if ((slope_1 * slope_2 <= 0.0)
97  && (abs(result) > abs(3.0 * slope_1))) {
98  derv_z[i][j][k] = 3.0 * slope_1;
99  }
100  #ifdef FAST_NaN_GRID_DEBUG
101  if(abs(derv_z[i][j][k])>=20.0) {
102  cout << "***Warning: bogus derivative in the z-direction***" << endl;
103  cout << "---i==0 condition---" << endl;
104  cout << "inc1: " << inc1 << "\tinc2: " << inc2 << endl;
105  cout << "data(" << i << "," << j << "," << k << "): " << data_3d(i,j,k)
106  << "\tdata(" << i+1 << "," << j << "," << k << "): " << data_3d(i+1,j,k)
107  << "\tdata(" << i+2 << "," << j << "," << k << "): " << data_3d(i+2,j,k)
108  << endl;
109  cout << "slope1: " << slope_1 << "\tslope2: " << slope_2 << endl;
110  cout << "result: " << result << endl;
111  cout << "derv_z[" << i << "][" << j << "][" << k << "]: "
112  << derv_z[i][j][k] << endl;
113  }
114  #endif
115  } else if (i == _kzmax) {
116  inc1 = _axis[0]->increment(i - 1);
117  inc2 = _axis[0]->increment(i);
118  slope_1 = (data_3d(i - 1, j, k) - data_3d(i - 2, j, k))
119  / inc1;
120  slope_2 = (data_3d(i, j, k) - data_3d(i - 1, j, k))
121  / inc2;
122  result =
123  ((2.0 * inc1 + inc2) * slope_2 - inc1 * slope_1)
124  / (inc1 + inc2);
125  derv_z[i][j][k] = result;
126  if (result * slope_1 <= 0.0) {
127  derv_z[i][j][k] = 0.0;
128  } else if ((slope_1 * slope_2 <= 0.0)
129  && (abs(result) > abs(3.0 * slope_1))) {
130  derv_z[i][j][k] = 3.0 * slope_1;
131  }
132  #ifdef FAST_NaN_GRID_DEBUG
133  if(abs(derv_z[i][j][k])>=20.0) {
134  cout << "***Warning: bogus derivative in the z-direction***" << endl;
135  cout << "---i==_kzmax condition---" << endl;
136  cout << "inc1: " << inc1 << "\tinc2: " << inc2 << endl;
137  cout << "data(" << i-2 << "," << j << "," << k << "): " << data_3d(i-2,j,k)
138  << "\tdata(" << i-1 << "," << j << "," << k << "): " << data_3d(i-1,j,k)
139  << "\tdata(" << i << "," << j << "," << k << "): " << data_3d(i,j,k)
140  << endl;
141  cout << "slope1: " << slope_1 << "\tslope2: " << slope_2 << endl;
142  cout << "result: " << result << endl;
143  cout << "derv_z[" << i << "][" << j << "][" << k << "]: "
144  << derv_z[i][j][k] << endl;
145  }
146  #endif
147  } else {
148  inc1 = _axis[0]->increment(i - 1);
149  inc2 = _axis[0]->increment(i);
150  w1 = 2.0 * inc2 + inc1;
151  w2 = inc2 + 2.0 * inc1;
152  slope_1 = (data_3d(i, j, k) - data_3d(i - 1, j, k))
153  / inc1;
154  slope_2 = (data_3d(i + 1, j, k) - data_3d(i, j, k))
155  / inc2;
156  if (slope_1 * slope_2 <= 0.0) {
157  derv_z[i][j][k] = 0.0;
158  } else {
159  derv_z[i][j][k] = (w1 + w2)
160  / ((w1 / slope_1) + (w2 / slope_2));
161  }
162  #ifdef FAST_NaN_GRID_DEBUG
163  if(abs(derv_z[i][j][k])>=20.0) {
164  cout << "***Warning: bogus derivative in the z-direction***" << endl;
165  cout << "---else condition---" << endl;
166  cout << "inc1: " << inc1 << "\tinc2: " << inc2 << endl;
167  cout << "data(" << i-1 << "," << j << "," << k << "): " << data_3d(i-1,j,k)
168  << "\tdata(" << i << "," << j << "," << k << "): " << data_3d(i,j,k)
169  << "\tdata(" << i+1 << "," << j << "," << k << "): " << data_3d(i+1,j,k)
170  << endl;
171  cout << "slope1: " << slope_1 << "\tslope2: " << slope_2 << endl;
172  cout << "w1: " << w1 << "\tw2: " << w2 << endl;
173  cout << "derv_z[" << i << "][" << j << "][" << k << "]: "
174  << derv_z[i][j][k] << endl;
175  }
176  #endif
177  }
178  } //end for-loop in k
179  } //end for-loop in j
180  } //end for-loop in i
181 
182  } // end Constructor
183 
196  double interpolate(double* location, double* derivative = NULL)
197  {
198  double result = 0.0;
199  unsigned k0, k1, k2; //indices of he offset data
200 
201  //bi-linear variables
202  double f11, f21, f12, f22, x_diff, y_diff;
203  double x, x1, x2, y, y1, y2;
204 
205  //pchip variables
206  double v1, v2;
207  double inc1;
208  double t, t_2, t_3;
209  double h00, h10, h01, h11;
210 
211  // find the interval index in each dimension
212 
213  for (unsigned dim = 0; dim < 3; ++dim) {
214 
215  // limit interpolation to axis domain if _edge_limit turned on
216 
217  if (edge_limit(dim)) {
218  double a = *(_axis[dim]->begin());
219  double b = *(_axis[dim]->rbegin());
220  double inc = _axis[dim]->increment(0);
221  if (inc < 0) { // a > b
222  if (location[dim] >= a) { //left of the axis
223  location[dim] = a;
224  _offset[dim] = 0;
225  } else if (location[dim] <= b) { //right of the axis
226  location[dim] = b;
227  _offset[dim] = _axis[dim]->size() - 2;
228  } else {
229  _offset[dim] = _axis[dim]->find_index(location[dim]); //somewhere in-between the endpoints of the axis
230  }
231  }
232  if (inc > 0) { // a < b
233  if (location[dim] <= a) { //left of the axis
234  location[dim] = a;
235  _offset[dim] = 0;
236  } else if (location[dim] >= b) { //right of the axis
237  location[dim] = b;
238  _offset[dim] = _axis[dim]->size() - 2;
239  } else {
240  _offset[dim] = _axis[dim]->find_index(location[dim]); //somewhere in-between the endpoints of the axis
241  }
242  }
243 
244  // allow extrapolation if _edge_limit turned off
245 
246  } else {
247  _offset[dim] = _axis[dim]->find_index(location[dim]);
248  }
249  }
250 
251  #ifdef FAST_GRID_DEBUG
252  cout << "_offset: (" << _offset[0] << "," << _offset[1] << "," << _offset[2] << ")" << endl;
253  cout << "axis[0]: ";
254  ( (*_axis[0])(_offset[0]) >= 1e6 ) ? (cout << (*_axis[0])(_offset[0])-wposition::earth_radius)
255  : cout << (*_axis[0])(_offset[0]);
256  cout << "\taxis[1]: " << (*_axis[1])(_offset[1])
257  << "\taxis[2]: " << (*_axis[2])(_offset[2]) << endl;
258  cout << "derv_z: (" << derv_z[_offset[0]][_offset[1]][_offset[2]]
259  << ", " << derv_z[_offset[0]+1][_offset[1]][_offset[2]]
260  << ", " << derv_z[_offset[0]+2][_offset[1]][_offset[2]]
261  << ", " << derv_z[_offset[0]+3][_offset[1]][_offset[2]] << ")" << endl;
262  #endif
263 
264  //** PCHIP contribution in zeroth dimension */
265  if (derivative) {
266  derivative[0] = 0;
267  }
268  k0 = _offset[0];
269  k1 = _offset[1];
270  k2 = _offset[2];
271 
272  // construct the interpolated plane to which the final bi-linear
273  // interpolation will happen
274  for (int i = 0; i < 2; ++i) {
275  for (int j = 0; j < 2; ++j) {
276  //extract data and take precautions when at boundaries of the axis
277  v1 = data_3d(k0, k1 + i, k2 + j);
278  v2 = data_3d(k0 + 1, k1 + i, k2 + j);
279  inc1 = _axis[0]->increment(k0);
280 
281  t = (location[0] - (*_axis[0])(k0)) / inc1;
282  t_2 = t * t;
283  t_3 = t_2 * t;
284 
285  //construct the hermite polynomials
286  h00 = (2 * t_3 - 3 * t_2 + 1);
287  h10 = (t_3 - 2 * t_2 + t);
288  h01 = (3 * t_2 - 2 * t_3);
289  h11 = (t_3 - t_2);
290 
291  _interp_plane(i, j) = h00 * v1 + h10 * derv_z[k0][k1 + i][k2 + j]
292  + h01 * v2 + h11 * derv_z[k0 + 1][k1 + i][k2 + j];
293 
294  #ifdef FAST_PCHIP_GRID_DEBUG
295  cout << "v1: " << v1 << "\tv2: " << v2 << endl;
296  cout << "inc1: " << inc1 << endl;
297  cout << "t: " << t << "\tt_2: " << t_2
298  << "\tt_3: " << t_3 << endl;
299  cout << "slope_1: " << derv_z[k0][k1+i][k2+j]
300  << "\tslope_2: " << derv_z[k0+1][k1+i][k2+j] << endl;
301  cout << "h00: " << h00 << "\th10: " << h10
302  << "\th01: " << h01 << "\th11: " << h11 << endl;
303  cout << "_interp_plane(" << i << ", " << j << "): "
304  << _interp_plane(i,j) << endl;
305  #endif
306 
307  if (derivative) {
308  _dz(i, j) = (6 * t_2 - 6 * t) * v1 / inc1
309  + (3 * t_2 - 4 * t + 1) * derv_z[k0][k1 + i][k2 + j]
310  / inc1 + (6 * t - 6 * t_2) * v2 / inc1
311  + (3 * t_2 - 2 * t) * derv_z[k0 + 1][k1 + i][k2 + j]
312  / inc1;
313  }
314  }
315  }
316 
317  //** Bi-Linear contributions from first/second dimensions */
318  //extract data around field point
319  x = location[1];
320  x1 = (*_axis[1])(k1);
321  x2 = (*_axis[1])(k1 + 1);
322  y = location[2];
323  y1 = (*_axis[2])(k2);
324  y2 = (*_axis[2])(k2 + 1);
325  f11 = _interp_plane(0, 0);
326  f21 = _interp_plane(1, 0);
327  f12 = _interp_plane(0, 1);
328  f22 = _interp_plane(1, 1);
329  x_diff = x2 - x1;
330  y_diff = y2 - y1;
331 
332  #ifdef FAST_GRID_DEBUG
333  cout << "k_indecies: (" << k0 << ", " << k1 << ", " << k2 << ")" << endl;
334  cout << "x: " << x << "\tx1: " << x1 << "\tx2: " << x2 << endl;
335  cout << "y: " << y << "\ty1: " << y1 << "\ty2: " << y2 << endl;
336  cout << "f11: " << f11 << "\tf21: " << f21 << "\tf12: " << f12 << "\tf22: " << f22 << endl;
337  #endif
338 
339  result = (f11 * (x2 - x) * (y2 - y) + f21 * (x - x1) * (y2 - y)
340  + f12 * (x2 - x) * (y - y1) + f22 * (x - x1) * (y - y1))
341  / (x_diff * y_diff);
342 
343  if (derivative) {
344  derivative[0] = (_dz(0, 0) * (x2 - x) * (y2 - y)
345  + _dz(1, 0) * (x - x1) * (y2 - y)
346  + _dz(0, 1) * (x2 - x) * (y - y1)
347  + _dz(1, 1) * (x - x1) * (y - y1)) / (x_diff * y_diff);
348  derivative[1] = (-f11 * (y2 - y) + f21 * (y2 - y) - f12 * (y - y1)
349  + f22 * (y - y1)) / (x_diff * y_diff);
350  derivative[2] = (-f11 * (x2 - x) - f21 * (x - x1) + f12 * (x2 - x)
351  + f22 * (x - x1)) / (x_diff * y_diff);
352  }
353 
354  return result;
355 
356  } // end interpolate at a single location.
357 
371  void interpolate(const matrix<double>& x, const matrix<double>& y,
372  const matrix<double>& z, matrix<double>* result,
373  matrix<double>* dx = NULL, matrix<double>* dy = NULL,
374  matrix<double>* dz = NULL)
375  {
376  double location[3];
377  double derivative[3];
378  for (unsigned n = 0; n < x.size1(); ++n) {
379  for (unsigned m = 0; m < x.size2(); ++m) {
380  location[0] = x(n, m);
381  location[1] = y(n, m);
382  location[2] = z(n, m);
383  if (dx == NULL || dy == NULL || dz == NULL) {
384  (*result)(n, m) = (double) interpolate(location);
385  } else {
386  (*result)(n, m) = (double) interpolate(location,
387  derivative);
388  (*dx)(n, m) = (double) derivative[0];
389  (*dy)(n, m) = (double) derivative[1];
390  (*dz)(n, m) = (double) derivative[2];
391  }
392  }
393  }
394 
395  } // end interpolate
396 
397 private:
398 
400  inline double data_3d(unsigned dim0, unsigned dim1, unsigned dim2)
401  {
402  unsigned grid_index[3];
403  grid_index[0] = dim0;
404  grid_index[1] = dim1;
405  grid_index[2] = dim2;
406  return data(grid_index);
407 
408  } // end data_3d
409 
414  unsigned _kzmax, _kxmax, _kymax; //max index on z-axis (depth)
415 
416  //bi-linear variable
417  c_matrix<double, 2, 2> _interp_plane;
418 
419  //pchip variables
420  c_matrix<double, 2, 2> _dz;
421  double*** derv_z;
422 
423 }; // end data_grid_svp class
424 
425 } // end of namespace types
426 } // end of namespace usml
427 
428 #endif
data_grid_svp(const data_grid< double, 3 > &grid, bool copy_data=true)
Constructor - Creates a fast interpolation grid from an existing data_grid.
Definition: data_grid_svp.h:51
static double earth_radius
Local radius of curvature in the area of operations.
Definition: wposition.h:100
Implements fast calculations for data_grids using a non-recursive engine on interpolation.
Definition: data_grid_svp.h:38
Definition: data_grid.h:23
c_matrix< double, 2, 2 > _interp_plane
Definition: data_grid_svp.h:417
c_matrix< double, 2, 2 > _dz
Definition: data_grid_svp.h:420
BOOST_UBLAS_INLINE matrix_unary1_traits< E, scalar_abs< typename E::value_type > >::result_type abs(const matrix_expression< E > &e)
Magnitude of a complex matrix.
Definition: matrix_math.h:257
double data_3d(unsigned dim0, unsigned dim1, unsigned dim2)
Utility accessor function for data grid values.
Definition: data_grid_svp.h:400
void interpolate(const matrix< double > &x, const matrix< double > &y, const matrix< double > &z, matrix< double > *result, matrix< double > *dx=NULL, matrix< double > *dy=NULL, matrix< double > *dz=NULL)
Interpolation 3-D specialization where the arguments, and results, are matrix<double>.
Definition: data_grid_svp.h:371
unsigned _kzmax
Create all variables needed for each calculation once to same time and memory.
Definition: data_grid_svp.h:414
N-dimensional data set and its associated axes.
Definition: data_grid.h:64
double interpolate(double *location, double *derivative=NULL)
Overrides the interpolate function within data_grid using the non-recursive formula.
Definition: data_grid_svp.h:196
double *** derv_z
Definition: data_grid_svp.h:421
Definition: data_grid.h:24