00001
00006 #pragma once
00007
00008 #include <usml/types/data_grid.h>
00009 #include <usml/types/seq_vector.h>
00010
00011 namespace usml {
00012 namespace types {
00015
00034 class USML_DECLSPEC data_grid_bathy: public data_grid<double, 2> {
00035
00036 public:
00044 data_grid_bathy(const data_grid<double, 2>* grid) :
00045 data_grid<double, 2>(*grid, true), _bicubic_coeff(16, 1),
00046 _field(16, 1), _xyloc(1, 16), _result_pchip(1, 1), _value(4, 4),
00047 _kmin(0u), _k0max(_axis[0]->size() - 1u), _k1max(_axis[1]->size() - 1u)
00048 {
00049
00050 _inv_bicubic_coeff = zero_matrix<double>(16, 16);
00051 _inv_bicubic_coeff(0, 0) = 1;
00052 _inv_bicubic_coeff(1, 8) = 1;
00053 _inv_bicubic_coeff(2, 0) = -3;
00054 _inv_bicubic_coeff(2, 1) = 3;
00055 _inv_bicubic_coeff(2, 8) = -2;
00056 _inv_bicubic_coeff(2, 9) = -1;
00057 _inv_bicubic_coeff(3, 0) = 2;
00058 _inv_bicubic_coeff(3, 1) = -2;
00059 _inv_bicubic_coeff(3, 8) = _inv_bicubic_coeff(3, 9) = 1;
00060 _inv_bicubic_coeff(4, 4) = 1;
00061 _inv_bicubic_coeff(5, 12) = 1;
00062 _inv_bicubic_coeff(6, 4) = -3;
00063 _inv_bicubic_coeff(6, 5) = 3;
00064 _inv_bicubic_coeff(6, 12) = -2;
00065 _inv_bicubic_coeff(6, 13) = -1;
00066 _inv_bicubic_coeff(7, 4) = 2;
00067 _inv_bicubic_coeff(7, 5) = -2;
00068 _inv_bicubic_coeff(7, 12) = _inv_bicubic_coeff(7, 13) = 1;
00069 _inv_bicubic_coeff(8, 0) = -3;
00070 _inv_bicubic_coeff(8, 2) = 3;
00071 _inv_bicubic_coeff(8, 4) = -2;
00072 _inv_bicubic_coeff(8, 6) = -1;
00073 _inv_bicubic_coeff(9, 8) = -3;
00074 _inv_bicubic_coeff(9, 10) = 3;
00075 _inv_bicubic_coeff(9, 12) = -2;
00076 _inv_bicubic_coeff(9, 14) = -1;
00077 _inv_bicubic_coeff(10, 0) = _inv_bicubic_coeff(10, 3) = 9;
00078 _inv_bicubic_coeff(10, 1) = _inv_bicubic_coeff(10, 2) = -9;
00079 _inv_bicubic_coeff(10, 4) = _inv_bicubic_coeff(10, 8) = 6;
00080 _inv_bicubic_coeff(10, 5) = _inv_bicubic_coeff(10, 10) = -6;
00081 _inv_bicubic_coeff(10, 6) = _inv_bicubic_coeff(10, 9) = 3;
00082 _inv_bicubic_coeff(10, 7) = _inv_bicubic_coeff(10, 11) = -3;
00083 _inv_bicubic_coeff(10, 12) = 4;
00084 _inv_bicubic_coeff(10, 13) = _inv_bicubic_coeff(10, 14) = 2;
00085 _inv_bicubic_coeff(10, 15) = 1;
00086 _inv_bicubic_coeff(11, 0) = _inv_bicubic_coeff(11, 3) = -6;
00087 _inv_bicubic_coeff(11, 1) = _inv_bicubic_coeff(11, 2) = 6;
00088 _inv_bicubic_coeff(11, 6) = _inv_bicubic_coeff(11, 12) =
00089 _inv_bicubic_coeff(11, 13) = -2;
00090 _inv_bicubic_coeff(11, 4) = -4;
00091 _inv_bicubic_coeff(11, 5) = 4;
00092 _inv_bicubic_coeff(11, 7) = 2;
00093 _inv_bicubic_coeff(11, 8) = _inv_bicubic_coeff(11, 9) = -3;
00094 _inv_bicubic_coeff(11, 10) = _inv_bicubic_coeff(11, 11) = 3;
00095 _inv_bicubic_coeff(11, 14) = _inv_bicubic_coeff(11, 15) = -1;
00096 _inv_bicubic_coeff(12, 0) = 2;
00097 _inv_bicubic_coeff(12, 2) = -2;
00098 _inv_bicubic_coeff(12, 4) = _inv_bicubic_coeff(12, 6) = 1;
00099 _inv_bicubic_coeff(13, 8) = 2;
00100 _inv_bicubic_coeff(13, 10) = -2;
00101 _inv_bicubic_coeff(13, 12) = _inv_bicubic_coeff(13, 14) = 1;
00102 _inv_bicubic_coeff(14, 0) = _inv_bicubic_coeff(14, 3) = -6;
00103 _inv_bicubic_coeff(14, 1) = _inv_bicubic_coeff(14, 2) = 6;
00104 _inv_bicubic_coeff(14, 4) = _inv_bicubic_coeff(14, 6) = -3;
00105 _inv_bicubic_coeff(14, 5) = _inv_bicubic_coeff(14, 7) = 3;
00106 _inv_bicubic_coeff(14, 8) = -4;
00107 _inv_bicubic_coeff(14, 10) = 4;
00108 _inv_bicubic_coeff(14, 9) = _inv_bicubic_coeff(14, 12) =
00109 _inv_bicubic_coeff(14, 14) = -2;
00110 _inv_bicubic_coeff(14, 11) = 2;
00111 _inv_bicubic_coeff(14, 13) = _inv_bicubic_coeff(14, 15) = -1;
00112 _inv_bicubic_coeff(15, 0) = _inv_bicubic_coeff(15, 3) = 4;
00113 _inv_bicubic_coeff(15, 1) = _inv_bicubic_coeff(15, 2) = -4;
00114 _inv_bicubic_coeff(15, 4) = _inv_bicubic_coeff(15, 6) =
00115 _inv_bicubic_coeff(15, 8) = _inv_bicubic_coeff(15, 9) = 2;
00116 _inv_bicubic_coeff(15, 5) = _inv_bicubic_coeff(15, 7) =
00117 _inv_bicubic_coeff(15, 10) = _inv_bicubic_coeff(15, 11) = -2;
00118 _inv_bicubic_coeff(15, 12) = _inv_bicubic_coeff(15, 13) =
00119 _inv_bicubic_coeff(15, 14) = _inv_bicubic_coeff(15, 15) = 1;
00120
00121 _fast_index[0] = 0;
00122 _fast_index[1] = 0;
00123
00124
00125 matrix<double> inc_x(_k0max + 1u, 1);
00126 for (size_t i = 0; i < _k0max + 1u; ++i) {
00127 if (i == 0 || i == _k0max) {
00128 inc_x(i, 0) = 2;
00129 } else {
00130 inc_x(i, 0) = (_axis[0]->increment(i - 1)
00131 + _axis[0]->increment(i + 1)) / _axis[0]->increment(i);
00132 }
00133 }
00134 matrix<double> inc_y(_k1max + 1u, 1);
00135 for (size_t i = 0; i < _k1max + 1u; ++i) {
00136 if (i == 0 || i == _k1max) {
00137 inc_y(i, 0) = 2;
00138 } else {
00139 inc_y(i, 0) = (_axis[1]->increment(i - 1)
00140 + _axis[1]->increment(i + 1)) / _axis[1]->increment(i);
00141 }
00142 }
00143
00144
00145 _derv_x = matrix<double>(_k0max + 1u, _k1max + 1u);
00146 _derv_y = matrix<double>(_k0max + 1u, _k1max + 1u);
00147 _derv_x_y = matrix<double>(_k0max + 1u, _k1max + 1u);
00148 for (size_t i = 0; i < _k0max + 1u; ++i) {
00149 for (size_t j = 0; j < _k1max + 1u; ++j) {
00150 if (i < 1 && j < 1) {
00151 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i, j))
00152 / inc_x(i, 0);
00153 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j))
00154 / inc_y(j, 0);
00155 _derv_x_y(i, j) = (data_2d(i + 1, j + 1) - data_2d(i + 1, j)
00156 - data_2d(i, j + 1) + data_2d(i, j))
00157 / (inc_x(i, 0) * inc_y(j, 0));
00158 } else if (i == _k0max && j == _k1max) {
00159 _derv_x(i, j) = (data_2d(i, j) - data_2d(i - 1, j))
00160 / inc_x(i, 0);
00161 _derv_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1))
00162 / inc_y(j, 0);
00163 _derv_x_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1)
00164 - data_2d(i - 1, j) + data_2d(i - 1, j - 1))
00165 / (inc_x(i, 0) * inc_y(j, 0));
00166 } else if (i < 1 && j == _k1max) {
00167 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i, j))
00168 / inc_x(i, 0);
00169 _derv_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1))
00170 / inc_y(j, 0);
00171 _derv_x_y(i, j) = (data_2d(i + 1, j) - data_2d(i + 1, j - 1)
00172 - data_2d(i, j) + data_2d(i, j - 1))
00173 / (inc_x(i, 0) * inc_y(j, 0));
00174 } else if (j < 1 && i == _k0max) {
00175 _derv_x(i, j) = (data_2d(i, j) - data_2d(i - 1, j))
00176 / inc_x(i, 0);
00177 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j))
00178 / inc_y(j, 0);
00179 _derv_x_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j)
00180 - data_2d(i - 1, j + 1) + data_2d(i - 1, j))
00181 / (inc_x(i, 0) * inc_y(j, 0));
00182 } else if (i < 1 && (1 <= j && j < _k1max)) {
00183 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i, j))
00184 / inc_x(i, 0);
00185 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1))
00186 / inc_y(j, 0);
00187 _derv_x_y(i, j) = (data_2d(i + 1, j + 1)
00188 - data_2d(i + 1, j - 1) - data_2d(i, j + 1)
00189 + data_2d(i, j - 1)) / (inc_x(i, 0) * inc_y(j, 0));
00190 } else if (j < 1 && (1 <= i && i < _k0max)) {
00191 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i - 1, j))
00192 / inc_x(i, 0);
00193 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j))
00194 / inc_y(j, 0);
00195 _derv_x_y(i, j) = (data_2d(i + 1, j + 1) - data_2d(i + 1, j)
00196 - data_2d(i - 1, j + 1) + data_2d(i - 1, j))
00197 / (inc_x(i, 0) * inc_y(j, 0));
00198 } else if (j == _k1max && (1 <= i && i < _k0max)) {
00199 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i - 1, j))
00200 / inc_x(i, 0);
00201 _derv_y(i, j) = (data_2d(i, j) - data_2d(i, j - 1))
00202 / inc_y(j, 0);
00203 _derv_x_y(i, j) = (data_2d(i + 1, j) - data_2d(i + 1, j - 1)
00204 - data_2d(i - 1, j) + data_2d(i - 1, j - 1))
00205 / (inc_x(i, 0) * inc_y(j, 0));
00206 } else if (i == _k0max && (1 <= j && j < _k1max)) {
00207 _derv_x(i, j) = (data_2d(i, j) - data_2d(i - 1, j))
00208 / inc_x(i, 0);
00209 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1))
00210 / inc_y(j, 0);
00211 _derv_x_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1)
00212 - data_2d(i - 1, j + 1) + data_2d(i - 1, j - 1))
00213 / (inc_x(i, 0) * inc_y(j, 0));
00214 } else {
00215 _derv_x(i, j) = (data_2d(i + 1, j) - data_2d(i - 1, j))
00216 / inc_x(i, 0);
00217 _derv_y(i, j) = (data_2d(i, j + 1) - data_2d(i, j - 1))
00218 / inc_y(j, 0);
00219 _derv_x_y(i, j) = (data_2d(i + 1, j + 1)
00220 - data_2d(i + 1, j - 1) - data_2d(i - 1, j + 1)
00221 + data_2d(i - 1, j - 1))
00222 / (inc_x(i, 0) * inc_y(j, 0));
00223 }
00224 }
00225 }
00226 delete grid ;
00227 }
00228
00232 virtual ~data_grid_bathy() {}
00233
00247 double interpolate(double* location, double* derivative = NULL) {
00248
00249 double result = 0;
00250
00251
00252 for (size_t dim = 0; dim < 2; ++dim) {
00253
00254
00255
00256 if (edge_limit(dim)) {
00257 double a = *(_axis[dim]->begin()) ;
00258 double b = *(_axis[dim]->rbegin());
00259 double inc = _axis[dim]->increment(0);
00260 if (inc < 0) {
00261 if (location[dim] >= a) {
00262 location[dim] = a;
00263 _offset[dim] = 0;
00264 } else if (location[dim] <= b) {
00265 location[dim] = b;
00266 _offset[dim] = _axis[dim]->size() - 2;
00267 } else {
00268 _offset[dim] = _axis[dim]->find_index(location[dim]);
00269 }
00270 }
00271 if (inc > 0) {
00272 if (location[dim] <= a) {
00273 location[dim] = a;
00274 _offset[dim] = 0;
00275 } else if (location[dim] >= b) {
00276 location[dim] = b;
00277 _offset[dim] = _axis[dim]->size() - 2;
00278 } else {
00279 _offset[dim] = _axis[dim]->find_index(location[dim]);
00280 }
00281 }
00282
00283
00284
00285 } else {
00286 _offset[dim] = _axis[dim]->find_index(location[dim]);
00287 }
00288 }
00289
00290 switch (interp_type(0)) {
00291
00292
00293 case -1:
00294 for (int dim = 0; dim < 2; ++dim) {
00295 double inc = _axis[dim]->increment(0);
00296 double u = abs(location[dim] - (*_axis[dim])(_offset[dim]))
00297 / inc;
00298 if (u < 0.5) {
00299 _fast_index[dim] = _offset[dim];
00300 } else {
00301 _fast_index[dim] = _offset[dim] + 1;
00302 }
00303 }
00304 if (derivative)
00305 derivative[0] = derivative[1] = 0;
00306 return data(_fast_index);
00307 break;
00308
00309
00310 case 0:
00311 double f11, f21, f12, f22, x_diff, y_diff;
00312 double x, x1, x2, y, y1, y2;
00313
00314 x = location[0];
00315 x1 = (*_axis[0])(_offset[0]);
00316 x2 = (*_axis[0])(_offset[0] + 1);
00317 y = location[1];
00318 y1 = (*_axis[1])(_offset[1]);
00319 y2 = (*_axis[1])(_offset[1] + 1);
00320 f11 = data(_offset);
00321 _fast_index[0] = _offset[0] + 1;
00322 _fast_index[1] = _offset[1];
00323 f21 = data(_fast_index);
00324 _fast_index[0] = _offset[0];
00325 _fast_index[1] = _offset[1] + 1;
00326 f12 = data(_fast_index);
00327 _fast_index[0] = _offset[0] + 1;
00328 _fast_index[1] = _offset[1] + 1;
00329 f22 = data(_fast_index);
00330 x_diff = x2 - x1;
00331 y_diff = y2 - y1;
00332 result = (f11 * (x2 - x) * (y2 - y) + f21 * (x - x1) * (y2 - y)
00333 + f12 * (x2 - x) * (y - y1) + f22 * (x - x1) * (y - y1))
00334 / (x_diff * y_diff);
00335 if (derivative) {
00336 derivative[0] = (f21 * (y2 - y) - f11 * (y2 - y)
00337 + f22 * (y - y1) - f12 * (y - y1)) / (x_diff * y_diff);
00338 derivative[1] = (f12 * (x2 - x) - f11 * (x2 - x)
00339 + f22 * (x - x1) - f21 * (x - x1)) / (x_diff * y_diff);
00340 }
00341 return result;
00342 break;
00343
00344
00345 case 1:
00346 result = fast_pchip(_offset, location, derivative);
00347 return result;
00348 break;
00349
00350 default:
00351 throw std::invalid_argument(
00352 "Interp must be NEAREST, LINEAR, or PCHIP");
00353 break;
00354 }
00355 }
00356
00370 void interpolate(const matrix<double>& x, const matrix<double>& y,
00371 matrix<double>* result, matrix<double>* dx = NULL,
00372 matrix<double>* dy = NULL) {
00373 double location[2];
00374 double derivative[2];
00375 for (size_t n = 0; n < x.size1(); ++n) {
00376 for (size_t m = 0; m < x.size2(); ++m) {
00377 location[0] = x(n, m);
00378 location[1] = y(n, m);
00379 if (dx == NULL || dy == NULL) {
00380 (*result)(n, m) = (double) interpolate(location);
00381 } else {
00382 (*result)(n, m) = (double) interpolate(location,
00383 derivative);
00384 (*dx)(n, m) = (double) derivative[0];
00385 (*dy)(n, m) = (double) derivative[1];
00386 }
00387 }
00388 }
00389 }
00390
00391 private:
00392
00394 inline double data_2d(size_t row, size_t col) {
00395 size_t grid_index[2];
00396 grid_index[0] = row;
00397 grid_index[1] = col;
00398 return data(grid_index);
00399 }
00400
00453 double fast_pchip(const size_t* interp_index, double* location,
00454 double* derivative = NULL) {
00455 size_t k0 = interp_index[0];
00456 size_t k1 = interp_index[1];
00457 double norm0, norm1;
00458
00459
00460 norm0 = _axis[0]->increment(k0) ;
00461 norm1 = _axis[1]->increment(k1) ;
00462 for (int i = -1; i < 3; ++i) {
00463 for (int j = -1; j < 3; ++j) {
00464
00465 if ((k0 + i) >= _k0max) {
00466 _fast_index[0] = _k0max ;
00467 } else if ((k0 + i) <= _kmin) {
00468 _fast_index[0] = _kmin ;
00469 } else {
00470 _fast_index[0] = k0 + i ;
00471 }
00472
00473 if ((k1 + j) >= _k1max) {
00474 _fast_index[1] = _k1max ;
00475 } else if ((k1 + j) <= _kmin) {
00476 _fast_index[1] = _kmin ;
00477 } else {
00478 _fast_index[1] = k1 + j ;
00479 }
00480 _value(i + 1, j + 1) = data(_fast_index) ;
00481 }
00482 }
00483
00484
00485 _field(0, 0) = _value(1, 1);
00486 _field(1, 0) = _value(1, 2);
00487 _field(2, 0) = _value(2, 1);
00488 _field(3, 0) = _value(2, 2);
00489 _field(4, 0) = _derv_x(k0, k1);
00490 _field(5, 0) = _derv_x(k0, k1 + 1);
00491 _field(6, 0) = _derv_x(k0 + 1, k1);
00492 _field(7, 0) = _derv_x(k0 + 1, k1 + 1);
00493 _field(8, 0) = _derv_y(k0, k1);
00494 _field(9, 0) = _derv_y(k0, k1 + 1);
00495 _field(10, 0) = _derv_y(k0 + 1, k1);
00496 _field(11, 0) = _derv_y(k0 + 1, k1 + 1);
00497 _field(12, 0) = _derv_x_y(k0, k1);
00498 _field(13, 0) = _derv_x_y(k0, k1 + 1);
00499 _field(14, 0) = _derv_x_y(k0 + 1, k1);
00500 _field(15, 0) = _derv_x_y(k0 + 1, k1 + 1);
00501
00502
00503 _bicubic_coeff = prod(_inv_bicubic_coeff, _field);
00504
00505
00506 double x_inv = location[0] - (*_axis[0])(k0);
00507 double y_inv = location[1] - (*_axis[1])(k1);
00508
00509 _xyloc(0, 0) = 1;
00510 _xyloc(0, 1) = y_inv / norm1;
00511 _xyloc(0, 2) = _xyloc(0, 1) * _xyloc(0, 1);
00512 _xyloc(0, 3) = _xyloc(0, 2) * _xyloc(0, 1);
00513 _xyloc(0, 4) = x_inv / norm0;
00514 _xyloc(0, 5) = _xyloc(0, 4) * _xyloc(0, 1);
00515 _xyloc(0, 6) = _xyloc(0, 4) * _xyloc(0, 2);
00516 _xyloc(0, 7) = _xyloc(0, 4) * _xyloc(0, 3);
00517 _xyloc(0, 8) = _xyloc(0, 4) * _xyloc(0, 4);
00518 _xyloc(0, 9) = _xyloc(0, 8) * _xyloc(0, 1);
00519 _xyloc(0, 10) = _xyloc(0, 8) * _xyloc(0, 2);
00520 _xyloc(0, 11) = _xyloc(0, 8) * _xyloc(0, 3);
00521 _xyloc(0, 12) = _xyloc(0, 8) * _xyloc(0, 4);
00522 _xyloc(0, 13) = _xyloc(0, 12) * _xyloc(0, 1);
00523 _xyloc(0, 14) = _xyloc(0, 12) * _xyloc(0, 2);
00524 _xyloc(0, 15) = _xyloc(0, 12) * _xyloc(0, 3);
00525
00526 _result_pchip = prod(_xyloc, _bicubic_coeff);
00527 if (derivative) {
00528 derivative[0] = 0;
00529 derivative[1] = 0;
00530 for (int i = 1; i < 4; ++i) {
00531 for (int j = 0; j < 4; ++j) {
00532 derivative[0] += i * _bicubic_coeff(i * 4 + j, 0)
00533 * _xyloc(0, 4 * (i - 1)) * _xyloc(0, j);
00534 }
00535 }
00536 derivative[0] /= _axis[0]->increment(k0) ;
00537 for (int i = 0; i < 4; ++i) {
00538 for (int j = 1; j < 4; ++j) {
00539 derivative[1] += j * _bicubic_coeff(i * 4 + j, 0)
00540 * _xyloc(0, 4 * i) * _xyloc(0, j - 1);
00541 }
00542 }
00543 derivative[1] /= _axis[1]->increment(k1) ;
00544 }
00545 return _result_pchip(0, 0);
00546 }
00547
00548
00549
00550
00556 c_matrix<double, 16, 16> _inv_bicubic_coeff;
00557
00565 c_matrix<double, 16, 1> _bicubic_coeff;
00566 c_matrix<double, 16, 1> _field;
00567 c_matrix<double, 1, 16> _xyloc;
00568 c_matrix<double, 1, 1> _result_pchip;
00569 c_matrix<double, 4, 4> _value;
00570 matrix<double> _derv_x;
00571 matrix<double> _derv_y;
00572 matrix<double> _derv_x_y;
00573 size_t _fast_index[2];
00574 const size_t _kmin;
00575 const size_t _k0max;
00576 const size_t _k1max;
00577
00578 };
00579
00580 }
00581 }