6 #ifndef USML_TYPES_DATA_GRID_BATHY_H
7 #define USML_TYPES_DATA_GRID_BATHY_H
9 #include <usml/types/data_grid.h>
10 #include <usml/types/seq_vector.h>
52 data_grid<double, 2>(grid, copy_data), _bicubic_coeff(16, 1),
53 _field(16, 1), _xyloc(1, 16), _result_pchip(1, 1), _value(4, 4),
54 _kmin(0u), _k0max(_axis[0]->size() - 1u), _k1max(_axis[1]->size() - 1u)
57 _inv_bicubic_coeff = zero_matrix<double>(16, 16);
58 _inv_bicubic_coeff(0, 0) = 1;
59 _inv_bicubic_coeff(1, 8) = 1;
60 _inv_bicubic_coeff(2, 0) = -3;
61 _inv_bicubic_coeff(2, 1) = 3;
62 _inv_bicubic_coeff(2, 8) = -2;
63 _inv_bicubic_coeff(2, 9) = -1;
64 _inv_bicubic_coeff(3, 0) = 2;
65 _inv_bicubic_coeff(3, 1) = -2;
66 _inv_bicubic_coeff(3, 8) = _inv_bicubic_coeff(3, 9) = 1;
67 _inv_bicubic_coeff(4, 4) = 1;
68 _inv_bicubic_coeff(5, 12) = 1;
69 _inv_bicubic_coeff(6, 4) = -3;
70 _inv_bicubic_coeff(6, 5) = 3;
71 _inv_bicubic_coeff(6, 12) = -2;
72 _inv_bicubic_coeff(6, 13) = -1;
73 _inv_bicubic_coeff(7, 4) = 2;
74 _inv_bicubic_coeff(7, 5) = -2;
75 _inv_bicubic_coeff(7, 12) = _inv_bicubic_coeff(7, 13) = 1;
76 _inv_bicubic_coeff(8, 0) = -3;
77 _inv_bicubic_coeff(8, 2) = 3;
78 _inv_bicubic_coeff(8, 4) = -2;
79 _inv_bicubic_coeff(8, 6) = -1;
80 _inv_bicubic_coeff(9, 8) = -3;
81 _inv_bicubic_coeff(9, 10) = 3;
82 _inv_bicubic_coeff(9, 12) = -2;
83 _inv_bicubic_coeff(9, 14) = -1;
84 _inv_bicubic_coeff(10, 0) = _inv_bicubic_coeff(10, 3) = 9;
85 _inv_bicubic_coeff(10, 1) = _inv_bicubic_coeff(10, 2) = -9;
86 _inv_bicubic_coeff(10, 4) = _inv_bicubic_coeff(10, 8) = 6;
87 _inv_bicubic_coeff(10, 5) = _inv_bicubic_coeff(10, 10) = -6;
88 _inv_bicubic_coeff(10, 6) = _inv_bicubic_coeff(10, 9) = 3;
89 _inv_bicubic_coeff(10, 7) = _inv_bicubic_coeff(10, 11) = -3;
90 _inv_bicubic_coeff(10, 12) = 4;
91 _inv_bicubic_coeff(10, 13) = _inv_bicubic_coeff(10, 14) = 2;
92 _inv_bicubic_coeff(10, 15) = 1;
93 _inv_bicubic_coeff(11, 0) = _inv_bicubic_coeff(11, 3) = -6;
94 _inv_bicubic_coeff(11, 1) = _inv_bicubic_coeff(11, 2) = 6;
95 _inv_bicubic_coeff(11, 6) = _inv_bicubic_coeff(11, 12) =
96 _inv_bicubic_coeff(11, 13) = -2;
97 _inv_bicubic_coeff(11, 4) = -4;
98 _inv_bicubic_coeff(11, 5) = 4;
99 _inv_bicubic_coeff(11, 7) = 2;
100 _inv_bicubic_coeff(11, 8) = _inv_bicubic_coeff(11, 9) = -3;
101 _inv_bicubic_coeff(11, 10) = _inv_bicubic_coeff(11, 11) = 3;
102 _inv_bicubic_coeff(11, 14) = _inv_bicubic_coeff(11, 15) = -1;
103 _inv_bicubic_coeff(12, 0) = 2;
104 _inv_bicubic_coeff(12, 2) = -2;
105 _inv_bicubic_coeff(12, 4) = _inv_bicubic_coeff(12, 6) = 1;
106 _inv_bicubic_coeff(13, 8) = 2;
107 _inv_bicubic_coeff(13, 10) = -2;
108 _inv_bicubic_coeff(13, 12) = _inv_bicubic_coeff(13, 14) = 1;
109 _inv_bicubic_coeff(14, 0) = _inv_bicubic_coeff(14, 3) = -6;
110 _inv_bicubic_coeff(14, 1) = _inv_bicubic_coeff(14, 2) = 6;
111 _inv_bicubic_coeff(14, 4) = _inv_bicubic_coeff(14, 6) = -3;
112 _inv_bicubic_coeff(14, 5) = _inv_bicubic_coeff(14, 7) = 3;
113 _inv_bicubic_coeff(14, 8) = -4;
114 _inv_bicubic_coeff(14, 10) = 4;
115 _inv_bicubic_coeff(14, 9) = _inv_bicubic_coeff(14, 12) =
116 _inv_bicubic_coeff(14, 14) = -2;
117 _inv_bicubic_coeff(14, 11) = 2;
118 _inv_bicubic_coeff(14, 13) = _inv_bicubic_coeff(14, 15) = -1;
119 _inv_bicubic_coeff(15, 0) = _inv_bicubic_coeff(15, 3) = 4;
120 _inv_bicubic_coeff(15, 1) = _inv_bicubic_coeff(15, 2) = -4;
121 _inv_bicubic_coeff(15, 4) = _inv_bicubic_coeff(15, 6) =
122 _inv_bicubic_coeff(15, 8) = _inv_bicubic_coeff(15, 9) = 2;
123 _inv_bicubic_coeff(15, 5) = _inv_bicubic_coeff(15, 7) =
124 _inv_bicubic_coeff(15, 10) = _inv_bicubic_coeff(15, 11) = -2;
125 _inv_bicubic_coeff(15, 12) = _inv_bicubic_coeff(15, 13) =
126 _inv_bicubic_coeff(15, 14) = _inv_bicubic_coeff(15, 15) = 1;
132 matrix<double> inc_x(_k0max + 1u, 1);
133 for (
unsigned i = 0; i < _k0max + 1u; ++i) {
134 if (i == 0 || i == _k0max) {
137 inc_x(i, 0) = (_axis[0]->increment(i - 1)
138 + _axis[0]->increment(i + 1)) / _axis[0]->increment(i);
141 matrix<double> inc_y(_k1max + 1u, 1);
142 for (
unsigned i = 0; i < _k1max + 1u; ++i) {
143 if (i == 0 || i == _k1max) {
146 inc_y(i, 0) = (_axis[1]->increment(i - 1)
147 + _axis[1]->increment(i + 1)) / _axis[1]->increment(i);
152 _derv_x = matrix<double>(_k0max + 1u, _k1max + 1u);
153 _derv_y = matrix<double>(_k0max + 1u, _k1max + 1u);
154 _derv_x_y = matrix<double>(_k0max + 1u, _k1max + 1u);
155 for (
unsigned i = 0; i < _k0max + 1u; ++i) {
156 for (
unsigned j = 0; j < _k1max + 1u; ++j) {
157 if (i < 1 && j < 1) {
158 #ifdef DERV_CONSTRUCT
159 cout <<
"***Condition: i<1 && j<1***" << endl;
161 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i, j))
163 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j))
165 _derv_x_y(i, j) = (data_2d(i + 1, j + 1) - data_2d(i + 1, j)
166 - data_2d(i, j + 1) + data_2d(i, j))
167 / (inc_x(i, 0) * inc_y(j, 0));
168 }
else if (i == _k0max && j == _k1max) {
169 #ifdef DERV_CONSTRUCT
170 cout <<
"***Condition: i==_k0max && j==_k1max***" << endl;
172 _derv_x(i, j) = (data_2d(i, j) - data_2d(i - 1, j))
174 _derv_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1))
176 _derv_x_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1)
177 - data_2d(i - 1, j) + data_2d(i - 1, j - 1))
178 / (inc_x(i, 0) * inc_y(j, 0));
179 }
else if (i < 1 && j == _k1max) {
180 #ifdef DERV_CONSTRUCT
181 cout <<
"***Condition: i<1 && j==_k1max***" << endl;
183 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i, j))
185 _derv_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1))
187 _derv_x_y(i, j) = (data_2d(i + 1, j) - data_2d(i + 1, j - 1)
188 - data_2d(i, j) + data_2d(i, j - 1))
189 / (inc_x(i, 0) * inc_y(j, 0));
190 }
else if (j < 1 && i == _k0max) {
191 #ifdef DERV_CONSTRUCT
192 cout <<
"***Condition: j<1 && i==_k0max***" << endl;
194 _derv_x(i, j) = (data_2d(i, j) - data_2d(i - 1, j))
196 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j))
198 _derv_x_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j)
199 - data_2d(i - 1, j + 1) + data_2d(i - 1, j))
200 / (inc_x(i, 0) * inc_y(j, 0));
201 }
else if (i < 1 && (1 <= j && j < _k1max)) {
202 #ifdef DERV_CONSTRUCT
203 cout <<
"***Condition: i<1 && (1<=j && j<_k1max)***" << endl;
205 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i, j))
207 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1))
209 _derv_x_y(i, j) = (data_2d(i + 1, j + 1)
210 - data_2d(i + 1, j - 1) - data_2d(i, j + 1)
211 + data_2d(i, j - 1)) / (inc_x(i, 0) * inc_y(j, 0));
212 }
else if (j < 1 && (1 <= i && i < _k0max)) {
213 #ifdef DERV_CONSTRUCT
214 cout <<
"***Condition: j<1 && (1<=i && i<_k0max)***" << endl;
216 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i - 1, j))
218 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j))
220 _derv_x_y(i, j) = (data_2d(i + 1, j + 1) - data_2d(i + 1, j)
221 - data_2d(i - 1, j + 1) + data_2d(i - 1, j))
222 / (inc_x(i, 0) * inc_y(j, 0));
223 }
else if (j == _k1max && (1 <= i && i < _k0max)) {
224 #ifdef DERV_CONSTRUCT
225 cout <<
"***Condition: j>_k1max && (1<=i && i<_k0max)***" << endl;
227 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i - 1, j))
229 _derv_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1))
231 _derv_x_y(i, j) = (data_2d(i + 1, j) - data_2d(i + 1, j - 1)
232 - data_2d(i - 1, j) + data_2d(i - 1, j - 1))
233 / (inc_x(i, 0) * inc_y(j, 0));
234 }
else if (i == _k0max && (1 <= j && j < _k1max)) {
235 #ifdef DERV_CONSTRUCT
236 cout <<
"***Condition: i>_k0max && (1<=j && j<_k1max)***" << endl;
238 _derv_x(i, j) = (data_2d(i, j) - data_2d(i - 1, j))
240 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1))
242 _derv_x_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1)
243 - data_2d(i - 1, j + 1) + data_2d(i - 1, j - 1))
244 / (inc_x(i, 0) * inc_y(j, 0));
246 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i - 1, j))
248 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1))
250 _derv_x_y(i, j) = (data_2d(i + 1, j + 1)
251 - data_2d(i + 1, j - 1) - data_2d(i - 1, j + 1)
252 + data_2d(i - 1, j - 1))
253 / (inc_x(i, 0) * inc_y(j, 0));
277 for (
unsigned dim = 0; dim < 2; ++dim) {
281 if (edge_limit(dim)) {
282 double a = *(_axis[dim]->begin());
283 double b = *(_axis[dim]->rbegin());
284 double inc = _axis[dim]->increment(0);
286 if (location[dim] >= a) {
289 }
else if (location[dim] <= b) {
291 _offset[dim] = _axis[dim]->size() - 2;
293 _offset[dim] = _axis[dim]->find_index(location[dim]);
297 if (location[dim] <= a) {
300 }
else if (location[dim] >= b) {
302 _offset[dim] = _axis[dim]->size() - 2;
304 _offset[dim] = _axis[dim]->find_index(location[dim]);
311 _offset[dim] = _axis[dim]->find_index(location[dim]);
315 switch (interp_type(0)) {
319 for (
int dim = 0; dim < 2; ++dim) {
320 double inc = _axis[dim]->increment(0);
321 double u =
abs(location[dim] - (*_axis[dim])(_offset[dim]))
324 _fast_index[dim] = _offset[dim];
326 _fast_index[dim] = _offset[dim] + 1;
330 derivative[0] = derivative[1] = 0;
331 return data(_fast_index);
336 double f11, f21, f12, f22, x_diff, y_diff;
337 double x, x1, x2, y, y1, y2;
340 x1 = (*_axis[0])(_offset[0]);
341 x2 = (*_axis[0])(_offset[0] + 1);
343 y1 = (*_axis[1])(_offset[1]);
344 y2 = (*_axis[1])(_offset[1] + 1);
346 _fast_index[0] = _offset[0] + 1;
347 _fast_index[1] = _offset[1];
348 f21 = data(_fast_index);
349 _fast_index[0] = _offset[0];
350 _fast_index[1] = _offset[1] + 1;
351 f12 = data(_fast_index);
352 _fast_index[0] = _offset[0] + 1;
353 _fast_index[1] = _offset[1] + 1;
354 f22 = data(_fast_index);
357 result = (f11 * (x2 - x) * (y2 - y) + f21 * (x - x1) * (y2 - y)
358 + f12 * (x2 - x) * (y - y1) + f22 * (x - x1) * (y - y1))
361 derivative[0] = (f21 * (y2 - y) - f11 * (y2 - y)
362 + f22 * (y - y1) - f12 * (y - y1)) / (x_diff * y_diff);
363 derivative[1] = (f12 * (x2 - x) - f11 * (x2 - x)
364 + f22 * (x - x1) - f21 * (x - x1)) / (x_diff * y_diff);
371 result = fast_pchip(_offset, location, derivative);
376 throw std::invalid_argument(
377 "Interp must be NEAREST, LINEAR, or PCHIP");
395 void interpolate(
const matrix<double>& x,
const matrix<double>& y,
396 matrix<double>* result, matrix<double>* dx = NULL,
397 matrix<double>* dy = NULL) {
399 double derivative[2];
400 for (
unsigned n = 0; n < x.size1(); ++n) {
401 for (
unsigned m = 0; m < x.size2(); ++m) {
402 location[0] = x(n, m);
403 location[1] = y(n, m);
404 if (dx == NULL || dy == NULL) {
405 (*result)(n, m) = (
double) interpolate(location);
407 (*result)(n, m) = (
double) interpolate(location,
409 (*dx)(n, m) = (
double) derivative[0];
410 (*dy)(n, m) = (
double) derivative[1];
419 inline double data_2d(
unsigned row,
unsigned col) {
420 unsigned grid_index[2];
423 return data(grid_index);
478 double fast_pchip(
const unsigned* interp_index,
double* location,
479 double* derivative = NULL) {
480 int k0 = interp_index[0];
481 int k1 = interp_index[1];
485 norm0 = (*_axis[0])(k0 + 1) - (*_axis[0])(k0);
486 norm1 = (*_axis[1])(k1 + 1) - (*_axis[1])(k1);
487 for (
int i = -1; i < 3; ++i) {
488 for (
int j = -1; j < 3; ++j) {
490 if ((k0 + i) >= _k0max) {
491 _fast_index[0] = _k0max;
492 }
else if ((k0 + i) <= _kmin) {
493 _fast_index[0] = _kmin;
495 _fast_index[0] = k0 + i;
498 if ((k1 + j) >= _k1max) {
499 _fast_index[1] = _k1max;
500 }
else if ((k1 + j) <= _kmin) {
501 _fast_index[1] = _kmin;
503 _fast_index[1] = k1 + j;
505 _value(i + 1, j + 1) = data(_fast_index);
509 #ifdef FAST_GRID_DEBUG
510 cout <<
"loc0: " << location[0] <<
" loc1: " << location[1] << endl;
511 cout <<
"offset0: " << k0 <<
" offset1: " << k1 << endl;
512 cout <<
"axis0: " << (*_axis[0])(k0) <<
" axis1: " << (*_axis[1])(k1) << endl;
513 cout <<
"data value at offset: " << ( (data(interp_index) > 1e6) ?
515 cout <<
"_value: " << _value << endl;
516 cout <<
"_derv_x: " << _derv_x << endl;
517 cout <<
"_derv_y: " << _derv_y << endl;
518 cout <<
"_derv_x_y: " << _derv_x_y << endl;
522 _field(0, 0) = _value(1, 1);
523 _field(1, 0) = _value(1, 2);
524 _field(2, 0) = _value(2, 1);
525 _field(3, 0) = _value(2, 2);
526 _field(4, 0) = _derv_x(k0, k1);
527 _field(5, 0) = _derv_x(k0, k1 + 1);
528 _field(6, 0) = _derv_x(k0 + 1, k1);
529 _field(7, 0) = _derv_x(k0 + 1, k1 + 1);
530 _field(8, 0) = _derv_y(k0, k1);
531 _field(9, 0) = _derv_y(k0, k1 + 1);
532 _field(10, 0) = _derv_y(k0 + 1, k1);
533 _field(11, 0) = _derv_y(k0 + 1, k1 + 1);
534 _field(12, 0) = _derv_x_y(k0, k1);
535 _field(13, 0) = _derv_x_y(k0, k1 + 1);
536 _field(14, 0) = _derv_x_y(k0 + 1, k1);
537 _field(15, 0) = _derv_x_y(k0 + 1, k1 + 1);
540 _bicubic_coeff = prod(_inv_bicubic_coeff, _field);
543 double x_inv = location[0] - (*_axis[0])(k0);
544 double y_inv = location[1] - (*_axis[1])(k1);
546 #ifdef FAST_GRID_DEBUG
547 cout <<
"_field: " << _field << endl;
548 cout <<
"_bicubic_coeff: " << _bicubic_coeff << endl;
549 cout <<
"x_inv/norm0: " << x_inv/norm0 <<
"\ty_inv/norm1: " << y_inv/norm1 << endl;
553 _xyloc(0, 1) = y_inv / norm1;
554 _xyloc(0, 2) = _xyloc(0, 1) * _xyloc(0, 1);
555 _xyloc(0, 3) = _xyloc(0, 2) * _xyloc(0, 1);
556 _xyloc(0, 4) = x_inv / norm0;
557 _xyloc(0, 5) = _xyloc(0, 4) * _xyloc(0, 1);
558 _xyloc(0, 6) = _xyloc(0, 4) * _xyloc(0, 2);
559 _xyloc(0, 7) = _xyloc(0, 4) * _xyloc(0, 3);
560 _xyloc(0, 8) = _xyloc(0, 4) * _xyloc(0, 4);
561 _xyloc(0, 9) = _xyloc(0, 8) * _xyloc(0, 1);
562 _xyloc(0, 10) = _xyloc(0, 8) * _xyloc(0, 2);
563 _xyloc(0, 11) = _xyloc(0, 8) * _xyloc(0, 3);
564 _xyloc(0, 12) = _xyloc(0, 8) * _xyloc(0, 4);
565 _xyloc(0, 13) = _xyloc(0, 12) * _xyloc(0, 1);
566 _xyloc(0, 14) = _xyloc(0, 12) * _xyloc(0, 2);
567 _xyloc(0, 15) = _xyloc(0, 12) * _xyloc(0, 3);
569 _result_pchip = prod(_xyloc, _bicubic_coeff);
573 for (
int i = 1; i < 4; ++i) {
574 for (
int j = 0; j < 4; ++j) {
575 derivative[0] += i * _bicubic_coeff(i * 4 + j, 0)
576 * _xyloc(0, 4 * (i - 1)) * _xyloc(0, j);
579 for (
int i = 0; i < 4; ++i) {
580 for (
int j = 1; j < 4; ++j) {
581 derivative[1] += j * _bicubic_coeff(i * 4 + j, 0)
582 * _xyloc(0, 4 * i) * _xyloc(0, j - 1);
586 return _result_pchip(0, 0);
614 unsigned _fast_index[2];
c_matrix< double, 1, 1 > _result_pchip
Definition: data_grid_bathy.h:609
static double earth_radius
Local radius of curvature in the area of operations.
Definition: wposition.h:100
const int _kmin
Definition: data_grid_bathy.h:615
matrix< double > _derv_y
Definition: data_grid_bathy.h:612
c_matrix< double, 16, 1 > _bicubic_coeff
Create variables that are used multiple times through out the course of multiple calls per instance...
Definition: data_grid_bathy.h:606
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
Implements fast calculations for data_grids using a non-recursive engine on interpolation.
Definition: data_grid_bathy.h:38
const int _k0max
Definition: data_grid_bathy.h:616
const int _k1max
Definition: data_grid_bathy.h:617
void interpolate(const matrix< double > &x, const matrix< double > &y, matrix< double > *result, matrix< double > *dx=NULL, matrix< double > *dy=NULL)
Overrides the interpolate function within data_grid using the non-recursive formula.
Definition: data_grid_bathy.h:395
matrix< double > _derv_x_y
Definition: data_grid_bathy.h:613
data_grid_bathy(const data_grid< double, 2 > &grid, bool copy_data=true)
Constructor - Creates a fast interpolation grid from an existing data_grid.
Definition: data_grid_bathy.h:51
c_matrix< double, 16, 16 > _inv_bicubic_coeff
Matrix that is the inverse of the bicubic coefficients This matrix will be used to construct the bicu...
Definition: data_grid_bathy.h:597
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_bathy.h:272
double data_2d(unsigned row, unsigned col)
Utility accessor function for data grid values.
Definition: data_grid_bathy.h:419
c_matrix< double, 1, 16 > _xyloc
Definition: data_grid_bathy.h:608
double fast_pchip(const unsigned *interp_index, double *location, double *derivative=NULL)
A non-recursive version of the Piecewise Cubic Hermite polynomial (PCHIP) specific to the 2-dimension...
Definition: data_grid_bathy.h:478
matrix< double > _derv_x
Definition: data_grid_bathy.h:611
c_matrix< double, 4, 4 > _value
Definition: data_grid_bathy.h:610
c_matrix< double, 16, 1 > _field
Definition: data_grid_bathy.h:607