1 #include "mrilib.h"
2
3 /* cubic interpolation polynomials */
4
5 #undef P_M1
6 #undef P_00
7 #undef P_P1
8 #undef P_P2
9
10 #define P_M1(x) ((x)*(1.0-(x))*((x)-2.0)*0.1666667)
11 #define P_00(x) (((x)+1.0)*((x)-1.0)*((x)-2.0)*0.5)
12 #define P_P1(x) ((x)*((x)+1.0)*(2.0-(x))*0.5)
13 #define P_P2(x) ((x)*((x)+1.0)*((x)-1.0)*0.1666667)
14
15 /*------------------------------------*/
16 /*! Cubic interpolation in a floatvec */
17
interp_floatvec(floatvec * fv,float x)18 float interp_floatvec( floatvec *fv , float x )
19 {
20 int ix , im1,ip1,ip2 , itop ;
21 float fx , val , abot,atop ;
22
23 if( fv == NULL || fv->ar == NULL ) return 0.0f ;
24 itop = fv->nar - 1 ;
25
26 if( itop <= 1 || fv->dx == 0.0 ) return(fv->ar[0]) ;
27
28 /* if input x is out of range, return the edge value */
29
30 fx = (x - fv->x0) / fv->dx ;
31 if( fx <= 0.0f ) return(fv->ar[0]) ;
32 else if( fx >= itop ) return(fv->ar[itop]) ;
33
34 /* input x is between point #ix and #ix+1 */
35 /* fractional offset between them is fx */
36
37 ix = (int)fx ; fx = fx - ix ;
38
39 /* get indexes below (im1) and above (ip1 and ip2) */
40
41 im1 = ix-1 ; if( im1 < 0 ) im1 = 0 ;
42 ip1 = ix+1 ;
43 if( ip1 > itop ){
44 ip1 = ip2 = itop ;
45 } else {
46 ip2 = ip1+1 ; if( ip2 > itop ) ip2 = itop ;
47 }
48
49 /* cubic interpolation between these 4 points */
50
51 val = P_M1(fx)*fv->ar[im1] + P_00(fx)*fv->ar[ix]
52 + P_P1(fx)*fv->ar[ip1] + P_P2(fx)*fv->ar[ip2] ;
53
54 /* make sure result lies in the local range of values */
55
56 abot = fv->ar[ix] ; atop = fv->ar[ip1] ;
57 if( abot > atop ){ fx = abot; abot = atop; atop = fx; }
58
59 if( val < abot ) val = abot; else if( val > atop ) val = atop;
60
61 return(val) ;
62 }
63
64 /*----------------------------------------------------------*/
65
regula_falsi_step(floatvec * fv,float y,float x0,float x1)66 static float regula_falsi_step( floatvec *fv, float y, float x0, float x1 )
67 {
68 float y0 , y1 , dy ;
69
70 y0 = interp_floatvec(fv,x0) ;
71 y1 = interp_floatvec(fv,x1) ; dy = y1-y0 ;
72 if( dy == 0.0f || fabsf(dy) < 0.00666f*(fabsf(y-y0)+fabsf(y-y1)) ) return x0;
73
74 dy = x0 + (x1-x0)/dy * (y-y0) ; return dy ;
75 }
76
77 /*----------------------------------------------------------*/
78 /* Inverse interpolation in a floatvec (assumed monotonic). */
79
80 #undef DO_FIVE
81
interp_inverse_floatvec(floatvec * fv,float y)82 float interp_inverse_floatvec( floatvec *fv , float y )
83 {
84 int ip,itop ; float ym,yp,dx , x0,x1,x2 , xm,xp,y0,y1,y2 , xx[5],yy[5] ;
85
86 /* check for stoopid inputs */
87
88 if( fv == NULL ) return 0.0f ;
89 itop = fv->nar - 1 ;
90 if( fv->ar == NULL || itop <= 1 || fv->dx == 0.0 ) return(fv->x0) ;
91
92 /* off the left edge? */
93
94 if( (fv->ar[0] < fv->ar[itop] && y <= fv->ar[0]) ||
95 (fv->ar[0] > fv->ar[itop] && y >= fv->ar[0]) ) return(fv->x0) ;
96
97 /* off the right edge? */
98
99 if( (fv->ar[0] < fv->ar[itop] && y >= fv->ar[itop]) ||
100 (fv->ar[0] > fv->ar[itop] && y <= fv->ar[itop]) ) return(fv->x0+fv->dx*itop) ;
101
102 /* find the intermediate point that brackets the desired result */
103 /* [27 Feb 2014] -- replace simple linear interpolation with
104 linear interpolation plus a regula falsi step for improvement
105 (since the forward interpolation method is cubic, not linear). */
106
107 for( ip=1 ; ip <= itop ; ip++ ){
108 ym = fv->ar[ip-1] ; yp = fv->ar[ip] ;
109 if( (y-ym) * (y-yp) <= 0.0f ){ /* the desired y is now bracketed */
110 dx = (y-ym) / (yp-ym) ;
111 #if 0
112 return( fv->x0 + fv->dx *(ip-1.0+dx) ) ; /* old way = linear interp */
113 #else
114 /* x0 = linear interp result */
115 x0 = fv->x0 + fv->dx *(ip-1.0+dx) ; y0 = interp_floatvec(fv,x0) ;
116 x1 = x0 + 0.05f * fv->dx ; /* try nearby points above and below */
117 x2 = x0 - 0.05f * fv->dx ; /* and then regula falsi from them */
118 xp = regula_falsi_step(fv,y,x0,x1) ; yp = interp_floatvec(fv,xp) ;
119 xm = regula_falsi_step(fv,y,x0,x2) ; ym = interp_floatvec(fv,xm) ;
120 #ifdef DO_FIVE /* extra regula falsi steps (not needed IMHO) */
121 x1 = regula_falsi_step(fv,y,x0,xp) ; y1 = interp_floatvec(fv,x1) ;
122 x2 = regula_falsi_step(fv,y,x0,xm) ; y2 = interp_floatvec(fv,x2) ;
123 yp = fabsf(yp-y) ; ym = fabsf(ym-y) ; y0 = fabsf(y0-y) ;
124 y1 = fabsf(y1-y) ; y2 = fabsf(y2-y) ;
125 xx[0] = x0 ; xx[1] = x1 ; xx[2] = x2 ; xx[3] = xm ; xx[4] = xp ;
126 yy[0] = y0 ; yy[1] = y1 ; yy[2] = y2 ; yy[3] = ym ; yy[4] = yp ;
127 qsort_floatfloat(5,yy,xx) ;
128 #else /* not DO_FIVE */
129 yp = fabsf(yp-y) ; ym = fabsf(ym-y) ; y0 = fabsf(y0-y) ;
130 xx[0] = x0 ; xx[1] = xm ; xx[2] = xp ; /* candidate x values */
131 yy[0] = y0 ; yy[1] = ym ; yy[2] = yp ; /* residuals at them */
132 qsort_floatfloat(3,yy,xx) ; /* find smallest residual */
133 #endif /* DO_FIVE */
134 return xx[0] ; /* x value with the smallest residual */
135 #endif
136 }
137 }
138
139 /* should never happen */
140
141 return( fv->x0 + fv->dx * 0.5*itop ) ; /* the midpoint */
142 }
143