00001
00005 #pragma once
00006
00007 #include <string.h>
00008 #include <typeinfo>
00009 #include <sstream>
00010 #include <netcdfcpp.h>
00011 #include <usml/types/wposition.h>
00012 #include <usml/types/wvector.h>
00013 #include <usml/types/seq_vector.h>
00014
00015 using namespace usml::ublas;
00016
00017 namespace usml {
00018 namespace types {
00019
00022
00024 enum GRID_INTERP_TYPE
00025 {
00026 GRID_INTERP_NEAREST = -1, GRID_INTERP_LINEAR = 0,
00027 GRID_INTERP_PCHIP = 1
00028 };
00029
00041 template<size_t Dim> inline size_t data_grid_compute_offset(
00042 seq_vector *axis[], const size_t* index)
00043 {
00044 return index[Dim] + axis[Dim]->size() * data_grid_compute_offset<Dim - 1> (
00045 axis, index);
00046 }
00047
00052 template<> inline size_t data_grid_compute_offset<0> (seq_vector *axis[],
00053 const size_t* index)
00054 {
00055 return index[0];
00056 }
00057
00067 template<class DATA_TYPE, size_t NUM_DIMS>
00068 class USML_DLLEXPORT data_grid {
00069
00070 typedef data_grid<DATA_TYPE,NUM_DIMS> self_type ;
00071 typedef size_t size_type ;
00072
00073 public:
00074
00084 data_grid(seq_vector *axis[])
00085 {
00086 size_type N = 1 ;
00087 for (unsigned n = 0; n < NUM_DIMS; ++n) {
00088 _axis[n] = axis[n]->clone();
00089 N *= _axis[n]->size();
00090 interp_type( n, GRID_INTERP_LINEAR ) ;
00091 }
00092 _data = new DATA_TYPE[N];
00093 memset(_data, 0, N * sizeof(DATA_TYPE));
00094 memset(_edge_limit, true, NUM_DIMS * sizeof(bool));
00095 }
00096
00106 data_grid(const data_grid& other, bool copy_data)
00107 {
00108 size_type N = 1 ;
00109 for (unsigned n = 0; n < NUM_DIMS; ++n) {
00110 _axis[n] = other._axis[n]->clone() ;
00111 N *= _axis[n]->size();
00112 }
00113 _data = new DATA_TYPE[N] ;
00114 if (copy_data) {
00115 memcpy(_data, other._data, N * sizeof(DATA_TYPE)) ;
00116 } else {
00117 memset(_data, 0, N * sizeof(DATA_TYPE)) ;
00118 }
00119 memcpy(_interp_type, other._interp_type, NUM_DIMS
00120 * sizeof(enum GRID_INTERP_TYPE)) ;
00121 memset(_edge_limit, true, NUM_DIMS * sizeof(bool)) ;
00122 }
00123
00128 void copy(const self_type& rhs) {
00129 size_type N = 1 ;
00130 for(size_type n = 0; n < NUM_DIMS; ++n) {
00131 if(_axis[n] != NULL) {
00132 delete _axis[n] ;
00133 }
00134 _axis[n] = rhs._axis[n]->clone() ;
00135 N *= _axis[n]->size();
00136 }
00137 if(_data != NULL) {
00138 delete[] _data ;
00139 }
00140 _data = new DATA_TYPE[N] ;
00141 memcpy( _data, rhs._data, N * sizeof(DATA_TYPE) ) ;
00142 }
00143
00147 virtual ~data_grid()
00148 {
00149 for (size_t n = 0; n < NUM_DIMS; ++n) {
00150 if (_axis[n] != NULL) {
00151 delete _axis[n];
00152 }
00153 }
00154 if (_data != NULL) {
00155 delete[] _data;
00156 }
00157 }
00158
00162 inline const seq_vector* axis(size_t dim) const
00163 {
00164 return _axis[dim];
00165 }
00166
00170 inline seq_vector* axis(size_t dim)
00171 {
00172 return _axis[dim];
00173 }
00174
00180 inline DATA_TYPE data(const size_t* index) const
00181 {
00182 const size_t offset = data_grid_compute_offset<NUM_DIMS - 1> (
00183 (seq_vector**) _axis, index);
00184 return _data[offset];
00185 }
00186
00193 inline void data(const size_t* index, DATA_TYPE value)
00194 {
00195 const size_t offset = data_grid_compute_offset<NUM_DIMS - 1> (_axis,
00196 index);
00197 _data[offset] = value;
00198 }
00199
00204 void write_netcdf( const char* filename ) {
00205
00206 NcFile* _file = new NcFile( filename, NcFile::Replace ) ;
00207
00208 vector<const NcDim*> axis_dim(NUM_DIMS) ;
00209 vector<NcVar*> axis_var(NUM_DIMS) ;
00210 const NcDim** axis_size = new const NcDim*[NUM_DIMS] ;
00211 long* data_size = new long[NUM_DIMS] ;
00212
00213
00214 NcType type = ncDouble ;
00215 const std::type_info& dt = typeid(DATA_TYPE) ;
00216 const char* dtype = dt.name() ;
00217 if( dtype == typeid(int).name() ) {
00218 type = ncInt ;
00219 }else if( dtype == typeid(float).name() ) {
00220 type = ncFloat ;
00221 }else if( dtype == typeid(double).name() ) {
00222 type = ncDouble ;
00223 }
00224
00225 NcVar* earth_radius = _file->add_var( "earth_radius", ncDouble, 0 ) ;
00226 for(long i=0; i < NUM_DIMS; ++i) {
00227 std::stringstream ss ;
00228 ss << "axis" << i << "\0" ;
00229 axis_dim[i] = _file->add_dim( (ss.str()).c_str(), (long) _axis[i]->size() ) ;
00230 axis_size[i] = axis_dim[i] ;
00231 axis_var[i] = _file->add_var( (ss.str()).c_str(), type, axis_dim[i] ) ;
00232 data_size[i] = (long) _axis[i]->size() ;
00233 }
00234 NcVar* data_var = _file->add_var( "data", type, NUM_DIMS, &*axis_size ) ;
00235
00236 earth_radius->put( &wposition::earth_radius, 1 ) ;
00237 for(long i=0; i < NUM_DIMS; ++i) {
00238 axis_var[i]->put( boost::numeric::ublas::vector<double>(*_axis[i]).data().begin(),
00239 data_size[i] ) ;
00240 }
00241 data_var->put( _data, data_size ) ;
00242
00243
00244 delete _file ;
00245 delete[] axis_size ;
00246 delete[] data_size ;
00247 _file = NULL ;
00248 data_size = NULL ;
00249 }
00250
00254 inline enum GRID_INTERP_TYPE interp_type(size_t dimension) const
00255 {
00256 return _interp_type[dimension];
00257 }
00258
00268 inline void interp_type(size_t dimension, enum GRID_INTERP_TYPE type)
00269 {
00270 const size_t size = _axis[dimension]->size() ;
00271 if ( type > GRID_INTERP_NEAREST && size < 2 ) {
00272 type = GRID_INTERP_NEAREST ;
00273 } else if ( type > GRID_INTERP_LINEAR && size < 4 ) {
00274 type = GRID_INTERP_LINEAR ;
00275 } else {
00276 _interp_type[dimension] = type;
00277 }
00278 }
00279
00283 inline bool edge_limit(size_t dimension) const
00284 {
00285 return _edge_limit[dimension];
00286 }
00287
00295 inline void edge_limit(size_t dimension, bool flag)
00296 {
00297 _edge_limit[dimension] = flag;
00298 }
00299
00318 DATA_TYPE interpolate(double* location, DATA_TYPE* derivative = NULL)
00319 {
00320
00321
00322 for (size_t dim = 0; dim < NUM_DIMS; ++dim) {
00323
00324
00325
00326 if ( _edge_limit[dim] ) {
00327 double a = *(_axis[dim]->begin()) ;
00328 double b = *(_axis[dim]->rbegin()) ;
00329 double inc = _axis[dim]->increment(0);
00330 if ( inc < 0) {
00331 if ( location[dim] >= a ) {
00332 location[dim] = a ;
00333 _offset[dim] = 0 ;
00334 } else if ( location[dim] <= b ) {
00335 location[dim] = b ;
00336 _offset[dim] = _axis[dim]->size()-2 ;
00337 } else {
00338 _offset[dim] = _axis[dim]->find_index(location[dim]);
00339 }
00340 }
00341 if (inc > 0 ) {
00342 if ( location[dim] <= a ) {
00343 location[dim] = a ;
00344 _offset[dim] = 0 ;
00345 } else if ( location[dim] >= b ) {
00346 location[dim] = b ;
00347 _offset[dim] = _axis[dim]->size()-2 ;
00348 } else {
00349 _offset[dim] = _axis[dim]->find_index(location[dim]);
00350 }
00351 }
00352
00353
00354
00355 } else {
00356 _offset[dim] = _axis[dim]->find_index(location[dim]);
00357 }
00358 }
00359
00360
00361
00362 DATA_TYPE dresult;
00363 return interp(NUM_DIMS - 1, _offset, location, dresult, derivative);
00364 }
00365
00375 void interpolate(const matrix<double>& x, matrix<double>* result, matrix<
00376 double>* dx = NULL)
00377 {
00378 double location[1];
00379 DATA_TYPE derivative[1];
00380 for (size_t n = 0; n < x.size1(); ++n) {
00381 for (size_t m = 0; m < x.size2(); ++m) {
00382 location[0] = x(n, m);
00383 if (dx == NULL) {
00384 (*result)(n, m) = (double) interpolate(location);
00385 } else {
00386 (*result)(n, m)
00387 = (double) interpolate(location, derivative);
00388 (*dx)(n, m) = (double) derivative[0];
00389 }
00390 }
00391 }
00392 }
00393
00405 void interpolate(const matrix<double>& x, const matrix<double>& y, matrix<
00406 double>* result, matrix<double>* dx = NULL, matrix<double>* dy =
00407 NULL)
00408 {
00409 double location[2];
00410 DATA_TYPE derivative[2];
00411 for (size_t n = 0; n < x.size1(); ++n) {
00412 for (size_t m = 0; m < x.size2(); ++m) {
00413 location[0] = x(n, m);
00414 location[1] = y(n, m);
00415 if (dx == NULL || dy == NULL) {
00416 (*result)(n, m) = (double) interpolate(location);
00417 } else {
00418 (*result)(n, m)
00419 = (double) interpolate(location, derivative);
00420 (*dx)(n, m) = (double) derivative[0];
00421 (*dy)(n, m) = (double) derivative[1];
00422 }
00423 }
00424 }
00425 }
00426
00440 void interpolate(const matrix<double>& x, const matrix<double>& y,
00441 const matrix<double>& z, matrix<double>* result,
00442 matrix<double>* dx = NULL, matrix<double>* dy = NULL,
00443 matrix<double>* dz = NULL)
00444 {
00445 double location[3];
00446 DATA_TYPE derivative[3];
00447 for (size_t n = 0; n < x.size1(); ++n) {
00448 for (size_t m = 0; m < x.size2(); ++m) {
00449 location[0] = x(n, m);
00450 location[1] = y(n, m);
00451 location[2] = z(n, m);
00452 if (dx == NULL || dy == NULL || dz == NULL) {
00453 (*result)(n, m) = (double) interpolate(location);
00454 } else {
00455 (*result)(n, m)
00456 = (double) interpolate(location, derivative);
00457 (*dx)(n, m) = (double) derivative[0];
00458 (*dy)(n, m) = (double) derivative[1];
00459 (*dz)(n, m) = (double) derivative[2];
00460 }
00461 }
00462 }
00463 }
00464
00465 protected:
00466
00468 seq_vector* _axis[NUM_DIMS] ;
00469
00471 enum GRID_INTERP_TYPE _interp_type[NUM_DIMS];
00472
00474 bool _edge_limit[NUM_DIMS];
00475
00477 size_t _offset[NUM_DIMS] ;
00478
00485 DATA_TYPE* _data ;
00486
00490 data_grid() {
00491
00492 for (unsigned n = 0; n < NUM_DIMS; ++n) {
00493 _axis[n] = NULL ;
00494 }
00495 _data = NULL ;
00496
00497 memset(_interp_type, GRID_INTERP_LINEAR, NUM_DIMS * sizeof(enum GRID_INTERP_TYPE)) ;
00498 memset(_edge_limit, true, NUM_DIMS * sizeof(bool)) ;
00499 }
00500
00501 private:
00502
00503
00504
00505
00524 DATA_TYPE interp(int dim, const size_t* index, const double* location,
00525 DATA_TYPE& deriv, DATA_TYPE* deriv_vec) const;
00526
00527
00542 DATA_TYPE nearest(int dim, const size_t* index, const double* location,
00543 DATA_TYPE& deriv, DATA_TYPE* deriv_vec) const
00544 {
00545 DATA_TYPE result, da;
00546
00547
00548
00549 const size_t k = index[dim];
00550 seq_vector* ax = _axis[dim];
00551 const double u = (location[dim] - (*ax)(k)) / ax->increment(k);
00552 if (u < 0.5) {
00553 result = interp(dim - 1, index, location, da, deriv_vec);
00554 } else {
00555 size_t next[NUM_DIMS];
00556 memcpy(next, index, NUM_DIMS * sizeof(size_t));
00557 ++next[dim];
00558 result = interp(dim - 1, next, location, da, deriv_vec);
00559 }
00560
00561
00562
00563 if (deriv_vec) {
00564 deriv = 0.0;
00565 deriv_vec[dim] = deriv;
00566 if (dim > 0) deriv_vec[dim - 1] = da;
00567 }
00568
00569
00570
00571 return result;
00572 }
00573
00588 DATA_TYPE linear(int dim, const size_t* index, const double* location,
00589 DATA_TYPE& deriv, DATA_TYPE* deriv_vec) const
00590 {
00591 DATA_TYPE result, da, db;
00592
00593
00594
00595 const DATA_TYPE a = interp(dim - 1, index, location, da, deriv_vec);
00596 size_t next[NUM_DIMS];
00597 memcpy(next, index, NUM_DIMS * sizeof(size_t));
00598 ++next[dim];
00599 const DATA_TYPE b = interp(dim - 1, next, location, db, deriv_vec);
00600 const size_t k = index[dim];
00601 seq_vector* ax = _axis[dim];
00602
00603
00604
00605 const DATA_TYPE h = (DATA_TYPE) ax->increment(k) ;
00606 const DATA_TYPE u = (location[dim] - (*ax)(k)) / h ;
00607 result = a * (1.0 - u) + b * u;
00608
00609
00610
00611 if (deriv_vec) {
00612 deriv = (b - a) / h ;
00613 deriv_vec[dim] = deriv;
00614 if (dim > 0) {
00615 deriv_vec[dim - 1] = da * (1.0 - u) + db * u;
00616 }
00617 }
00618
00619
00620
00621 return result;
00622 }
00623
00687 DATA_TYPE pchip(int dim, const size_t* index, const double* location,
00688 DATA_TYPE& deriv, DATA_TYPE* deriv_vec) const
00689 {
00690 DATA_TYPE result ;
00691 DATA_TYPE y0, y1, y2, y3 ;
00692 DATA_TYPE dy0=0, dy1=0, dy2=0, dy3=0 ;
00693 const size_t kmin = 1u ;
00694 const size_t kmax = _axis[dim]->size()-3u ;
00695
00696
00697
00698 const size_t k = index[dim];
00699 seq_vector* ax = _axis[dim];
00700 y1 = interp( dim-1, index, location, dy1, deriv_vec );
00701
00702 if ( k >= kmin ) {
00703 size_t prev[NUM_DIMS];
00704 memcpy(prev, index, NUM_DIMS * sizeof(size_t));
00705 --prev[dim];
00706 y0 = interp( dim-1, prev, location, dy0, deriv_vec );
00707 } else {
00708 y0 = y1 ;
00709 dy0 = dy1 ;
00710 }
00711
00712
00713
00714 size_t next[NUM_DIMS];
00715 memcpy(next, index, NUM_DIMS * sizeof(size_t));
00716 ++next[dim];
00717 y2 = interp( dim-1, next, location, dy2, deriv_vec );
00718
00719 if ( k <= kmax ) {
00720 size_t last[NUM_DIMS];
00721 memcpy(last, next, NUM_DIMS * sizeof(size_t));
00722 ++last[dim];
00723 y3 = interp(dim - 1, last, location, dy3, deriv_vec);
00724 } else {
00725 y3 = y2 ;
00726 dy3 = dy2 ;
00727 }
00728
00729
00730
00731 const DATA_TYPE h0 = (DATA_TYPE) ax->increment(k - 1);
00732 const DATA_TYPE h1 = (DATA_TYPE) ax->increment(k);
00733 const DATA_TYPE h2 = (DATA_TYPE) ax->increment(k + 1);
00734 const DATA_TYPE h1_2 = h1 * h1;
00735 const DATA_TYPE h1_3 = h1_2 * h1;
00736
00737 const DATA_TYPE s = location[dim]-(*ax)(k);
00738 const DATA_TYPE s_2 = s * s, s_3 = s_2 * s;
00739 const DATA_TYPE sh_minus = s - h1;
00740 const DATA_TYPE sh_term = 3.0 * h1 * s_2 - 2.0 * s_3;
00741
00742
00743
00744
00745 const DATA_TYPE deriv0 = (y1 - y0) / h0;
00746 const DATA_TYPE deriv1 = (y2 - y1) / h1;
00747 const DATA_TYPE deriv2 = (y3 - y2) / h2;
00748
00749 DATA_TYPE dderiv0=0.0, dderiv1=0.0, dderiv2=0.0 ;
00750 if (deriv_vec) {
00751 dderiv0 = (dy1 - dy0) / h0;
00752 dderiv1 = (dy2 - dy1) / h1;
00753 dderiv2 = (dy3 - dy2) / h2;
00754 }
00755
00756
00757
00758
00759
00760
00761
00762 DATA_TYPE slope1=0.0, dslope1=0.0;
00763
00764
00765
00766
00767 if ( k >= kmin ) {
00768 const DATA_TYPE w0 = 2.0 * h1 + h0;
00769 const DATA_TYPE w1 = h1 + 2.0 * h0;
00770 if ( deriv0 * deriv1 > 0.0 ) {
00771 slope1 = (w0 + w1) / ( w0 / deriv0 + w1 / deriv1 );
00772 }
00773 if ( deriv_vec != NULL && dderiv0 * dderiv1 > 0.0 ) {
00774 dslope1 = (w0 + w1) / ( w0 / dderiv0 + w1 / dderiv1 );
00775 }
00776
00777
00778
00779
00780 } else {
00781 slope1 = ( (2.0+h1+h2) * deriv1 - h1 * deriv2 ) / (h1+h2) ;
00782 if ( slope1 * deriv1 < 0.0 ) {
00783 slope1 = 0.0 ;
00784 } else if ( (deriv1*deriv2 < 0.0) && (abs(slope1) > abs(3.0*deriv1)) ) {
00785 slope1 = 3.0*deriv1 ;
00786 }
00787 if ( deriv_vec ) {
00788 dslope1 = ( (2.0+h1+h2) * dderiv1 - h1 * dderiv2 ) / (h1+h2) ;
00789 if ( dslope1 * dderiv1 < 0.0 ) {
00790 dslope1 = 0.0 ;
00791 } else if ( (dderiv1*dderiv2 < 0.0) && (abs(dslope1) > abs(3.0*dderiv1)) ) {
00792 dslope1 = 3.0*dderiv1 ;
00793 }
00794 }
00795 }
00796
00797
00798
00799
00800
00801
00802
00803 DATA_TYPE slope2=0.0, dslope2=0.0;
00804
00805
00806
00807
00808 if ( k <= kmax ) {
00809 const DATA_TYPE w1 = 2.0 * h1 + h0;
00810 const DATA_TYPE w2 = h1 + 2.0 * h0;
00811 if ( deriv1 * deriv2 > 0.0 ) {
00812 slope2 = (w1 + w2) / ( w1 / deriv1 + w2 / deriv2 );
00813 }
00814 if ( deriv_vec != NULL && dderiv1 * dderiv2 > 0.0 ) {
00815 dslope2 = (w1 + w2) / ( w1 / dderiv1 + w2 / dderiv2 );
00816 }
00817
00818
00819
00820
00821 } else {
00822 slope2 = ( (2.0+h1+h2) * deriv1 - h1 * deriv0 ) / (h1+h0) ;
00823 if ( slope2 * deriv1 < 0.0 ) {
00824 slope2 = 0.0 ;
00825 } else if ( (deriv1*deriv0 < 0.0) && (abs(slope2) > abs(3.0*deriv1)) ) {
00826 slope2 = 3.0*deriv1 ;
00827 }
00828 if ( deriv_vec ) {
00829 dslope2 = ( (2.0+h1+h2) * dderiv1 - h1 * dderiv0 ) / (h1+h0) ;
00830 if ( dslope2 * dderiv1 < 0.0 ) {
00831 dslope2 = 0.0 ;
00832 } else if ( (dderiv1*dderiv0 < 0.0) && (abs(dslope2) > abs(3.0*dderiv1)) ) {
00833 dslope2 = 3.0*dderiv1 ;
00834 }
00835 }
00836 }
00837
00838
00839
00840 result = y2 * sh_term / h1_3
00841 + y1 * (h1_3 - sh_term) / h1_3
00842 + slope2 * s_2 * sh_minus / h1_2
00843 + slope1 * s * sh_minus * sh_minus / h1_2;
00844
00845
00846
00847
00848 if (deriv_vec) {
00849 const DATA_TYPE u = s / h1;
00850 deriv = slope1 * (1.0 - u) + slope2 * u;
00851 deriv_vec[dim] = deriv ;
00852 if (dim > 0) {
00853 deriv_vec[dim-1] = dy2 * sh_term / h1_3
00854 + dy1 * (h1_3 - sh_term) / h1_3
00855 + dslope2 * s_2 * sh_minus / h1_2
00856 + dslope1 * s * sh_minus * sh_minus / h1_2;
00857 }
00858 }
00859
00860
00861
00862 return result;
00863 }
00864
00865 };
00866
00867
00868
00869
00873 template<class DATA_TYPE, size_t NUM_DIMS>
00874 DATA_TYPE data_grid<DATA_TYPE, NUM_DIMS>::interp(int dim,
00875 const size_t* index, const double* location, DATA_TYPE& deriv,
00876 DATA_TYPE* deriv_vec) const
00877 {
00878 DATA_TYPE result;
00879
00880 if (dim < 0) {
00881 result = data(index);
00882
00883
00884 } else {
00885 if (_interp_type[dim] == GRID_INTERP_LINEAR) {
00886 result = linear(dim, index, location, deriv, deriv_vec);
00887 } else if (_interp_type[dim] == GRID_INTERP_PCHIP) {
00888 result = pchip(dim, index, location, deriv, deriv_vec);
00889 } else {
00890 result = nearest(dim, index, location, deriv, deriv_vec);
00891 }
00892 }
00893 return result;
00894 }
00895
00896 }
00897 }