1 //fibe1_f.h
2
3 //Skalarna (float) verzija
4
5 // MC maj 2012
6
7
8 #define EDGEAVG 8
9
10 double PI=3.14159265358979;
11
12 //---------------------------------------------------------
13 //koeficienti za biquad lowpass iz f in q
14 // f v Nyquistih 0.0 < f < 0.5
calcab_lp1(float f,float q,float * a0,float * a1,float * a2,float * b0,float * b1,float * b2)15 void calcab_lp1(float f, float q, float *a0, float *a1, float *a2, float *b0, float *b1, float *b2)
16 {
17 float a,b;
18
19 a=sinf(PI*f)/2.0/q;
20 b=cosf(PI*f);
21 *b0=(1.0-b)/2.0;
22 *b1=1.0-b;
23 *b2=(1.0-b)/2.0;
24 *a0=1.0+a;
25 *a1=-2.0*b;
26 *a2=1.0-a;
27 //printf("a=%9.6f %9.6f %9.6f b=%9.6f %9.6f %9.6f\n",*a0,*a1,*a2,*b0,*b1,*b2);
28 }
29
30 //---------------------------------------------------
31 //kompenzacija na desni
32 //c=0.0 "odziv na zacetno stanje" (zunaj crno)
33 //gain ni kompenziran
rep(float v1,float v2,float c,float * i1,float * i2,int n,float a1,float a2)34 void rep(float v1, float v2, float c, float *i1, float *i2, int n, float a1, float a2)
35 {
36 int i;
37 float lb[8192];
38
39 lb[0]=v1;lb[1]=v2;
40 for (i=2;i<n-2;i++)
41 {
42 lb[i]=c-a1*lb[i-1]-a2*lb[i-2];
43 }
44
45 lb[n-2]=0.0;lb[n-1]=0.0;
46 for (i=n-3;i>=0;i--)
47 {
48 lb[i]=lb[i]-a1*lb[i+1]-a2*lb[i+2];
49 }
50
51 *i1=lb[0]; *i2=lb[1];
52 }
53
54 //-------------------------------------------------------
55 // 2-tap IIR v stirih smereh a only verzija, a0=1.0
56 //desno kompenzacijo izracuna direktno (rdx,rsx,rcx)
57 //optimized for speed
58
59 // rep za navzgor racuna iz ze procesiranih
60 // (fibe-2 ga racuna iz deviskih)
61
fibe2o_f(float s[],int w,int h,float a1,float a2,float rd1,float rd2,float rs1,float rs2,float rc1,float rc2,int ec)62 void fibe2o_f(float s[], int w, int h, float a1, float a2, float rd1, float rd2, float rs1, float rs2, float rc1, float rc2, int ec)
63 {
64 float cr,g,g4,avg,gavg,avgg,iavg;
65 float rep1,rep2;
66 int i,j;
67 int jw,jww,h1w,h2w,iw,i1w,i2w;
68
69 g=1.0/(1.0+a1+a2);
70 g4=1.0/g/g/g/g;
71 avg=EDGEAVG; //koliko vzorcev za povprecje pri edge comp
72 gavg=g4/avg;
73 avgg=1.0/g/avg;
74 iavg=1.0/avg;
75
76 for (j=0;j<avg;j++) //prvih avg vrstic tja in nazaj
77 {
78 jw=j*w; jww=jw+w;
79 cr=0.0;
80 if (ec!=0)
81 { //edge comp (popvprecje prvih)
82 for (i=0;i<avg;i++)
83 {
84 cr=cr+s[jw+i];
85 }
86 cr=cr*gavg;
87 }
88 s[jw]=g4*s[jw]-(a1+a2)*g*cr;
89 s[jw+1]=g4*s[jw+1]-a1*s[jw]-a2*g*cr;
90
91 if (ec!=0)
92 { //edge comp za nazaj
93 cr=0.0;
94 for (i=w-avg;i<w;i++)
95 {
96 cr=cr+s[jw+i];
97 }
98 cr=cr*gavg;
99 }
100
101 for (i=2;i<w;i++) //tja
102 {
103 s[jw+i]=g4*s[jw+i]-a1*s[jw+i-1]-a2*s[jw+i-2];
104 }
105
106 rep1=(s[jww-1]+s[jww-2])*0.5*rs1+(s[jww-1]-s[jww-2])*rd1;
107 rep2=(s[jww-1]+s[jww-2])*0.5*rs2+(s[jww-1]-s[jww-2])*rd2;
108
109 if (ec!=0)
110 {
111 rep1=rep1+rc1*cr;
112 rep2=rep2+rc2*cr;
113 }
114
115 s[jww-1]=s[jww-1]-a1*rep1-a2*rep2;
116 s[jww-2]=s[jww-2]-a1*s[jww-1]-a2*rep1;
117
118 for (i=w-3;i>=0;i--) //nazaj
119 {
120 s[jw+i]=s[jw+i]-a1*s[jw+i+1]-a2*s[jw+i+2];
121 }
122 } //prvih avg vrstic
123
124 //printaj(s,w,h,1,1,0);
125
126 //edge comp zgoraj za navzdol
127 for (j=0;j<w;j++) //po stolpcih
128 {
129 cr=0.0;
130 if (ec!=0)
131 { //edge comp (popvprecje prvih)
132 for (i=0;i<avg;i++)
133 {
134 cr=cr+s[j+w*i];
135 }
136 cr=cr*iavg;
137 }
138
139 //zgornji vrstici
140 s[j]=s[j]-(a1+a2)*g*cr;
141 s[j+w]=s[j+w]-a1*s[j]-a2*g*cr;
142 }
143
144 //tretja do avg, samo navzdol (nazaj so ze)
145 for (i=2;i<avg;i++)
146 {
147 iw=i*w; i1w=iw-w;
148 for (j=0;j<w;j++) //po stolpcih
149 {
150 s[j+iw]=s[j+iw]-a1*s[j+i1w]-a2*s[j+i1w-w];
151 }
152 }
153
154 for (j=avg;j<h;j++) //po vrsticah tja, nazaj in dol
155 {
156 jw=j*w; jww=jw+w;
157 cr=0.0;
158 if (ec!=0)
159 { //edge comp (popvprecje prvih)
160 for (i=0;i<avg;i++)
161 {
162 cr=cr+s[jw+i];
163 }
164 cr=cr*gavg;
165 }
166 s[jw]=g4*s[jw]-(a1+a2)*g*cr;
167 s[jw+1]=g4*s[jw+1]-a1*s[jw]-a2*g*cr;
168
169 if (ec!=0)
170 { //edge comp za nazaj
171 cr=0.0;
172 for (i=w-avg;i<w;i++)
173 {
174 cr=cr+s[jw+i];
175 }
176 cr=cr*gavg;
177 }
178
179 for (i=2;i<w;i++) //tja
180 {
181 s[jw+i]=g4*s[jw+i]-a1*s[jw+i-1]-a2*s[jw+i-2];
182 }
183
184 rep1=(s[jww-1]+s[jww-2])*0.5*rs1+(s[jww-1]-s[jww-2])*rd1;
185 rep2=(s[jww-1]+s[jww-2])*0.5*rs2+(s[jww-1]-s[jww-2])*rd2;
186
187 if (ec!=0)
188 {
189 rep1=rep1+rc1*cr;
190 rep2=rep2+rc2*cr;
191 }
192
193 s[jww-1]=s[jww-1]-a1*rep1-a2*rep2;
194 s[jww-2]=s[jww-2]-a1*s[jww-1]-a2*rep1;
195
196 for (i=w-3;i>=0;i--) //po stolpcih
197 { //nazaj
198 s[jw+i]=s[jw+i]-a1*s[jw+i+1]-a2*s[jw+i+2];
199 //dol
200 s[jw+i+2]=s[jw+i+2]-a1*s[jw-w+i+2]-a2*s[jw-w-w+i+2];
201 }
202
203 //se leva stolpca dol
204 s[jw+1]=s[jw+1]-a1*s[jw-w+1]-a2*s[jw-w-w+1];
205 s[jw]=s[jw]-a1*s[jw-w]-a2*s[jw-w-w];
206
207 } //po vrsticah
208
209 //printaj(s,w,h,1,1,0);
210 //printf("\n\n");
211 //printaj(s,w,h,100,100,0);
212
213 //pa se navzgor
214 //spodnji dve vrstici
215 h1w=(h-1)*w; h2w=(h-2)*w;
216 for (j=0;j<w;j++) //po stolpcih
217 {
218 if (ec!=0)
219 { //edge comp za gor
220 cr=0.0;
221 for (i=h-avg;i<h;i++)
222 {
223 cr=cr+s[j+w*i];
224 }
225 cr=cr*avgg;
226 }
227
228 rep1=(s[j+h1w]+s[j+h2w])*0.5*rs1+(s[j+h1w]-s[j+h2w])*rd1;
229 rep2=(s[j+h1w]+s[j+h2w])*0.5*rs2+(s[j+h1w]-s[j+h2w])*rd2;
230
231 if (ec!=0)
232 { //edge comp
233 rep1=rep1+rc1*cr;
234 rep2=rep2+rc2*cr;
235 }
236
237 s[j+h1w]=s[j+h1w]-a1*rep1-a2*rep2;
238 s[j+h2w]=s[j+h2w]-a1*s[j+h1w]-a2*rep1;
239 }
240
241 //ostale vrstice
242 for (i=h-3;i>=0;i--) //gor
243 {
244 iw=i*w; i1w=iw+w; i2w=i1w+w;
245 for (j=0;j<w;j++)
246 {
247 s[j+iw]=s[j+iw]-a1*s[j+i1w]-a2*s[j+i2w];
248 }
249 }
250
251 //printaj(s,w,h,1,1,1);
252 //printf("\n\n");
253 //printaj(s,w,h,100,100,1);
254 //printaj(s,w,h,300,300,0);
255 //printaj(s,w,h,300,574,0);
256
257 }
258