00001
00006 #pragma once
00007
00008 #include <usml/types/data_grid.h>
00009
00010 namespace usml {
00011 namespace types {
00014
00033 class USML_DECLSPEC data_grid_svp: public data_grid<double, 3> {
00034
00035 public:
00036
00043 data_grid_svp( const data_grid<double, 3>* grid )
00044 : data_grid<double, 3>(*grid, true),
00045 _kzmax(_axis[0]->size() - 1u),
00046 _kxmax(_axis[1]->size() - 1u),
00047 _kymax(_axis[2]->size() - 1u)
00048 {
00049 double result = 0.0;
00050
00051
00052 double inc1, inc2;
00053 double w1, w2;
00054 double slope_1, slope_2;
00055
00056
00057 if (interp_type(0) != GRID_INTERP_PCHIP) {
00058 interp_type(0, GRID_INTERP_PCHIP);
00059 }
00060 if ((interp_type(1) != GRID_INTERP_LINEAR)
00061 || (interp_type(1) != GRID_INTERP_LINEAR)) {
00062 interp_type(1, GRID_INTERP_LINEAR);
00063 interp_type(2, GRID_INTERP_LINEAR);
00064 }
00065 derv_z = new double**[_kzmax + 1u];
00066 for (int i = 0; i < _kzmax + 1u; ++i) {
00067 derv_z[i] = new double*[_kxmax + 1u];
00068 for (int j = 0; j < _kxmax + 1u; ++j) {
00069 derv_z[i][j] = new double[_kymax + 1u];
00070 }
00071 }
00072 for (int i = 0; i < _kzmax + 1u; ++i) {
00073 for (int j = 0; j < _kxmax + 1u; ++j) {
00074 for (int k = 0; k < _kymax + 1u; ++k) {
00075 if (i == 0) {
00076 inc1 = _axis[0]->increment(i);
00077 inc2 = _axis[0]->increment(i + 1);
00078 slope_1 = (data_3d(i + 1, j, k) - data_3d(i, j, k))
00079 / inc1;
00080 slope_2 = (data_3d(i + 2, j, k) - data_3d(i + 1, j, k))
00081 / inc2;
00082 result = ( (2.0 * inc1 + inc2) * slope_1 - inc1 * slope_2 )
00083 / (inc1 + inc2) ;
00084 if ( result * slope_1 <= 0.0 ) {
00085 result = 0.0 ;
00086 } else if ( (slope_1 * slope_2 <= 0.0)
00087 && (abs(result) > abs(3.0 * slope_1)) ) {
00088 result = 3.0 * slope_1 ;
00089 }
00090 } else if (i == _kzmax) {
00091 inc1 = _axis[0]->increment(i - 1);
00092 inc2 = _axis[0]->increment(i);
00093 slope_1 = (data_3d(i - 1, j, k) - data_3d(i - 2, j, k))
00094 / inc1;
00095 slope_2 = (data_3d(i, j, k) - data_3d(i - 1, j, k))
00096 / inc2;
00097 result = ( (2.0 * inc1 + inc2) * slope_2 - inc1 * slope_1 )
00098 / (inc1 + inc2) ;
00099 if (result * slope_1 <= 0.0) {
00100 result = 0.0;
00101 } else if ((slope_1 * slope_2 <= 0.0)
00102 && (abs(result) > abs(3.0 * slope_1))) {
00103 result = 3.0 * slope_1;
00104 }
00105 } else {
00106 inc1 = _axis[0]->increment(i - 1);
00107 inc2 = _axis[0]->increment(i);
00108 w1 = 2.0 * inc2 + inc1;
00109 w2 = inc2 + 2.0 * inc1;
00110 slope_1 = (data_3d(i, j, k) - data_3d(i - 1, j, k))
00111 / inc1;
00112 slope_2 = (data_3d(i + 1, j, k) - data_3d(i, j, k))
00113 / inc2;
00114 if (slope_1 * slope_2 <= 0.0) {
00115 result = 0.0;
00116 } else {
00117 result = (w1 + w2)
00118 / ((w1 / slope_1) + (w2 / slope_2));
00119 }
00120 }
00121 derv_z[i][j][k] = result ;
00122 }
00123 }
00124 }
00125 delete grid ;
00126 }
00127
00131 virtual ~data_grid_svp() {
00132 for(size_t i=0; i<=_kzmax; ++i) {
00133 for(size_t j=0; j<=_kxmax; ++j) {
00134 delete[] derv_z[i][j] ;
00135 }
00136 delete[] derv_z[i] ;
00137 }
00138 delete[] derv_z ;
00139 }
00140
00153 double interpolate(double* location, double* derivative = NULL)
00154 {
00155 double result = 0.0;
00156 size_t k0, k1, k2;
00157
00158
00159 double f11, f21, f12, f22, x_diff, y_diff;
00160 double x, x1, x2, y, y1, y2;
00161
00162
00163 double v1, v2;
00164 double inc1;
00165 double t, t_2, t_3;
00166 double h00, h10, h01, h11;
00167
00168
00169
00170 for (size_t dim = 0; dim < 3; ++dim) {
00171
00172
00173
00174 if (edge_limit(dim)) {
00175 double a = *(_axis[dim]->begin());
00176 double b = *(_axis[dim]->rbegin());
00177 double inc = _axis[dim]->increment(0);
00178 if (inc < 0) {
00179 if (location[dim] >= a) {
00180 location[dim] = a;
00181 _offset[dim] = 0;
00182 } else if (location[dim] <= b) {
00183 location[dim] = b;
00184 _offset[dim] = _axis[dim]->size() - 2;
00185 } else {
00186 _offset[dim] = _axis[dim]->find_index(location[dim]);
00187 }
00188 }
00189 if (inc > 0) {
00190 if (location[dim] <= a) {
00191 location[dim] = a;
00192 _offset[dim] = 0;
00193 } else if (location[dim] >= b) {
00194 location[dim] = b;
00195 _offset[dim] = _axis[dim]->size() - 2;
00196 } else {
00197 _offset[dim] = _axis[dim]->find_index(location[dim]);
00198 }
00199 }
00200
00201
00202
00203 } else {
00204 _offset[dim] = _axis[dim]->find_index(location[dim]);
00205 }
00206 }
00207
00208
00209 if (derivative) {
00210 derivative[0] = 0;
00211 }
00212 k0 = _offset[0];
00213 k1 = _offset[1];
00214 k2 = _offset[2];
00215
00216
00217
00218 for (int i = 0; i < 2; ++i) {
00219 for (int j = 0; j < 2; ++j) {
00220
00221 v1 = data_3d(k0, k1 + i, k2 + j);
00222 v2 = data_3d(k0 + 1, k1 + i, k2 + j);
00223 inc1 = _axis[0]->increment(k0);
00224
00225 t = (location[0] - (*_axis[0])(k0)) / inc1;
00226 t_2 = t * t;
00227 t_3 = t_2 * t;
00228
00229
00230 h00 = (2 * t_3 - 3 * t_2 + 1);
00231 h10 = (t_3 - 2 * t_2 + t);
00232 h01 = (3 * t_2 - 2 * t_3);
00233 h11 = (t_3 - t_2);
00234
00235 _interp_plane(i, j) = h00 * v1 + h10 * derv_z[k0][k1 + i][k2 + j]
00236 + h01 * v2 + h11 * derv_z[k0 + 1][k1 + i][k2 + j];
00237
00238 if (derivative) {
00239 _dz(i, j) = (6 * t_2 - 6 * t) * v1 / inc1
00240 + (3 * t_2 - 4 * t + 1) * derv_z[k0][k1 + i][k2 + j] / inc1
00241 + (6 * t - 6 * t_2) * v2 / inc1
00242 + (3 * t_2 - 2 * t) * derv_z[k0 + 1][k1 + i][k2 + j] / inc1 ;
00243 }
00244 }
00245 }
00246
00247
00248
00249 x = location[1];
00250 x1 = (*_axis[1])(k1);
00251 x2 = (*_axis[1])(k1 + 1);
00252 y = location[2];
00253 y1 = (*_axis[2])(k2);
00254 y2 = (*_axis[2])(k2 + 1);
00255 f11 = _interp_plane(0, 0);
00256 f21 = _interp_plane(1, 0);
00257 f12 = _interp_plane(0, 1);
00258 f22 = _interp_plane(1, 1);
00259 x_diff = x2 - x1;
00260 y_diff = y2 - y1;
00261
00262 result = (f11 * (x2 - x) * (y2 - y) + f21 * (x - x1) * (y2 - y)
00263 + f12 * (x2 - x) * (y - y1) + f22 * (x - x1) * (y - y1))
00264 / (x_diff * y_diff);
00265
00266 if (derivative) {
00267 derivative[0] = (_dz(0, 0) * (x2 - x) * (y2 - y)
00268 + _dz(1, 0) * (x - x1) * (y2 - y)
00269 + _dz(0, 1) * (x2 - x) * (y - y1)
00270 + _dz(1, 1) * (x - x1) * (y - y1)) / (x_diff * y_diff);
00271 derivative[1] = (-f11 * (y2 - y) + f21 * (y2 - y) - f12 * (y - y1)
00272 + f22 * (y - y1)) / (x_diff * y_diff);
00273 derivative[2] = (-f11 * (x2 - x) - f21 * (x - x1) + f12 * (x2 - x)
00274 + f22 * (x - x1)) / (x_diff * y_diff);
00275 }
00276
00277 return result;
00278
00279 }
00280
00294 void interpolate(const matrix<double>& x, const matrix<double>& y,
00295 const matrix<double>& z, matrix<double>* result,
00296 matrix<double>* dx = NULL, matrix<double>* dy = NULL,
00297 matrix<double>* dz = NULL)
00298 {
00299 double location[3];
00300 double derivative[3];
00301 for (size_t n = 0; n < x.size1(); ++n) {
00302 for (size_t m = 0; m < x.size2(); ++m) {
00303 location[0] = x(n, m);
00304 location[1] = y(n, m);
00305 location[2] = z(n, m);
00306 if (dx == NULL || dy == NULL || dz == NULL) {
00307 (*result)(n, m) = (double) interpolate(location);
00308 } else {
00309 (*result)(n, m) = (double) interpolate(location,
00310 derivative);
00311 (*dx)(n, m) = (double) derivative[0];
00312 (*dy)(n, m) = (double) derivative[1];
00313 (*dz)(n, m) = (double) derivative[2];
00314 }
00315 }
00316 }
00317
00318 }
00319
00320 private:
00321
00323 inline double data_3d(size_t dim0, size_t dim1, size_t dim2)
00324 {
00325 size_t grid_index[3];
00326 grid_index[0] = dim0;
00327 grid_index[1] = dim1;
00328 grid_index[2] = dim2;
00329 return data(grid_index);
00330
00331 }
00332
00337 size_t _kzmax, _kxmax, _kymax;
00338
00339
00340 c_matrix<double, 2, 2> _interp_plane;
00341
00342
00343 c_matrix<double, 2, 2> _dz;
00344 double*** derv_z;
00345
00346 };
00347
00348 }
00349 }