1 /*****************************************************************************
2 Major portions of this software are copyrighted by the Medical College
3 of Wisconsin, 1994-2000, and are released under the Gnu General Public
4 License, Version 2. See the file README.Copyright for details.
5 ******************************************************************************/
6
7 #include "mrilib.h"
8
9 /******************* Routines to shift two rows at a time ********************/
10
11 /*---------------------------------------------------------------------------
12 Set the interpolation method for shifting:
13 input is one of MRI_NN, MRI_LINEAR, MRI_CUBIC, or MRI_FOURIER.
14 -----------------------------------------------------------------------------*/
15
16 typedef void (*shift_func)(int,int,float,float *,float,float *) ;
17 static shift_func shifter = fft_shift2 ;
18 static int shift_method = MRI_FOURIER ;
19
SHIFT_set_method(int mode)20 void SHIFT_set_method( int mode )
21 {
22 shift_method = mode ;
23 switch( mode ){
24 default: shift_method = MRI_FOURIER ; /* fall thru */
25 case MRI_FOURIER: shifter = fft_shift2 ; break ;
26
27 case MRI_LINEAR: shifter = lin_shift2 ; break ;
28 case MRI_CUBIC: shifter = cub_shift2 ; break ;
29 case MRI_QUINTIC: shifter = quint_shift2 ; break ; /* Nov 1998 */
30 case MRI_HEPTIC: shifter = hept_shift2 ; break ; /* Nov 1998 */
31
32 case MRI_NN: shifter = nn_shift2 ; break ; /* experimental */
33 case MRI_TSSHIFT: shifter = ts_shift2 ; break ; /* Dec 1999 */
34
35 case MRI_WSINC5: shifter = wsinc5_shift2 ; break ; /* Aug 2019 */
36 case MRI_WSINC9: shifter = wsinc9_shift2 ; break ; /* Aug 2019 */
37 }
38 return ;
39 }
40
SHIFT_get_method(void)41 int SHIFT_get_method( void ){ return shift_method ; }
42
43 /*--------------------------------------------------------------------------
44 The main entry point - note that g can be NULL, but f cannot.
45 ---------------------------------------------------------------------------*/
46
SHIFT_two_rows(int n,int nup,float af,float * f,float ag,float * g)47 void SHIFT_two_rows( int n, int nup, float af, float * f, float ag, float * g )
48 {
49 shifter( n,nup,af,f,ag,g ) ; return ;
50 }
51
52 /*--------------------------------------------------------------------------
53 Shift 2 rows at a time with the FFT:
54 n = length of a row
55 nup = length to use for FFTs (must be even)
56 af = shift for row f
57 ag = shift for row g
58 Input and output arrays are f[n] and g[n]. (Note: g may be NULL.)
59 ----------------------------------------------------------------------------*/
60
61 #define ZFILL
62 #define RECUR
63
fft_shift2(int n,int nup,float af,float * f,float ag,float * g)64 void fft_shift2( int n, int nup, float af, float * f, float ag, float * g )
65 {
66 static int nupold=0 , nuptop=0 ;
67 static complex * row=NULL , * cf=NULL , * cg=NULL ;
68
69 int ii , nby2=nup/2 , n21=nby2+1 ;
70 complex fac , gac ;
71 float sf , sg , dk ;
72 #ifdef RECUR
73 complex csf , csg ;
74 #endif
75
76 ENTRY("fft_shift2") ;
77
78 /* 15 Mar 2001: shift too big ==> return all zeros */
79
80 if( (af < -n || af > n) && (ag < -n || ag > n) ){
81 for( ii=0 ; ii < n ; ii++ ) f[ii] = g[ii] = 0.0 ;
82 EXRETURN ;
83 }
84
85 /* make new memory for row storage? */
86
87 if( nup > nuptop ){
88 if( row != NULL ){ free(row) ; free(cf) ; free(cg) ; }
89 row = (complex *) malloc( sizeof(complex) * nup ) ;
90 cf = (complex *) malloc( sizeof(complex) * n21 ) ;
91 cg = (complex *) malloc( sizeof(complex) * n21 ) ;
92 nuptop = nup ;
93 }
94
95 /* FFT the pair of rows */
96
97 if( g != NULL )
98 for( ii=0 ; ii < n ; ii++ ){ row[ii].r = f[ii] ; row[ii].i = g[ii] ; }
99 else
100 for( ii=0 ; ii < n ; ii++ ){ row[ii].r = f[ii] ; row[ii].i = 0 ; }
101
102 #ifdef ZFILL
103 for( ii=n ; ii < nup ; ii++ ){ row[ii].r = row[ii].i = 0.0 ; }
104 #else
105 if( nup > n ){
106 sf = 0.5 * (row[0].r + row[n-1].r) ; sg = 0.5 * (row[0].i + row[n-1].i) ;
107 for( ii=n ; ii < nup ; ii++ ){ row[ii].r = sf ; row[ii].i = sg ; }
108 }
109 #endif
110
111 csfft_cox( -1 , nup , row ) ;
112
113 /* untangle FFT coefficients from row into cf,cg */
114
115 cf[0].r = 2.0 * row[0].r ; cf[0].i = 0.0 ; /* twice too big */
116 cg[0].r = 2.0 * row[0].i ; cg[0].i = 0.0 ;
117 for( ii=1 ; ii < nby2 ; ii++ ){
118 cf[ii].r = row[ii].r + row[nup-ii].r ;
119 cf[ii].i = row[ii].i - row[nup-ii].i ;
120 cg[ii].r = row[ii].i + row[nup-ii].i ;
121 cg[ii].i = -row[ii].r + row[nup-ii].r ;
122 }
123 cf[nby2].r = 2.0 * row[nby2].r ; cf[nby2].i = 0.0 ;
124 cg[nby2].r = 2.0 * row[nby2].i ; cg[nby2].i = 0.0 ;
125
126 /* phase shift both rows (cf,cg) */
127
128 dk = (2.0*PI) / nup ;
129 sf = -af * dk ; sg = -ag * dk ;
130
131 #ifdef RECUR
132 csf = CEXPIT(sf) ; csg = CEXPIT(sg) ;
133 fac.r = gac.r = 1.0 ;
134 fac.i = gac.i = 0.0 ;
135 #endif
136
137 for( ii=1 ; ii <= nby2 ; ii++ ){
138 #ifdef RECUR
139 fac = CMULT( csf , fac ) ; cf[ii] = CMULT( fac , cf[ii] ) ;
140 gac = CMULT( csg , gac ) ; cg[ii] = CMULT( gac , cg[ii] ) ;
141 #else
142 fac = CEXPIT(ii*sf) ; cf[ii] = CMULT( fac , cf[ii] ) ;
143 gac = CEXPIT(ii*sg) ; cg[ii] = CMULT( gac , cg[ii] ) ;
144 #endif
145 }
146 cf[nby2].i = 0.0 ; cg[nby2].i = 0.0 ;
147
148 /* retangle the coefficients from 2 rows */
149
150 row[0].r = cf[0].r ; row[0].i = cg[0].r ;
151 for( ii=1 ; ii < nby2 ; ii++ ){
152 row[ii].r = cf[ii].r - cg[ii].i ;
153 row[ii].i = cf[ii].i + cg[ii].r ;
154 row[nup-ii].r = cf[ii].r + cg[ii].i ;
155 row[nup-ii].i = -cf[ii].i + cg[ii].r ;
156 }
157 row[nby2].r = cf[nby2].r ;
158 row[nby2].i = cg[nby2].r ;
159
160 /* inverse FFT and store back in output arrays */
161
162 csfft_cox( 1 , nup , row ) ;
163
164 sf = 0.5 / nup ; /* 0.5 to allow for twice too big above */
165
166 if( g != NULL )
167 for( ii=0; ii < n; ii++ ){ f[ii] = sf*row[ii].r; g[ii] = sf*row[ii].i; }
168 else
169 for( ii=0; ii < n; ii++ ){ f[ii] = sf*row[ii].r; }
170
171 EXRETURN ;
172 }
173
174 /*--------------------------------------------------------------------------
175 Some stuff needed for polynomial interpolation
176 ----------------------------------------------------------------------------*/
177
178 static int nlcbuf = 0 ; /* workspace */
179 static float * lcbuf = NULL ;
180
181 /* f[i], but inside the legal range */
182
183 #ifdef ZFILL
184 # define FINS(i) ( ((i)<0 || (i)>=n) ? 0.0 : f[(i)] )
185 #else
186 # define FINS(i) ( ((i)<0) ? f[0] : ((i)>=n) ? f[n-1] : f[(i)] )
187 #endif
188
189 #define SEPARATE_FINS
190
191 /*===========================================================================*/
192
193 /*---------------------------------------------------------------------------*/
194 /* Shift 1 row with weighted sinc interpolation. [Aug 2019] */
195 /*---------------------------------------------------------------------------*/
196
197 #undef PIF
198 #define PIF 3.1415927f /* PI in float */
199
200 /* sinc function = sin(PI*x)/(PI*x) [N.B.: x will always be >= 0] */
201
202 #undef sinc
203 #define sinc(x) ( ((x)>0.01f) ? sinf(PIF*(x))/(PIF*(x)) \
204 : 1.0f - 1.6449341f*(x)*(x) )
205
206 /* M3(x) = minimum sidelobe 3 term window
207 = declines from 1 to 0 as x goes from 0 to 1 */
208
209 #undef M3
210 #define M3(x) (0.4243801f+0.4973406f*cosf(PIF*(x))+0.0782793f*cosf(PIF*(x)*2.0f))
211
212 /* interpolating function = weighted sinc, for x in [0..5] */
213
214 #undef wwsinc5
215 #define wwsinc5(x) ( (x <= 5.0f) ? (sinc(x)*M3(0.19999f*(x))) : 0.0f )
216
wsinc5_shift(int n,float af,float * f)217 void wsinc5_shift( int n , float af , float *f )
218 {
219 int ii , ia , ix ;
220 float aa , xx ;
221 float wt_m4, wt_m3, wt_m2, wt_m1 , wt_00 , wt_p1, wt_p2, wt_p3, wt_p4, wt_p5 ;
222 int ibot,itop ;
223
224 ENTRY("wsinc5_shift") ;
225
226 if( fabsf(af) < 0.0001f ) EXRETURN ; /* little or nothing to do */
227
228 af = -af ; ia = (int)af ; if( af < 0 ) ia-- ;
229 if( ia <= -n || ia >= n ){
230 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
231 EXRETURN ;
232 }
233
234 aa = af - ia ; /* between 0 and 1 */
235 xx = aa+4.0f ; wt_m4 = wwsinc5(xx) ; /* arg to wwsinc5 is always positive */
236 xx = aa+3.0f ; wt_m3 = wwsinc5(xx) ; /* and is distance from data point */
237 xx = aa+2.0f ; wt_m2 = wwsinc5(xx) ; /* to 'aa'; e.g., for m3 = -3, */
238 xx = aa+1.0f ; wt_m1 = wwsinc5(xx) ; /* distance is aa+3 */
239 xx = aa ; wt_00 = wwsinc5(xx) ;
240 xx = 1.0f-aa ; wt_p1 = wwsinc5(xx) ;
241 xx = 2.0f-aa ; wt_p2 = wwsinc5(xx) ;
242 xx = 3.0f-aa ; wt_p3 = wwsinc5(xx) ;
243 xx = 4.0f-aa ; wt_p4 = wwsinc5(xx) ;
244 xx = 5.0f-aa ; wt_p5 = wwsinc5(xx) ;
245
246 if( n > nlcbuf ){
247 if( lcbuf != NULL ) free(lcbuf) ;
248 lcbuf = (float *) malloc( sizeof(float) * n ) ;
249 nlcbuf = n ;
250 }
251
252 ibot = 5-ia ; if( ibot < 0 ) ibot = 0 ;
253 itop = n-6-ia ; if( itop > n-1 ) itop = n-1 ;
254
255 for( ii=ibot ; ii <= itop ; ii++ ){
256 ix = ii + ia ;
257 lcbuf[ii] = wt_m4 * f[ix-4] + wt_m3 * f[ix-3]
258 + wt_m2 * f[ix-2] + wt_m1 * f[ix-1] + wt_00 * f[ix]
259 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] + wt_p3 * f[ix+3]
260 + wt_p4 * f[ix+4] + wt_p5 * f[ix+5] ;
261 }
262
263 if( ibot > n ) ibot = n ;
264 for( ii=0 ; ii < ibot ; ii++ ){
265 ix = ii + ia ;
266 lcbuf[ii] = wt_m4 * FINS(ix-4) + wt_m3 * FINS(ix-3)
267 + wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
268 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
269 + wt_p4 * FINS(ix+4) + wt_p5 * FINS(ix+5) ;
270 }
271
272 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
273 for( ii=itop+1 ; ii < n ; ii++ ){
274 ix = ii + ia ;
275 lcbuf[ii] = wt_m4 * FINS(ix-4) + wt_m3 * FINS(ix-3)
276 + wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
277 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
278 + wt_p4 * FINS(ix+4) + wt_p5 * FINS(ix+5) ;
279 }
280
281 memcpy( f , lcbuf , sizeof(float)*n ) ;
282 EXRETURN ;
283 }
284
wsinc5_shift2(int n,int nup,float af,float * f,float ag,float * g)285 void wsinc5_shift2( int n, int nup, float af, float * f, float ag, float * g )
286 {
287 wsinc5_shift( n , af , f ) ;
288 if( g != NULL ) wsinc5_shift( n , ag , g ) ;
289 return ;
290 }
291
292 /*----------------------------------------------------------------------------*/
293
294 #undef wwsinc9
295 #define wwsinc9(x) ( (x <= 9.0f) ? (sinc(x)*M3(0.11111f*(x))) : 0.0f )
296
wsinc9_shift(int n,float af,float * f)297 void wsinc9_shift( int n , float af , float *f )
298 {
299 int ii , ia , ix ;
300 float aa , xx ;
301 float wt_m4, wt_m3, wt_m2, wt_m1 , wt_00 , wt_p1, wt_p2, wt_p3, wt_p4, wt_p5 ;
302 float wt_m5, wt_m6, wt_m7, wt_m8 , wt_p6, wt_p7, wt_p8, wt_p9 ;
303 int ibot,itop ;
304
305 ENTRY("wsinc9_shift") ;
306
307 if( fabsf(af) < 0.0001f ) EXRETURN ; /* little or nothing to do */
308
309 af = -af ; ia = (int)af ; if( af < 0 ) ia-- ;
310 if( ia <= -n || ia >= n ){
311 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
312 EXRETURN ;
313 }
314
315 aa = af - ia ; /* between 0 and 1 */
316 xx = aa+8.0f ; wt_m8 = wwsinc9(xx) ;
317 xx = aa+7.0f ; wt_m7 = wwsinc9(xx) ;
318 xx = aa+6.0f ; wt_m6 = wwsinc9(xx) ;
319 xx = aa+5.0f ; wt_m5 = wwsinc9(xx) ;
320 xx = aa+4.0f ; wt_m4 = wwsinc9(xx) ;
321 xx = aa+3.0f ; wt_m3 = wwsinc9(xx) ;
322 xx = aa+2.0f ; wt_m2 = wwsinc9(xx) ;
323 xx = aa+1.0f ; wt_m1 = wwsinc9(xx) ;
324 xx = aa ; wt_00 = wwsinc9(xx) ;
325 xx = 1.0f-aa ; wt_p1 = wwsinc9(xx) ;
326 xx = 2.0f-aa ; wt_p2 = wwsinc9(xx) ;
327 xx = 3.0f-aa ; wt_p3 = wwsinc9(xx) ;
328 xx = 4.0f-aa ; wt_p4 = wwsinc9(xx) ;
329 xx = 5.0f-aa ; wt_p5 = wwsinc9(xx) ;
330 xx = 6.0f-aa ; wt_p6 = wwsinc9(xx) ;
331 xx = 7.0f-aa ; wt_p7 = wwsinc9(xx) ;
332 xx = 8.0f-aa ; wt_p8 = wwsinc9(xx) ;
333 xx = 9.0f-aa ; wt_p9 = wwsinc9(xx) ;
334
335 if( n > nlcbuf ){
336 if( lcbuf != NULL ) free(lcbuf) ;
337 lcbuf = (float *) malloc( sizeof(float) * n ) ;
338 nlcbuf = n ;
339 }
340
341 ibot = 9-ia ; if( ibot < 0 ) ibot = 0 ;
342 itop = n-10-ia ; if( itop > n-1 ) itop = n-1 ;
343
344 for( ii=ibot ; ii <= itop ; ii++ ){
345 ix = ii + ia ;
346 lcbuf[ii] = wt_m8 * f[ix-8] + wt_m7 * f[ix-7]
347 + wt_m6 * f[ix-6] + wt_m5 * f[ix-5]
348 + wt_m4 * f[ix-4] + wt_m3 * f[ix-3]
349 + wt_m2 * f[ix-2] + wt_m1 * f[ix-1] + wt_00 * f[ix]
350 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] + wt_p3 * f[ix+3]
351 + wt_p4 * f[ix+4] + wt_p5 * f[ix+5]
352 + wt_p6 * f[ix+6] + wt_p7 * f[ix+7] + wt_p8 * f[ix+8]
353 + wt_p9 * f[ix+9] ;
354 }
355
356 if( ibot > n ) ibot = n ;
357 for( ii=0 ; ii < ibot ; ii++ ){
358 ix = ii + ia ;
359 lcbuf[ii] = wt_m8 * FINS(ix-8) + wt_m7 * FINS(ix-7)
360 + wt_m6 * FINS(ix-6) + wt_m5 * FINS(ix-5)
361 + wt_m4 * FINS(ix-4) + wt_m3 * FINS(ix-3)
362 + wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
363 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
364 + wt_p4 * FINS(ix+4) + wt_p5 * FINS(ix+5)
365 + wt_p6 * FINS(ix+6) + wt_p7 * FINS(ix+7) + wt_p8 * FINS(ix+8)
366 + wt_p9 * FINS(ix+9) ;
367 }
368
369 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
370 for( ii=itop+1 ; ii < n ; ii++ ){
371 ix = ii + ia ;
372 lcbuf[ii] = wt_m8 * FINS(ix-8) + wt_m7 * FINS(ix-7)
373 + wt_m6 * FINS(ix-6) + wt_m5 * FINS(ix-5)
374 + wt_m4 * FINS(ix-4) + wt_m3 * FINS(ix-3)
375 + wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
376 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
377 + wt_p4 * FINS(ix+4) + wt_p5 * FINS(ix+5)
378 + wt_p6 * FINS(ix+6) + wt_p7 * FINS(ix+7) + wt_p8 * FINS(ix+8)
379 + wt_p9 * FINS(ix+9) ;
380 }
381
382 memcpy( f , lcbuf , sizeof(float)*n ) ;
383 EXRETURN ;
384 }
385
wsinc9_shift2(int n,int nup,float af,float * f,float ag,float * g)386 void wsinc9_shift2( int n, int nup, float af, float * f, float ag, float * g )
387 {
388 wsinc9_shift( n , af , f ) ;
389 if( g != NULL ) wsinc9_shift( n , ag , g ) ;
390 return ;
391 }
392
393 /*---------------------------------------------------------------------------
394 Shift 1 row with with heptic Lagrange polynomial interpolation [Nov 1998].
395 Note that heptic interpolation is about the same as Hamming-weighted
396 3-sidelobe sinc interpolation .
397 -----------------------------------------------------------------------------*/
398
399 /* seventh order polynomials */
400
401 #define S_M3(x) (x*(x*x-1.0)*(x*x-4.0)*(x-3.0)*(4.0-x)*0.0001984126984)
402 #define S_M2(x) (x*(x*x-1.0)*(x-2.0)*(x*x-9.0)*(x-4.0)*0.001388888889)
403 #define S_M1(x) (x*(x-1.0)*(x*x-4.0)*(x*x-9.0)*(4.0-x)*0.004166666667)
404 #define S_00(x) ((x*x-1.0)*(x*x-4.0)*(x*x-9.0)*(x-4.0)*0.006944444444)
405 #define S_P1(x) (x*(x+1.0)*(x*x-4.0)*(x*x-9.0)*(4.0-x)*0.006944444444)
406 #define S_P2(x) (x*(x*x-1.0)*(x+2.0)*(x*x-9.0)*(x-4.0)*0.004166666667)
407 #define S_P3(x) (x*(x*x-1.0)*(x*x-4.0)*(x+3.0)*(4.0-x)*0.001388888889)
408 #define S_P4(x) (x*(x*x-1.0)*(x*x-4.0)*(x*x-9.0)*0.0001984126984)
409
hept_shift(int n,float af,float * f)410 void hept_shift( int n , float af , float * f )
411 {
412 int ii , ia , ix ;
413 float wt_m1,wt_00,wt_p1,wt_p2 , aa , wt_m2,wt_p3,wt_m3,wt_p4;
414 #ifdef SEPARATE_FINS
415 int ibot,itop ;
416 #endif
417
418 ENTRY("hept_shift") ;
419
420 af = -af ; ia = (int) af ; if( af < 0 ) ia-- ; /* ia = floor */
421
422 /* 15 Mar 2001: if shift is too large, return all zeros */
423
424 if( ia <= -n || ia >= n ){
425 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
426 EXRETURN ;
427 }
428
429 aa = af - ia ;
430 wt_m1 = S_M1(aa) ; wt_00 = S_00(aa) ;
431 wt_p1 = S_P1(aa) ; wt_p2 = S_P2(aa) ;
432 wt_m2 = S_M2(aa) ; wt_p3 = S_P3(aa) ;
433 wt_m3 = S_M3(aa) ; wt_p4 = S_P4(aa) ;
434
435 if( n > nlcbuf ){
436 if( lcbuf != NULL ) free(lcbuf) ;
437 lcbuf = (float *) malloc( sizeof(float) * n ) ;
438 nlcbuf = n ;
439 }
440
441 #ifdef SEPARATE_FINS
442 ibot = 3-ia ; if( ibot < 0 ) ibot = 0 ;
443 itop = n-5-ia ; if( itop > n-1 ) itop = n-1 ;
444
445 for( ii=ibot ; ii <= itop ; ii++ ){
446 ix = ii + ia ;
447 lcbuf[ii] = wt_m2 * f[ix-2] + wt_m1 * f[ix-1] + wt_00 * f[ix]
448 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] + wt_p3 * f[ix+3]
449 + wt_m3 * f[ix-3] + wt_p4 * f[ix+4] ;
450 }
451
452 if( ibot > n ) ibot = n ; /* 15 Mar 2001 */
453 for( ii=0 ; ii < ibot ; ii++ ){
454 ix = ii + ia ;
455 lcbuf[ii] = wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
456 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
457 + wt_m3 * FINS(ix-3) + wt_p4 * FINS(ix+4) ;
458 }
459
460 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
461 for( ii=itop+1 ; ii < n ; ii++ ){
462 ix = ii + ia ;
463 lcbuf[ii] = wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
464 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
465 + wt_m3 * FINS(ix-3) + wt_p4 * FINS(ix+4) ;
466 }
467 #else /* not SEPARATE_FINS */
468 for( ii=0 ; ii < n ; ii++ ){
469 ix = ii + ia ;
470 if( ix > 1 && ix < n-3 )
471 lcbuf[ii] = wt_m2 * f[ix-2] + wt_m1 * f[ix-1] + wt_00 * f[ix]
472 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] + wt_p3 * f[ix+3]
473 + wt_m3 * f[ix-3] + wt_p4 * f[ix+4] ;
474 else
475 lcbuf[ii] = wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
476 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3)
477 + wt_m3 * FINS(ix-3) + wt_p4 * FINS(ix+4) ;
478 }
479 #endif /* SEPARATE_FINS */
480
481 memcpy( f , lcbuf , sizeof(float)*n ) ;
482 EXRETURN ;
483 }
484
hept_shift2(int n,int nup,float af,float * f,float ag,float * g)485 void hept_shift2( int n, int nup, float af, float * f, float ag, float * g )
486 {
487 hept_shift( n , af , f ) ;
488 if( g != NULL ) hept_shift( n , ag , g ) ;
489 return ;
490 }
491
492 /*---------------------------------------------------------------------------
493 Shift 1 row with with quintic Lagrange polynomial interpolation [Nov 1998].
494 Note that quintic interpolation is about the same as Hamming-weighted
495 2-sidelobe sinc interpolation .
496 -----------------------------------------------------------------------------*/
497
498 /* quintic interpolation polynomials (Lagrange) */
499
500 #define Q_M2(x) (x*(x*x-1.0)*(2.0-x)*(x-3.0)*0.008333333)
501 #define Q_M1(x) (x*(x*x-4.0)*(x-1.0)*(x-3.0)*0.041666667)
502 #define Q_00(x) ((x*x-4.0)*(x*x-1.0)*(3.0-x)*0.083333333)
503 #define Q_P1(x) (x*(x*x-4.0)*(x+1.0)*(x-3.0)*0.083333333)
504 #define Q_P2(x) (x*(x*x-1.0)*(x+2.0)*(3.0-x)*0.041666667)
505 #define Q_P3(x) (x*(x*x-1.0)*(x*x-4.0)*0.008333333)
506
quint_shift(int n,float af,float * f)507 void quint_shift( int n , float af , float * f )
508 {
509 int ii , ia , ix ;
510 float wt_m1 , wt_00 , wt_p1 , wt_p2 , aa , wt_m2 , wt_p3 ;
511 #ifdef SEPARATE_FINS
512 int ibot,itop ;
513 #endif
514
515 ENTRY("quint_shift") ;
516
517 af = -af ; ia = (int) af ; if( af < 0 ) ia-- ; /* ia = floor */
518
519 /* 15 Mar 2001: if shift is too large, return all zeros */
520
521 if( ia <= -n || ia >= n ){
522 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
523 EXRETURN ;
524 }
525
526 aa = af - ia ;
527 wt_m1 = Q_M1(aa) ; wt_00 = Q_00(aa) ;
528 wt_p1 = Q_P1(aa) ; wt_p2 = Q_P2(aa) ;
529 wt_m2 = Q_M2(aa) ; wt_p3 = Q_P3(aa) ;
530
531 if( n > nlcbuf ){
532 if( lcbuf != NULL ) free(lcbuf) ;
533 lcbuf = (float *) malloc( sizeof(float) * n ) ;
534 nlcbuf = n ;
535 }
536
537 #ifdef SEPARATE_FINS
538 ibot = 2-ia ; if( ibot < 0 ) ibot = 0 ;
539 itop = n-4-ia ; if( itop > n-1 ) itop = n-1 ;
540
541 for( ii=ibot ; ii <= itop ; ii++ ){
542 ix = ii + ia ;
543 lcbuf[ii] = wt_m2 * f[ix-2] + wt_m1 * f[ix-1] + wt_00 * f[ix]
544 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] + wt_p3 * f[ix+3] ;
545 }
546
547 if( ibot > n ) ibot = n ; /* 15 Mar 2001 */
548 for( ii=0 ; ii < ibot ; ii++ ){
549 ix = ii + ia ;
550 lcbuf[ii] = wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
551 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3) ;
552 }
553
554 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
555 for( ii=itop+1 ; ii < n ; ii++ ){
556 ix = ii + ia ;
557 lcbuf[ii] = wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
558 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3) ;
559 }
560 #else /* not SEPARATE_FINS */
561 for( ii=0 ; ii < n ; ii++ ){
562 ix = ii + ia ;
563 if( ix > 1 && ix < n-3 )
564 lcbuf[ii] = wt_m2 * f[ix-2] + wt_m1 * f[ix-1] + wt_00 * f[ix]
565 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] + wt_p3 * f[ix+3] ;
566 else
567 lcbuf[ii] = wt_m2 * FINS(ix-2) + wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
568 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) + wt_p3 * FINS(ix+3) ;
569 }
570 #endif /* SEPARATE_FINS */
571
572 memcpy( f , lcbuf , sizeof(float)*n ) ;
573 EXRETURN ;
574 }
575
quint_shift2(int n,int nup,float af,float * f,float ag,float * g)576 void quint_shift2( int n, int nup, float af, float * f, float ag, float * g )
577 {
578 quint_shift( n , af , f ) ;
579 if( g != NULL ) quint_shift( n , ag , g ) ;
580 return ;
581 }
582
583 /*---------------------------------------------------------------------------
584 Shift 1 row with with cubic interpolation
585 -----------------------------------------------------------------------------*/
586
587 /* cubic interpolation polynomials */
588
589 #define P_M1(x) ((x)*(1.0-(x))*((x)-2.0)*0.1666667)
590 #define P_00(x) (((x)+1.0)*((x)-1.0)*((x)-2.0)*0.5)
591 #define P_P1(x) ((x)*((x)+1.0)*(2.0-(x))*0.5)
592 #define P_P2(x) ((x)*((x)+1.0)*((x)-1.0)*0.1666667)
593
cub_shift(int n,float af,float * f)594 void cub_shift( int n , float af , float * f )
595 {
596 int ii , ia , ix ;
597 float wt_m1 , wt_00 , wt_p1 , wt_p2 , aa ;
598 #ifdef SEPARATE_FINS
599 int ibot,itop ;
600 #endif
601
602 ENTRY("cub_shift") ;
603
604 af = -af ; ia = (int) af ; if( af < 0 ) ia-- ; /* ia = floor */
605
606 /* 15 Mar 2001: if shift is too large, return all zeros */
607
608 if( ia <= -n || ia >= n ){
609 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
610 EXRETURN ;
611 }
612
613 aa = af - ia ;
614 wt_m1 = P_M1(aa) ; wt_00 = P_00(aa) ;
615 wt_p1 = P_P1(aa) ; wt_p2 = P_P2(aa) ;
616
617 if( n > nlcbuf ){
618 if( lcbuf != NULL ) free(lcbuf) ;
619 lcbuf = (float *) malloc( sizeof(float) * n ) ;
620 nlcbuf = n ;
621 }
622
623 #ifdef SEPARATE_FINS
624 ibot = 1-ia ; if( ibot < 0 ) ibot = 0 ;
625 itop = n-3-ia ; if( itop > n-1 ) itop = n-1 ;
626
627 for( ii=ibot ; ii <= itop ; ii++ ){
628 ix = ii + ia ;
629 lcbuf[ii] = wt_m1 * f[ix-1] + wt_00 * f[ix]
630 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] ;
631 }
632
633 if( ibot > n ) ibot = n ; /* 15 Mar 2001 */
634 for( ii=0 ; ii < ibot ; ii++ ){
635 ix = ii + ia ;
636 lcbuf[ii] = wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
637 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) ;
638 }
639
640 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
641 for( ii=itop+1 ; ii < n ; ii++ ){
642 ix = ii + ia ;
643 lcbuf[ii] = wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
644 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) ;
645 }
646 #else /* not SEPARATE_FINS */
647 for( ii=0 ; ii < n ; ii++ ){
648 ix = ii + ia ;
649 if( ix > 0 && ix < n-2 )
650 lcbuf[ii] = wt_m1 * f[ix-1] + wt_00 * f[ix]
651 + wt_p1 * f[ix+1] + wt_p2 * f[ix+2] ;
652 else
653 lcbuf[ii] = wt_m1 * FINS(ix-1) + wt_00 * FINS(ix)
654 + wt_p1 * FINS(ix+1) + wt_p2 * FINS(ix+2) ;
655 }
656 #endif /* SEPARATE_FINS */
657
658 memcpy( f , lcbuf , sizeof(float)*n ) ;
659 EXRETURN ;
660 }
661
cub_shift2(int n,int nup,float af,float * f,float ag,float * g)662 void cub_shift2( int n, int nup, float af, float * f, float ag, float * g )
663 {
664 cub_shift( n , af , f ) ;
665 if( g != NULL ) cub_shift( n , ag , g ) ;
666 return ;
667 }
668
669 /*---------------------------------------------------------------------------
670 Shift one row with linear interpolation
671 -----------------------------------------------------------------------------*/
672
lin_shift(int n,float af,float * f)673 void lin_shift( int n , float af , float * f )
674 {
675 int ii , ia , ix ;
676 float wt_00 , wt_p1 , aa ;
677 #ifdef SEPARATE_FINS
678 int ibot,itop ;
679 #endif
680
681 ENTRY("lin_shift") ;
682
683 af = -af ; ia = (int) af ; if( af < 0 ) ia-- ; /* ia = floor */
684 aa = af - ia ;
685 wt_00 = 1.0 - aa ; wt_p1 = aa ; /* linear interpolation weights */
686
687 /* 15 Mar 2001: if shift is too large, return all zeros */
688
689 if( ia <= -n || ia >= n ){
690 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
691 EXRETURN ;
692 }
693
694 if( n > nlcbuf ){
695 if( lcbuf != NULL ) free(lcbuf) ;
696 lcbuf = (float *) malloc( sizeof(float) * n ) ;
697 nlcbuf = n ;
698 }
699
700 #ifdef SEPARATE_FINS
701 ibot = -ia ; if( ibot < 0 ) ibot = 0 ;
702 itop = n-2-ia ; if( itop > n-1 ) itop = n-1 ;
703
704 #if 0
705 if(PRINT_TRACING){
706 char str[256]; sprintf(str,"n=%d ia=%d ibot=%d itop=%d",n,ia,ibot,itop); STATUS(str);
707 }
708 #endif
709
710 for( ii=ibot ; ii <= itop ; ii++ ){
711 ix = ii + ia ;
712 lcbuf[ii] = wt_00 * f[ix] + wt_p1 * f[ix+1] ;
713 }
714
715 if( ibot > n ) ibot = n ; /* 15 Mar 2001 */
716 for( ii=0 ; ii < ibot ; ii++ ){
717 ix = ii + ia ;
718 lcbuf[ii] = wt_00 * FINS(ix) + wt_p1 * FINS(ix+1) ;
719 }
720
721 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
722 for( ii=itop+1 ; ii < n ; ii++ ){
723 ix = ii + ia ;
724 lcbuf[ii] = wt_00 * FINS(ix) + wt_p1 * FINS(ix+1) ;
725 }
726 #else
727 for( ii=0 ; ii < n ; ii++ ){
728 ix = ii + ia ;
729 if( ix >= 0 && ix < n-1 )
730 lcbuf[ii] = wt_00 * f[ix] + wt_p1 * f[ix+1] ;
731 else
732 lcbuf[ii] = wt_00 * FINS(ix) + wt_p1 * FINS(ix+1) ;
733 }
734 #endif /* SEPARATE_FINS */
735
736 memcpy( f , lcbuf , sizeof(float)*n ) ;
737 EXRETURN ;
738 }
739
lin_shift2(int n,int nup,float af,float * f,float ag,float * g)740 void lin_shift2( int n, int nup, float af, float * f, float ag, float * g )
741 {
742 lin_shift( n , af , f ) ;
743 if( g != NULL ) lin_shift( n , ag , g ) ;
744 return ;
745 }
746
747 /*--------------------------------------------------------------------------
748 This is for experimental purposes only -- DO NOT USE!
749 ----------------------------------------------------------------------------*/
750
nn_shift(int n,float af,float * f)751 void nn_shift( int n , float af , float * f )
752 {
753 int ii , ia , ix ;
754
755 ENTRY("nn_shift") ;
756
757 af = -af ; ia = (int) af ; if( af < 0 ) ia-- ; /* ia = floor */
758
759 /* 15 Mar 2001: if shift is too large, return all zeros */
760
761 if( ia <= -n || ia >= n ){
762 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
763 EXRETURN ;
764 }
765
766 if( n > nlcbuf ){
767 if( lcbuf != NULL ) free(lcbuf) ;
768 lcbuf = (float *) malloc( sizeof(float) * n ) ;
769 nlcbuf = n ;
770 }
771
772 for( ii=0 ; ii < n ; ii++ ){
773 ix = ii + ia ;
774 lcbuf[ii] = FINS(ix) ;
775 }
776
777 memcpy( f , lcbuf , sizeof(float)*n ) ;
778 EXRETURN ;
779 }
780
nn_shift2(int n,int nup,float af,float * f,float ag,float * g)781 void nn_shift2( int n, int nup, float af, float * f, float ag, float * g )
782 {
783 nn_shift( n , af , f ) ;
784 if( g != NULL ) nn_shift( n , ag , g ) ;
785 return ;
786 }
787
788 /*---------------------------------------------------------------------------
789 More experiments: two-step interpolation
790 -----------------------------------------------------------------------------*/
791
ts_shift(int n,float af,float * f)792 void ts_shift( int n , float af , float * f )
793 {
794 register int ii , ia , ix ;
795 float aa ;
796 int ibot,itop ;
797
798 af = -af ; ia = (int) af ; if( af < 0 ) ia-- ; /* ia = floor */
799
800 /* 15 Mar 2001: if shift is too large, return all zeros */
801
802 if( ia <= -n || ia >= n ){
803 for( ii=0 ; ii < n ; ii++ ) f[ii] = 0.0 ;
804 EXRETURN ;
805 }
806
807 aa = af - ia ;
808
809 if( n > nlcbuf ){
810 if( lcbuf != NULL ) free(lcbuf) ;
811 lcbuf = (float *) malloc( sizeof(float) * n ) ;
812 nlcbuf = n ;
813 }
814
815 ibot = -ia ; if( ibot < 0 ) ibot = 0 ;
816 itop = n-2-ia ; if( itop > n-1 ) itop = n-1 ;
817
818 if( aa < 0.30 ){
819 memcpy( lcbuf+ibot, f+(ibot+ia) , (itop+1-ibot)*sizeof(float) );
820 for( ii=0 ; ii < ibot ; ii++ ){
821 ix = ii + ia ; lcbuf[ii] = FINS(ix) ;
822 }
823 for( ii=itop+1 ; ii < n ; ii++ ){
824 ix = ii + ia ; lcbuf[ii] = FINS(ix) ;
825 }
826 }
827
828 else if( aa > 0.70 ){
829 memcpy( lcbuf+ibot, f+(ibot+1+ia), (itop+1-ibot)*sizeof(float) );
830 for( ii=0 ; ii < ibot ; ii++ ){
831 ix = ii + ia ; lcbuf[ii] = FINS(ix+1) ;
832 }
833 for( ii=itop+1 ; ii < n ; ii++ ){
834 ix = ii + ia ; lcbuf[ii] = FINS(ix+1) ;
835 }
836
837 } else {
838 for( ii=ibot ; ii <= itop ; ii++ ){
839 ix = ii + ia ; lcbuf[ii] = 0.5*( f[ix] + f[ix+1] ) ;
840 }
841 if( ibot > n ) ibot = n ; /* 15 Mar 2001 */
842 for( ii=0 ; ii < ibot ; ii++ ){
843 ix = ii + ia ; lcbuf[ii] = 0.5*( FINS(ix) + FINS(ix+1) ) ;
844 }
845 if( itop < 0 ) itop = -1 ; /* 15 Mar 2001 */
846 for( ii=itop+1 ; ii < n ; ii++ ){
847 ix = ii + ia ; lcbuf[ii] = 0.5*( FINS(ix) + FINS(ix+1) ) ;
848 }
849 }
850 memcpy( f , lcbuf , sizeof(float)*n ) ;
851 return ;
852 }
853
ts_shift2(int n,int nup,float af,float * f,float ag,float * g)854 void ts_shift2( int n, int nup, float af, float * f, float ag, float * g )
855 {
856 ts_shift( n , af , f ) ;
857 if( g != NULL ) ts_shift( n , ag , g ) ;
858 return ;
859 }
860