6 #ifndef USML_TYPES_DATA_GRID_SVP_H
7 #define USML_TYPES_DATA_GRID_SVP_H
9 #include <usml/types/data_grid.h>
53 _kzmax(_axis[0]->size() - 1u),
54 _kxmax(_axis[1]->size() - 1u),
55 _kymax(_axis[2]->size() - 1u)
62 double slope_1, slope_2;
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];
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) {
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))
88 slope_2 = (data_3d(i + 2, j, k) - data_3d(i + 1, j, k))
91 ((2.0 * inc1 + inc2) * slope_1 - inc1 * slope_2)
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;
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)
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;
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))
120 slope_2 = (data_3d(i, j, k) - data_3d(i - 1, j, k))
123 ((2.0 * inc1 + inc2) * slope_2 - inc1 * slope_1)
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;
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)
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;
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))
154 slope_2 = (data_3d(i + 1, j, k) - data_3d(i, j, k))
156 if (slope_1 * slope_2 <= 0.0) {
157 derv_z[i][j][k] = 0.0;
159 derv_z[i][j][k] = (w1 + w2)
160 / ((w1 / slope_1) + (w2 / slope_2));
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)
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;
202 double f11, f21, f12, f22, x_diff, y_diff;
203 double x, x1, x2, y, y1, y2;
209 double h00, h10, h01, h11;
213 for (
unsigned dim = 0; dim < 3; ++dim) {
217 if (edge_limit(dim)) {
218 double a = *(_axis[dim]->begin());
219 double b = *(_axis[dim]->rbegin());
220 double inc = _axis[dim]->increment(0);
222 if (location[dim] >= a) {
225 }
else if (location[dim] <= b) {
227 _offset[dim] = _axis[dim]->size() - 2;
229 _offset[dim] = _axis[dim]->find_index(location[dim]);
233 if (location[dim] <= a) {
236 }
else if (location[dim] >= b) {
238 _offset[dim] = _axis[dim]->size() - 2;
240 _offset[dim] = _axis[dim]->find_index(location[dim]);
247 _offset[dim] = _axis[dim]->find_index(location[dim]);
251 #ifdef FAST_GRID_DEBUG
252 cout <<
"_offset: (" << _offset[0] <<
"," << _offset[1] <<
"," << _offset[2] <<
")" << endl;
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;
274 for (
int i = 0; i < 2; ++i) {
275 for (
int j = 0; j < 2; ++j) {
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);
281 t = (location[0] - (*_axis[0])(k0)) / inc1;
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);
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];
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;
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]
320 x1 = (*_axis[1])(k1);
321 x2 = (*_axis[1])(k1 + 1);
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);
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;
339 result = (f11 * (x2 - x) * (y2 - y) + f21 * (x - x1) * (y2 - y)
340 + f12 * (x2 - x) * (y - y1) + f22 * (x - x1) * (y - y1))
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);
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)
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);
386 (*result)(n, m) = (
double) interpolate(location,
388 (*dx)(n, m) = (
double) derivative[0];
389 (*dy)(n, m) = (
double) derivative[1];
390 (*dz)(n, m) = (
double) derivative[2];
400 inline double data_3d(
unsigned dim0,
unsigned dim1,
unsigned dim2)
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);
420 c_matrix<double, 2, 2>
_dz;
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