00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 #include "LAStreams.h"
00058 #include "math_num.h"
00059 #include "std.h"
00060 #include <float.h>
00061
00062
00063 namespace linalg
00064 {
00065 using namespace linalg;
00066
00067
00068
00069
00070
00071
00072
00073
00074 class ALInterp
00075 {
00076 Vector arg;
00077 Vector val;
00078 double q;
00079
00080
00081 ALInterp(const ALInterp&);
00082 void operator = (const ALInterp&);
00083
00084 public:
00085
00086
00087 ALInterp(const double q, const double x0, const double s, const Vector& y);
00088
00089
00090 ALInterp(const double q, const Vector& x, const Vector& y);
00091
00092 double interpolate(void);
00093 };
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109 ALInterp::ALInterp(const double _q, const double x0,
00110 const double s, const Vector& y)
00111 : arg(y.q_no_elems()), val(y.q_no_elems()), q(_q)
00112 {
00113 const int n = y.q_no_elems();
00114 assure( n > 1, "Vector y (function values) must have at least 2 points");
00115 assure( s > 0, "The grid mesh has to be positive");
00116
00117
00118
00119
00120
00121
00122 int js = (int)( (q-x0)/s + 1.5 );
00123
00124 if( js < 1 )
00125 js = 1;
00126 else if( js > n )
00127 js = n;
00128
00129
00130
00131 bool right_pt_is_closer = q > x0 + (js-1)*s;
00132
00133 register int jcurr = js, jleft = js, jright = js;
00134
00135
00136 for(LAStreamOut args(arg), vals(val); !args.eof(); )
00137 {
00138 args.get() = x0 + (jcurr-1)*s;
00139 vals.get() = y(jcurr-1+y.q_lwb());
00140 if( jright >= n )
00141 right_pt_is_closer = false;
00142 if( jleft <= 1 )
00143 right_pt_is_closer = true;
00144 if( right_pt_is_closer )
00145 jcurr = ++jright, right_pt_is_closer = false;
00146 else
00147 jcurr = --jleft, right_pt_is_closer = true;
00148 }
00149 }
00150
00151
00152 static inline int fsign(const float f)
00153 { return f < 0 ? -1 : f==0 ? 0 : 1; }
00154
00155
00156
00157 ALInterp::ALInterp(const double _q, const Vector& x, const Vector& y)
00158 : arg(x.q_no_elems()), val(y.q_no_elems()), q(_q)
00159 {
00160 assure( y.q_no_elems() > 1,
00161 "Vector y (function values) must have at least 2 points");
00162 are_compatible(x,y);
00163
00164
00165
00166
00167
00168 class index_permutation
00169 {
00170 struct El { int x_ind; float x_to_q; };
00171 El * const permutation;
00172 const int n;
00173 static int comparison_func(const void * ip, const void * jp)
00174 { return fsign(((const El*)ip)->x_to_q - ((const El*)jp)->x_to_q); }
00175 public:
00176 index_permutation(const double q, const Vector& x) :
00177 permutation(new El[x.q_no_elems()]), n(x.q_no_elems())
00178 {
00179 register El * pp = permutation;
00180 for(register int i=x.q_lwb(); i<=x.q_upb(); i++,pp++)
00181 pp->x_ind = i, pp->x_to_q = fabs(q-x(i));
00182
00183
00184
00185 qsort(permutation,n,sizeof(permutation[0]),comparison_func);
00186 }
00187 ~index_permutation(void) { delete permutation; }
00188
00189
00190 void apply(Vector& arg, Vector& val, const Vector& x, const Vector& y)
00191 {
00192 register const El* pp = permutation;
00193 for(LAStreamOut args(arg), vals(val); !args.eof(); pp++)
00194 args.get() = x(pp->x_ind), vals.get() = y(pp->x_ind);
00195 assert(pp==permutation+n);
00196 }
00197 };
00198
00199 index_permutation(q,x).apply(arg,val,x,y);
00200 }
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 double ALInterp::interpolate()
00211 {
00212 LAStreamIn args(arg);
00213 LAStreamOut vals(val);
00214 register double valp = vals.peek();
00215 register double diffp = DBL_MAX;
00216
00217 #ifdef DEBUG
00218 arg.print("arg - interpolation nodes");
00219 val.print("Arranged table of function values");
00220 #endif
00221
00222
00223 for(register int j=2; j<=val.q_upb(); j++)
00224 {
00225 register double argj = (args.get(), args.peek());
00226 register REAL& valj = (vals.get(), vals.peek());
00227 args.rewind(); vals.rewind();
00228 for(register int i=1; i<=j-1; i++)
00229 {
00230 double argi = args.get();
00231 valj = ( vals.get()*(q-argj) - valj*(q-argi) ) / (argi - argj);
00232 }
00233
00234 #ifdef DEBUG
00235 message("\nval(j) = %g, valp = %g, arg(j) = %g",valj,valp,argj);
00236 #endif
00237
00238 register double diff = fabs( valj - valp );
00239
00240 if( j>2 && diff == 0 )
00241 break;
00242
00243 if( j>4 && diff > diffp )
00244 break;
00245
00246 valp = valj; diffp = diff;
00247 }
00248
00249 return valp;
00250 }
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260 double ali(const double q, const double x0, const double s, const Vector& y)
00261 {
00262 ALInterp al(q,x0,s,y);
00263 return al.interpolate();
00264 }
00265
00266
00267 double ali(const double q, const Vector& x, const Vector& y)
00268 {
00269 ALInterp al(q,x,y);
00270 return al.interpolate();
00271 }
00272
00273 }