5 #ifndef USML_TYPES_DATA_GRID_H
6 #define USML_TYPES_DATA_GRID_H
9 #include <usml/types/wvector.h>
10 #include <usml/types/seq_vector.h>
12 using namespace usml::ublas;
50 const unsigned* index)
64 template<
class DATA_TYPE,
unsigned NUM_DIMS>
class data_grid
84 unsigned _offset[NUM_DIMS];
102 inline DATA_TYPE
data(
const unsigned* index)
const
106 return _data[offset];
115 inline void data(
const unsigned* index, DATA_TYPE value)
119 _data[offset] = value;
137 return _interp_type[dimension];
151 const unsigned size = _axis[dimension]->size() ;
157 _interp_type[dimension] = type;
167 bool _edge_limit[NUM_DIMS];
176 return _edge_limit[dimension];
188 _edge_limit[dimension] = flag;
214 DATA_TYPE interp(
int dim,
const unsigned* index,
const double* location,
215 DATA_TYPE& deriv, DATA_TYPE* deriv_vec)
const;
232 DATA_TYPE
nearest(
int dim,
const unsigned* index,
const double* location,
233 DATA_TYPE& deriv, DATA_TYPE* deriv_vec)
const
235 DATA_TYPE result, da;
239 const unsigned k = index[dim];
241 const double u = (location[dim] - (*ax)(k)) / ax->
increment(k);
243 result = interp(dim - 1, index, location, da, deriv_vec);
245 unsigned next[NUM_DIMS];
246 memcpy(next, index, NUM_DIMS *
sizeof(
unsigned));
248 result = interp(dim - 1, next, location, da, deriv_vec);
255 deriv_vec[dim] = deriv;
256 if (dim > 0) deriv_vec[dim - 1] = da;
278 DATA_TYPE
linear(
int dim,
const unsigned* index,
const double* location,
279 DATA_TYPE& deriv, DATA_TYPE* deriv_vec)
const
281 DATA_TYPE result, da, db;
285 unsigned next[NUM_DIMS];
286 memcpy(next, index, NUM_DIMS *
sizeof(
unsigned));
289 const DATA_TYPE a = interp(dim - 1, index, location, da, deriv_vec);
290 const DATA_TYPE b = interp(dim - 1, next, location, db, deriv_vec);
291 const unsigned k = index[dim];
296 const DATA_TYPE h = (DATA_TYPE) ax->
increment(k) ;
297 const DATA_TYPE u = (location[dim] - (*ax)(k)) / h ;
298 result = a * (1.0 - u) + b * u;
303 deriv = (b - a) / h ;
304 deriv_vec[dim] = deriv;
306 deriv_vec[dim - 1] = da * (1.0 - u) + db * u;
378 DATA_TYPE
pchip(
int dim,
const unsigned* index,
const double* location,
379 DATA_TYPE& deriv, DATA_TYPE* deriv_vec)
const
382 DATA_TYPE y0, y1, y2, y3 ;
383 DATA_TYPE dy0, dy1, dy2, dy3 ;
384 const unsigned kmin = 1u ;
385 const unsigned kmax = _axis[dim]->size()-3u ;
389 const unsigned k = index[dim];
391 y1 = interp( dim-1, index, location, dy1, deriv_vec );
394 unsigned prev[NUM_DIMS];
395 memcpy(prev, index, NUM_DIMS *
sizeof(
unsigned));
397 y0 = interp( dim-1, prev, location, dy0, deriv_vec );
405 unsigned next[NUM_DIMS];
406 memcpy(next, index, NUM_DIMS *
sizeof(
unsigned));
408 y2 = interp( dim-1, next, location, dy2, deriv_vec );
411 unsigned last[NUM_DIMS];
412 memcpy(last, next, NUM_DIMS *
sizeof(
unsigned));
414 y3 = interp(dim - 1, last, location, dy3, deriv_vec);
422 const DATA_TYPE h0 = (DATA_TYPE) ax->
increment(k - 1);
423 const DATA_TYPE h1 = (DATA_TYPE) ax->
increment(k);
424 const DATA_TYPE h2 = (DATA_TYPE) ax->
increment(k + 1);
425 const DATA_TYPE h1_2 = h1 * h1;
426 const DATA_TYPE h1_3 = h1_2 * h1;
428 const DATA_TYPE s = location[dim]-(*ax)(k);
429 const DATA_TYPE s_2 = s * s, s_3 = s_2 * s;
430 const DATA_TYPE sh_minus = s - h1;
431 const DATA_TYPE sh_term = 3.0 * h1 * s_2 - 2.0 * s_3;
436 const DATA_TYPE deriv0 = (y1 - y0) / h0;
437 const DATA_TYPE deriv1 = (y2 - y1) / h1;
438 const DATA_TYPE deriv2 = (y3 - y2) / h2;
440 DATA_TYPE dderiv0=0.0, dderiv1=0.0, dderiv2=0.0 ;
442 dderiv0 = (dy1 - dy0) / h0;
443 dderiv1 = (dy2 - dy1) / h1;
444 dderiv2 = (dy3 - dy2) / h2;
453 DATA_TYPE slope1=0.0, dslope1=0.0;
459 const DATA_TYPE w0 = 2.0 * h1 + h0;
460 const DATA_TYPE w1 = h1 + 2.0 * h0;
461 if ( deriv0 * deriv1 > 0.0 ) {
462 slope1 = (w0 + w1) / ( w0 / deriv0 + w1 / deriv1 );
464 if ( deriv_vec != NULL && dderiv0 * dderiv1 > 0.0 ) {
465 dslope1 = (w0 + w1) / ( w0 / dderiv0 + w1 / dderiv1 );
472 slope1 = ( (2.0+h1+h2) * deriv1 - h1 * deriv2 ) / (h1+h2) ;
473 if ( slope1 * deriv1 < 0.0 ) {
475 }
else if ( (deriv1*deriv2 < 0.0) && (
abs(slope1) >
abs(3.0*deriv1)) ) {
476 slope1 = 3.0*deriv1 ;
479 dslope1 = ( (2.0+h1+h2) * dderiv1 - h1 * dderiv2 ) / (h1+h2) ;
480 if ( dslope1 * dderiv1 < 0.0 ) {
482 }
else if ( (dderiv1*dderiv2 < 0.0) && (
abs(dslope1) >
abs(3.0*dderiv1)) ) {
483 dslope1 = 3.0*dderiv1 ;
494 DATA_TYPE slope2=0.0, dslope2=0.0;
500 const DATA_TYPE w1 = 2.0 * h1 + h0;
501 const DATA_TYPE w2 = h1 + 2.0 * h0;
502 if ( deriv1 * deriv2 > 0.0 ) {
503 slope2 = (w1 + w2) / ( w1 / deriv1 + w2 / deriv2 );
505 if ( deriv_vec != NULL && dderiv1 * dderiv2 > 0.0 ) {
506 dslope2 = (w1 + w2) / ( w1 / dderiv1 + w2 / dderiv2 );
513 slope2 = ( (2.0+h1+h2) * deriv1 - h1 * deriv0 ) / (h1+h0) ;
514 if ( slope2 * deriv1 < 0.0 ) {
516 }
else if ( (deriv1*deriv0 < 0.0) && (
abs(slope2) >
abs(3.0*deriv1)) ) {
517 slope2 = 3.0*deriv1 ;
520 dslope2 = ( (2.0+h1+h2) * dderiv1 - h1 * dderiv0 ) / (h1+h0) ;
521 if ( dslope2 * dderiv1 < 0.0 ) {
523 }
else if ( (dderiv1*dderiv0 < 0.0) && (
abs(dslope2) >
abs(3.0*dderiv1)) ) {
524 dslope2 = 3.0*dderiv1 ;
531 result = y2 * sh_term / h1_3
532 + y1 * (h1_3 - sh_term) / h1_3
533 + slope2 * s_2 * sh_minus / h1_2
534 + slope1 * s * sh_minus * sh_minus / h1_2;
540 const DATA_TYPE u = s / h1;
541 deriv = slope1 * (1.0 - u) + slope2 * u;
542 deriv_vec[dim] = deriv;
544 deriv_vec[dim-1] = dy2 * sh_term / h1_3
545 + dy1 * (h1_3 - sh_term) / h1_3
546 + dslope2 * s_2 * sh_minus / h1_2
547 + dslope1 * s * sh_minus * sh_minus / h1_2;
576 DATA_TYPE
interpolate(
double* location, DATA_TYPE* derivative = NULL)
580 for (
unsigned dim = 0; dim < NUM_DIMS; ++dim) {
584 if ( _edge_limit[dim] ) {
585 double a = *(_axis[dim]->begin()) ;
586 double b = *(_axis[dim]->rbegin()) ;
587 double inc = _axis[dim]->increment(0);
589 if ( location[dim] >= a ) {
592 }
else if ( location[dim] <= b ) {
594 _offset[dim] = _axis[dim]->size()-2 ;
596 _offset[dim] = _axis[dim]->find_index(location[dim]);
600 if ( location[dim] <= a ) {
603 }
else if ( location[dim] >= b ) {
605 _offset[dim] = _axis[dim]->size()-2 ;
607 _offset[dim] = _axis[dim]->find_index(location[dim]);
614 _offset[dim] = _axis[dim]->find_index(location[dim]);
621 return interp(NUM_DIMS - 1, _offset, location, dresult, derivative);
633 void interpolate(
const matrix<double>& x, matrix<double>* result, matrix<
637 DATA_TYPE derivative[1];
638 for (
unsigned n = 0; n < x.size1(); ++n) {
639 for (
unsigned m = 0; m < x.size2(); ++m) {
640 location[0] = x(n, m);
642 (*result)(n, m) = (
double) interpolate(location);
645 = (
double) interpolate(location, derivative);
646 (*dx)(n, m) = (
double) derivative[0];
663 void interpolate(
const matrix<double>& x,
const matrix<double>& y, matrix<
664 double>* result, matrix<double>* dx = NULL, matrix<double>* dy =
668 DATA_TYPE derivative[2];
669 for (
unsigned n = 0; n < x.size1(); ++n) {
670 for (
unsigned m = 0; m < x.size2(); ++m) {
671 location[0] = x(n, m);
672 location[1] = y(n, m);
673 if (dx == NULL || dy == NULL) {
674 (*result)(n, m) = (
double) interpolate(location);
677 = (
double) interpolate(location, derivative);
678 (*dx)(n, m) = (
double) derivative[0];
679 (*dy)(n, m) = (
double) derivative[1];
698 void interpolate(
const matrix<double>& x,
const matrix<double>& y,
699 const matrix<double>& z, matrix<double>* result,
700 matrix<double>* dx = NULL, matrix<double>* dy = NULL,
701 matrix<double>* dz = NULL)
704 DATA_TYPE derivative[3];
705 for (
unsigned n = 0; n < x.size1(); ++n) {
706 for (
unsigned m = 0; m < x.size2(); ++m) {
707 location[0] = x(n, m);
708 location[1] = y(n, m);
709 location[2] = z(n, m);
710 if (dx == NULL || dy == NULL || dz == NULL) {
711 (*result)(n, m) = (
double) interpolate(location);
714 = (
double) interpolate(location, derivative);
715 (*dx)(n, m) = (
double) derivative[0];
716 (*dy)(n, m) = (
double) derivative[1];
717 (*dz)(n, m) = (
double) derivative[2];
733 for (
unsigned n = 0; n < NUM_DIMS; ++n) {
739 memset(_edge_limit,
true, NUM_DIMS *
sizeof(
bool));
756 for (
unsigned n = 0; n < NUM_DIMS; ++n) {
757 _axis[n] = axis[n]->
clone();
758 N *= _axis[n]->
size();
761 _data =
new DATA_TYPE[N];
762 memset(_data, 0, N *
sizeof(DATA_TYPE));
763 memset(_edge_limit,
true, NUM_DIMS *
sizeof(
bool));
778 for (
unsigned n = 0; n < NUM_DIMS; ++n) {
780 N *= _axis[n]->
size();
782 _data =
new DATA_TYPE[N];
784 memcpy(_data, other.
_data, N *
sizeof(DATA_TYPE));
786 memset(_data, 0, N *
sizeof(DATA_TYPE));
790 memset(_edge_limit,
true, NUM_DIMS *
sizeof(
bool));
798 for (
unsigned n = 0; n < NUM_DIMS; ++n) {
799 if (_axis[n] != NULL) {
816 template<
class DATA_TYPE,
unsigned NUM_DIMS>
818 const unsigned* index,
const double* location, DATA_TYPE& deriv,
819 DATA_TYPE* deriv_vec)
const
824 result = data(index);
829 result = linear(dim, index, location, deriv, deriv_vec);
831 result = pchip(dim, index, location, deriv, deriv_vec);
833 result = nearest(dim, index, location, deriv, deriv_vec);
seq_vector * _axis[NUM_DIMS]
Axis associated with each dimension of the data grid.
Definition: data_grid.h:73
const seq_vector * axis(int unsigned dim) const
Extract a reference to one of the axes.
Definition: data_grid.h:92
value_type increment(size_type index) const
Retrieves the increment between two elements in this sequence.
Definition: seq_vector.h:119
data_grid(seq_vector *axis[])
Create data grid from its associated axes.
Definition: data_grid.h:753
DATA_TYPE pchip(int dim, const unsigned *index, const double *location, DATA_TYPE &deriv, DATA_TYPE *deriv_vec) const
Interpolate this dimension using the Piecewise Cubic Hermite Interpolation Polynomial (PCHIP) algorit...
Definition: data_grid.h:378
data_grid(const data_grid &other, bool copy_data)
Create data grid from an existing grid.
Definition: data_grid.h:775
Definition: data_grid.h:23
virtual seq_vector * clone() const =0
Create a copy using a reference to the base class.
size_t data_grid_compute_offset< 0 >(seq_vector *axis[], const unsigned *index)
Definition: data_grid.h:49
DATA_TYPE interpolate(double *location, DATA_TYPE *derivative=NULL)
Multi-dimensional interpolation with the derivative calculation.
Definition: data_grid.h:576
Definition: data_grid.h:23
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
DATA_TYPE * _data
Multi-dimensional data stored as a linear array in column major order.
Definition: data_grid.h:81
data_grid()
Default constructor for sub-classes.
Definition: data_grid.h:731
~data_grid()
Destroys memory area for field data.
Definition: data_grid.h:796
size_t data_grid_compute_offset(seq_vector *axis[], const unsigned *index)
Definition: data_grid.h:38
void interp_type(int unsigned dimension, enum GRID_INTERP_TYPE type)
Define the type of interpolation for one of the axes.
Definition: data_grid.h:149
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.h:698
void interpolate(const matrix< double > &x, matrix< double > *result, matrix< double > *dx=NULL)
Interpolation 1-D specialization where the arguments, and results, are matrix<double>.
Definition: data_grid.h:633
bool edge_limit(int unsigned dimension) const
Returns the edge_limit flag for the requested dimension.
Definition: data_grid.h:174
DATA_TYPE linear(int dim, const unsigned *index, const double *location, DATA_TYPE &deriv, DATA_TYPE *deriv_vec) const
Perform a linear interpolation on this dimension.
Definition: data_grid.h:278
size_type size() const
Returns the number of elements in this sequence.
Definition: seq_vector.h:138
void interpolate(const matrix< double > &x, const matrix< double > &y, matrix< double > *result, matrix< double > *dx=NULL, matrix< double > *dy=NULL)
Interpolation 2-D specialization where the arguments, and results, are matrix<double>.
Definition: data_grid.h:663
enum GRID_INTERP_TYPE _interp_type[NUM_DIMS]
Defines the type of interpolation for each axis.
Definition: data_grid.h:128
N-dimensional data set and its associated axes.
Definition: data_grid.h:64
GRID_INTERP_TYPE
Type of interpolation used for each axis.
Definition: data_grid.h:21
void edge_limit(int unsigned dimension, bool flag)
Set the edge_limit flag for the requested dimension.
Definition: data_grid.h:186
void data(const unsigned *index, DATA_TYPE value)
Define a new data value at a specific combination of indices.
Definition: data_grid.h:115
Definition: data_grid.h:24
DATA_TYPE nearest(int dim, const unsigned *index, const double *location, DATA_TYPE &deriv, DATA_TYPE *deriv_vec) const
Perform a nearest neighbor interpolation on this dimension.
Definition: data_grid.h:232
A read-only, monotonic sequence of values.
Definition: seq_vector.h:36
DATA_TYPE data(const unsigned *index) const
Extract a data value at a specific combination of indices.
Definition: data_grid.h:102