1 #ifndef INTERP_HPP
2 #define INTERP_HPP
3 //-----------------------------------------------------------------------------
mglLineart(const Treal * a,long nx,long ny,long nz,mreal x,mreal y,mreal z)4 template <class Treal> Treal mglLineart(const Treal *a, long nx, long ny, long nz, mreal x, mreal y, mreal z)
5 {
6 if(!a || nx<1 || ny<1 || nz<1) return 0;
7 Treal b=0,dx,dy,dz,b1,b0;
8 if(x<0 || y<0 || z<0 || x>nx-1 || y>ny-1 || z>nz-1)
9 return 0;
10 if(nz>1 && z!=floor(z)) // 3d interpolation
11 {
12 long kx=long(x), ky=long(y), kz=long(z);
13 dx = x-mreal(kx); dy = y-mreal(ky); dz = z-mreal(kz);
14
15 long i0 = kx+nx*(ky+ny*kz);
16 b0 = a[i0]*(mreal(1)-dx-dy+dx*dy) + dx*(mreal(1)-dy)*a[i0+1] +
17 dy*(mreal(1)-dx)*a[i0+nx] + dx*dy*a[i0+nx+1];
18 i0 = kx+nx*(ky+ny*(kz+1));
19 b1 = a[i0]*(mreal(1)-dx-dy+dx*dy) + dx*(mreal(1)-dy)*a[i0+1] +
20 dy*(mreal(1)-dx)*a[i0+nx] + dx*dy*a[i0+nx+1];
21 b = b0 + dz*(b1-b0);
22 }
23 else if(ny>1 && y!=floor(y)) // 2d interpolation
24 {
25 long kx=long(x), ky=long(y);
26 dx = x-kx; dy=y-ky;
27 long i0 = kx+nx*ky;
28 b = a[i0]*(mreal(1)-dx-dy+dx*dy) + dx*(mreal(1)-dy)*a[i0+1] +
29 dy*(mreal(1)-dx)*a[i0+nx] + dx*dy*a[i0+nx+1];
30 }
31 else if(nx>1 && x!=floor(x)) // 1d interpolation
32 {
33 long kx = long(x);
34 b = a[kx] + (x-kx)*(a[kx+1]-a[kx]);
35 }
36 else // no interpolation
37 b = a[long(x+nx*(y+ny*z))];
38 return b;
39 }
40 //-----------------------------------------------------------------------------
mgl_spline3t(const Treal y[4],long n,mreal dx,Treal & dy)41 template <class Treal> Treal mgl_spline3t(const Treal y[4], long n, mreal dx, Treal &dy)
42 {
43 Treal d[3];
44 d[0] = -(y[2]-mreal(4)*y[1]+mreal(3)*y[0])/mreal(2);
45 d[1] = (y[2]-y[0])/mreal(2);
46 d[2] = (y[3]-y[1])/mreal(2);
47
48 Treal t0 = (y[2]+y[0])/mreal(2)-y[1];
49 Treal t1 = (y[3]+y[1])/mreal(2)-y[2];
50 Treal f0 = y[n], d0 = d[n], res = 0;
51 if(n==1)
52 {
53 Treal df = y[2]-f0, d1 = d[2];
54 Treal b3 = mreal(10)*df+t1-mreal(3)*t0-mreal(4)*d1-mreal(6)*d0;
55 Treal b4 = mreal(-15)*df-mreal(2)*t1+mreal(3)*t0+mreal(7)*d1+mreal(8)*d0;
56 Treal b5 = mreal(6)*df+t1-t0-mreal(3)*d1-mreal(3)*d0;
57 dy = d0 + dx*(mreal(2)*t0+dx*(mreal(3)*b3+dx*(mreal(4)*b4+dx*mreal(5)*b5)));
58 // d2y = mreal(2)*t0 + dx*(mreal(6)*b3+dx*(mreal(12)*b4+dx*mreal(20)*b5)); // 2nd derivative for future
59 res = f0 + dx*(d0+dx*(t0+dx*(b3+dx*(b4+dx*b5))));
60 }
61 else if(n<1)
62 { res = f0 + dx*(d0+dx*t0); dy = d0+dx*t0*mreal(2); }
63 else
64 { res = f0 + dx*(d0+dx*t1); dy = d0+dx*t1*mreal(2); }
65 return res;
66 }
67 //-----------------------------------------------------------------------------
mgl_spline3st(const Treal y[4],long n,mreal dx)68 template <class Treal> Treal mgl_spline3st(const Treal y[4], long n, mreal dx)
69 {
70 Treal d[3];
71 d[0] = -(y[2]-mreal(4)*y[1]+mreal(3)*y[0])/mreal(2);
72 d[1] = (y[2]-y[0])/mreal(2);
73 d[2] = (y[3]-y[1])/mreal(2);
74
75 Treal f0 = y[n], d0 = d[n], res;
76 Treal t0 = (y[2]+y[0])/mreal(2)-y[1];
77 Treal t1 = (y[3]+y[1])/mreal(2)-y[2];
78 if(n==1)
79 {
80 Treal df = y[2]-f0, d1 = d[2];
81 Treal b3 = mreal(10)*df+t1-mreal(3)*t0-mreal(4)*d1-mreal(6)*d0;
82 Treal b4 = mreal(-15)*df-mreal(2)*t1+mreal(3)*t0+mreal(7)*d1+mreal(8)*d0;
83 Treal b5 = mreal(6)*df+t1-t0-mreal(3)*d1-mreal(3)*d0;
84 res = f0 + dx*(d0+dx*(t0+dx*(b3+dx*(b4+dx*b5))));
85 }
86 else res = f0 + dx*(d0+dx*(n<1?t0:t1));
87 return res;
88 }
89 //-----------------------------------------------------------------------------
mglSpline1t(const Treal * a,long nx,mreal x,Treal * dx=0)90 template <class Treal> Treal mglSpline1t(const Treal *a, long nx, mreal x, Treal *dx=0)
91 {
92 Treal r,d;
93 if(nx>3)
94 {
95 long k = long(x);
96 if(k>0 && k<nx-2) r = mgl_spline3t<Treal>(a+k-1, 1, x-k, d);
97 else if(k<1) r = mgl_spline3t<Treal>(a, 0, x, d);
98 else r = mgl_spline3t<Treal>(a+nx-4, 2, x+2-nx, d);
99 }
100 else if(nx<2) { d=0; r = a[0]; }
101 else if(nx==2) { d=a[1]-a[0]; r = a[0]+(a[1]-a[0])*x; }
102 else // nx==3
103 {
104 Treal b1=-(a[2]-mreal(4)*a[1]+mreal(3)*a[0])/mreal(2), b2=(a[2]-mreal(2)*a[1]+a[0])/mreal(2);
105 d = b1+mreal(2)*b2*x; r = a[0]+x*(b1+b2*x);
106 }
107 if(dx) *dx=d;
108 return r;
109 }
110 //-----------------------------------------------------------------------------
mglSpline1st(const Treal * a,long nx,mreal x)111 template <class Treal> Treal mglSpline1st(const Treal *a, long nx, mreal x)
112 {
113 Treal r;
114 if(nx>3)
115 {
116 long k = long(x);
117 if(k>0 && k<nx-2) r = mgl_spline3st<Treal>(a+k-1, 1, x-k);
118 else if(k<1) r = mgl_spline3st<Treal>(a, 0, x);
119 else r = mgl_spline3st<Treal>(a+nx-4, 2, x+2-nx);
120 }
121 else if(nx<2) r = a[0];
122 else if(nx==2) r = a[0]+(a[1]-a[0])*x;
123 else // nx==3
124 {
125 Treal b1=-(a[2]-mreal(4)*a[1]+mreal(3)*a[0])/mreal(2), b2=(a[2]-mreal(2)*a[1]+a[0])/mreal(2);
126 r = a[0]+x*(b1+b2*x);
127 }
128 return r;
129 }
130 //-----------------------------------------------------------------------------
mglSpline3t(const Treal * a,long nx,long ny,long nz,mreal x,mreal y,mreal z,Treal * dx=0,Treal * dy=0,Treal * dz=0)131 template <class Treal> Treal mglSpline3t(const Treal *a, long nx, long ny, long nz, mreal x, mreal y, mreal z, Treal *dx=0, Treal *dy=0, Treal *dz=0)
132 {
133 // if(!a || nx<1 || ny<1 || nz<1) return 0; // NOTE remove this line because this should already checked
134 Treal gx=0,gy=0,gz=0;
135 x = x>0 ?(x<nx-1 ? x:nx-1):0;
136 y = y>0 ?(y<ny-1 ? y:ny-1):0;
137 z = z>0 ?(z<nz-1 ? z:nz-1):0;
138 Treal b;
139 if(nz>1) // 3d interpolation
140 {
141 Treal tz[4], yz[4], xz[4];
142 long kz=long(z)-1, mz, k=long(y)-1, m;
143 if(nz>3)
144 { mz = 4; kz = kz>=0?kz:0;
145 if(kz>nz-4) kz = nz-4; }
146 else { mz = nz; kz=0; }
147 if(ny>3)
148 { m = 4; k = k>=0?k:0;
149 if(k>ny-4) k = ny-4; }
150 else { m = ny; k=0; }
151 for(long j=0;j<mz;j++)
152 {
153 Treal t[4], d[4];
154 for(long i=0;i<m;i++)
155 t[i] = mglSpline1t<Treal>(a+nx*(i+k+ny*(j+kz)),nx,x,d+i);
156 tz[j] = mglSpline1t<Treal>(t,m,y-k,yz+j);
157 xz[j] = mglSpline1t<Treal>(d,m,y-k,0);
158 }
159 b = mglSpline1t<Treal>(tz,mz,z-kz,&gz);
160 gx = mglSpline1t<Treal>(xz,mz,z-kz,0);
161 gy = mglSpline1t<Treal>(yz,mz,z-kz,0);
162 }
163 else if(ny>1) // 2d interpolation
164 {
165 Treal t[4], d[4];
166 long k = long(y)-1, m;
167 if(ny>3)
168 { m = 4; k = k>=0?k:0; if(k>ny-4) k = ny-4; }
169 else { m = ny; k=0; }
170 for(long i=0;i<m;i++)
171 t[i] = mglSpline1t<Treal>(a+nx*(i+k),nx,x,d+i);
172 b = mglSpline1t<Treal>(t,m,y-k,&gy);
173 gx = mglSpline1t<Treal>(d,m,y-k,0);
174 }
175 else // 1d interpolation
176 b = mglSpline1t<Treal>(a,nx,x,&gx);
177 if(dx) *dx=gx;
178 if(dy) *dy=gy;
179 if(dz) *dz=gz;
180 return b;
181 }
182 //-----------------------------------------------------------------------------
mglSpline3st(const Treal * a,long nx,long ny,long nz,mreal x,mreal y,mreal z)183 template <class Treal> Treal mglSpline3st(const Treal *a, long nx, long ny, long nz, mreal x, mreal y, mreal z)
184 {
185 // if(!a || nx<1 || ny<1 || nz<1) return 0; // NOTE remove this line because this should already checked
186 x = x>0 ?(x<nx-1 ? x:nx-1):0;
187 y = y>0 ?(y<ny-1 ? y:ny-1):0;
188 z = z>0 ?(z<nz-1 ? z:nz-1):0;
189 Treal b;
190 if(nz>1) // 3d interpolation
191 {
192 Treal tz[4], t[4];
193 long kz=long(z)-1, mz, k=long(y)-1, m;
194 if(nz>3)
195 { mz = 4; kz = kz>=0?kz:0;
196 if(kz>nz-4) kz = nz-4; }
197 else { mz = nz; kz=0; }
198 if(ny>3)
199 { m = 4; k = k>=0?k:0;
200 if(k>ny-4) k = ny-4; }
201 else { m = ny; k=0; }
202 for(long j=0;j<mz;j++)
203 {
204 for(long i=0;i<m;i++)
205 t[i] = mglSpline1st<Treal>(a+nx*(i+k+ny*(j+kz)),nx,x);
206 tz[j] = mglSpline1st<Treal>(t,m,y-k);
207 }
208 b = mglSpline1st<Treal>(tz,mz,z-kz);
209 }
210 else if(ny>1) // 2d interpolation
211 {
212 Treal t[4];
213 long k = long(y)-1, m;
214 if(ny>3)
215 { m = 4; k = k>=0?k:0;
216 if(k>ny-4) k = ny-4; }
217 else { m = ny; k=0; }
218 for(long i=0;i<m;i++)
219 t[i] = mglSpline1st<Treal>(a+nx*(i+k),nx,x);
220 b = mglSpline1st<Treal>(t,m,y-k);
221 }
222 else // 1d interpolation
223 b = mglSpline1st<Treal>(a,nx,x);
224 return b;
225 }
226 //-----------------------------------------------------------------------------
mgl_gspline_init(long n,const mreal * x,const Treal * v,Treal * c)227 template <class Treal> void mgl_gspline_init(long n, const mreal *x, const Treal *v, Treal *c)
228 { // c must have size 5*(n-1) !!!
229 // if(n<2) return; // NOTE remove this line because this should already checked
230 Treal *a = new Treal[n], *b = new Treal[n];
231 for(long i=0;i<n-1;i++) // basic coefficients
232 { c[5*i] = x[i+1]-x[i]; c[5*i+1] = v[i]; }
233 // progonka
234 a[0] = -0.5; b[0] = mreal(1.5)*(v[1]-v[0])/(x[1]-x[0]);
235 for(long i=1;i<n-1;i++) // solve relative derivative
236 {
237 mreal h0 = x[i]-x[i-1], h1 = x[i+1]-x[i];
238 Treal r = mreal(1)/(2/h0+2/h1 + a[i-1]/h0);
239 a[i] = - r/h1;
240 b[i] = ((3/h0/h0)*(v[i]-v[i-1]) + (3/h1/h1)*(v[i+1]-v[i]) - b[i-1]/h0)*r;
241 }
242 b[n-1] = ( (6/(x[n-1]-x[n-2]))*(v[n-1]-v[n-2]) - mreal(2)*b[n-2] )/(mreal(4)+mreal(2)*a[n-2]);
243 for(long i=n-2;i>=0;i--) b[i] += a[i]*b[i+1];
244 // now spline coefficients
245 for(long i=0;i<n-1;i++)
246 {
247 c[5*i+2] = b[i];
248 mreal h = 1/(x[i+1]-x[i]), h2 = h*h;
249 c[5*i+3] = (3*h2)*(v[i+1]-v[i]) - (b[i+1]+b[i]+b[i])*h;
250 c[5*i+4] = (2*h2*h)*(v[i]-v[i+1]) + (b[i+1]+b[i])*h2;
251 }
252 delete []a; delete []b;
253 }
254 //-----------------------------------------------------------------------------
255 struct mglEqTxT
256 {
257 std::vector<std::string> str;
258 HAEX *eqC;
259 HMEX *eqR;
260 const char *var;
261 char brd;
262 long m,n;
263 std::vector<mglDataA*> head;
264 HMDT t;
265
mglEqTxTmglEqTxT266 mglEqTxT(const char *vars=0):eqC(0),eqR(0),var(vars),brd(0) {}
~mglEqTxTmglEqTxT267 ~mglEqTxT()
268 {
269 if(eqR) { for(size_t i=0;i<str.size();i++) mgl_delete_expr(eqR[i]); delete []eqR; }
270 if(eqC) { for(size_t i=0;i<str.size();i++) mgl_delete_cexpr(eqC[i]); delete []eqC; }
271 }
FillStrmglEqTxT272 void FillStr(const char *eqs)
273 {
274 const char *f=eqs;
275 while(1)
276 {
277 const char *g = strchr(f,';');
278 if(g) str.push_back(std::string(f,g-f));
279 else { str.push_back(f); break; }
280 f = g+1;
281 }
282 }
FillRealmglEqTxT283 void FillReal(const char *eqs)
284 {
285 FillStr(eqs); size_t n = str.size();
286 if(n==0) return;
287 eqR = new HMEX[n];
288 for(size_t i=0;i<n;i++) eqR[i] = mgl_create_expr(str[i].c_str());
289 }
FillCmplxmglEqTxT290 void FillCmplx(const char *eqs)
291 {
292 FillStr(eqs); size_t n = str.size();
293 if(n==0) return;
294 eqC = new HAEX[n];
295 for(size_t i=0;i<n;i++) eqC[i] = mgl_create_cexpr(str[i].c_str());
296 }
297 };
298 //-----------------------------------------------------------------------------
299 #endif
300