1 // Copyright (c) <2012> <Leif Asbrink>
2 //
3 // Permission is hereby granted, free of charge, to any person
4 // obtaining a copy of this software and associated documentation
5 // files (the "Software"), to deal in the Software without restriction,
6 // including without limitation the rights to use, copy, modify,
7 // merge, publish, distribute, sublicense, and/or sell copies of
8 // the Software, and to permit persons to whom the Software is
9 // furnished to do so, subject to the following conditions:
10 //
11 // The above copyright notice and this permission notice shall be
12 // included in all copies or substantial portions of the Software.
13 //
14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
15 // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
16 // OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
17 // NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
18 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
19 // WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE
21 // OR OTHER DEALINGS IN THE SOFTWARE.
22
23
24 #include "globdef.h"
25 #include "uidef.h"
26 #include "llsqdef.h"
27
mask_tophat_filter2(float * xin,float * xout,int len,int pa,int pb,int size)28 void mask_tophat_filter2(float *xin,float *xout,int len, int pa,int pb,int size)
29 {
30 int i, k, ia, ib, ic;
31 int mask;
32 float t1,t2;
33 mask=size-1;
34 t1=0;
35 t2=0;
36 ic=pa;
37 for(i=0; i<len; i++)
38 {
39 t1+=xin[2*ic];
40 t2+=xin[2*ic+1];
41 ic=(ic+1)&mask;
42 }
43 k=len/2;
44 ib=pa;
45 for(i=0; i<k; i++)
46 {
47 xout[2*ib ]=t1/len;
48 xout[2*ib+1]=t2/len;
49 ib=(ib+1)&mask;
50 }
51 ia=pa;
52 while(ic != pb)
53 {
54 xout[2*ib ]=t1/len;
55 xout[2*ib+1]=t2/len;
56 t1+=xin[2*ic ];
57 t2+=xin[2*ic+1];
58 ic=(ic+1)&mask;
59 t1-=xin[2*ia ];
60 t2-=xin[2*ia+1];
61 ib=(ib+1)&mask;
62 ia=(ia+1)&mask;
63 }
64 while(ib != pb)
65 {
66 xout[2*ib ]=t1/len;
67 xout[2*ib+1]=t2/len;
68 ib=(ib+1)&mask;
69 }
70 }
71
mask_tophat_filter1(float * xin,float * xout,int len,int pa,int pb,int size)72 void mask_tophat_filter1(float *xin,float *xout,int len, int pa,int pb,int size)
73 {
74 int i, k, ia, ib, ic;
75 int mask;
76 float t1;
77 mask=size-1;
78 t1=0;
79 ic=pa;
80 for(i=0; i<len; i++)
81 {
82 t1+=xin[ic];
83 ic=(ic+1)&mask;
84 }
85 k=len/2;
86 ib=pa;
87 for(i=0; i<k; i++)
88 {
89 xout[ib ]=t1/len;
90 ib=(ib+1)&mask;
91 }
92 ia=pa;
93 while(ic != pb)
94 {
95 xout[ib ]=t1/len;
96 t1+=xin[ic ];
97 ic=(ic+1)&mask;
98 t1-=xin[ia ];
99 ib=(ib+1)&mask;
100 ia=(ia+1)&mask;
101 }
102 while(ib != pb)
103 {
104 xout[ib ]=t1/len;
105 ib=(ib+1)&mask;
106 }
107 }
108
109
110
111
parabolic_fit(float * amp,float * pos,float yy1,float yy2,float yy3)112 void parabolic_fit(float *amp, float *pos, float yy1, float yy2, float yy3)
113 {
114 float t3,t4;
115 // ***********************************
116 // Fit a parabola to the max point and it's nearest neighbours.
117 // As input we have 3 data points y(x)
118 // Find parameters a,b and c to place these points on the curve:
119 // y(x)=a+b*(x-c)**2=a+b*x**2-2*x*b*c+b*c**2
120 // The equations to solve are:
121 // y(-1)=a + b + 2*b*c + b*c**2
122 // y( 0)=a + b*c**2
123 // y( 1)=a + b - 2*b*c + b*c**2
124 // b has to be negative (for a positive peak)
125 // abs(c) has to be below 0.5 since y(0)is the largest value.
126 // t4=y(-1)-y(1)=4*b*c
127 // t3=2*(y(-1)+y(1)-2*y(0))=4*b
128 // t4/t3=c=peak offset
129 // ***********************************
130
131 t4=yy1-yy3;
132 t3=2*(yy1+yy3-2*yy2);
133 if(t3 < 0)
134 {
135 amp[0]=yy2-0.5*t4*t4/t3;
136 t4=t4/t3;
137 if(fabs(t4) > 1)t4/=fabs(t4);
138 pos[0]=t4;
139 }
140 else
141 {
142 if(yy1 > yy3)
143 {
144 amp[0]=yy1;
145 pos[0]=-1;
146 }
147 else
148 {
149 amp[0]=yy3;
150 pos[0]=1;
151 }
152 }
153 }
154
155
156
157
158
159
llsq1(void)160 int llsq1(void)
161 {
162 float aux[2*LLSQ_MAXPAR];
163 int ipiv[2*LLSQ_MAXPAR];
164 int kpiv;
165 int i,j,k;
166 float t1,sig,piv,beta;
167 if(llsq_npar > LLSQ_MAXPAR)
168 {
169 lirerr(1051);
170 return -1;
171 }
172 kpiv=piv=0;
173 for(k=0; k<llsq_npar; k++)
174 {
175 ipiv[k]=k;
176 t1=0;
177 for(i=0; i<llsq_neq; i++)
178 {
179 t1+=pow(llsq_derivatives[k*llsq_neq+i],2.0);
180 }
181 aux[k]=t1;
182 if(t1 > piv)
183 {
184 piv=t1;
185 kpiv=k;
186 }
187 }
188 if(piv == 0)return -1;
189 sig=sqrt(piv);
190 for(k=0; k<llsq_npar; k++)
191 {
192 if(kpiv>k)
193 {
194 t1=aux[k];
195 aux[k]=aux[kpiv];
196 aux[kpiv]=t1;
197 for(i=k; i<llsq_neq; i++)
198 {
199 t1=llsq_derivatives[k*llsq_neq+i];
200 llsq_derivatives[k*llsq_neq+i]=llsq_derivatives[kpiv*llsq_neq+i];
201 llsq_derivatives[kpiv*llsq_neq+i]=t1;
202 }
203 }
204 if(k > 0)
205 {
206 sig=0;
207 for(i=k; i<llsq_neq; i++)
208 {
209 sig+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[k*llsq_neq+i];
210 }
211 sig=sqrt(sig);
212 }
213 t1=llsq_derivatives[k*llsq_neq+k];
214 if(t1 < 0 )sig=-sig;
215 ipiv[kpiv]=ipiv[k];
216 ipiv[k]=kpiv;
217 beta=t1+sig;
218 llsq_derivatives[k*llsq_neq+k]=beta;
219 beta=1/(sig*beta);
220 j=llsq_npar+k;
221 aux[j]=-sig;
222 if(k<llsq_npar-1)
223 {
224 piv=0;
225 kpiv=k+1;
226 for(j=k+1; j<llsq_npar; j++)
227 {
228 t1=0;
229 for(i=k; i<llsq_neq; i++)
230 {
231 t1+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[j*llsq_neq+i];
232 }
233 t1=beta*t1;
234 for(i=k; i<llsq_neq; i++)
235 {
236 llsq_derivatives[j*llsq_neq+i]-=llsq_derivatives[k*llsq_neq+i]*t1;
237 }
238 t1=aux[j]-llsq_derivatives[j*llsq_neq+k]*llsq_derivatives[j*llsq_neq+k];
239 aux[j]=t1;
240 if(t1 > piv)
241 {
242 piv=t1;
243 kpiv=j;
244 }
245 }
246 }
247 t1=0;
248 for(i=k; i<llsq_neq; i++)
249 {
250 t1+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[i];
251 }
252 t1*=beta;
253 for(i=k; i<llsq_neq; i++)
254 {
255 llsq_errors[i]-=llsq_derivatives[k*llsq_neq+i]*t1;
256 }
257 }
258 llsq_steps[llsq_npar-1]=llsq_errors[llsq_npar-1]/aux[2*llsq_npar-1];
259 if(llsq_npar == 1)return 0;
260 for(k=llsq_npar-2; k>=0; k--)
261 {
262 t1=llsq_errors[k ];
263 for(i=k+1; i<llsq_npar; i++)
264 {
265 t1-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[i];
266 }
267 t1/=aux[llsq_npar+k];
268 llsq_steps[k]=llsq_steps[ipiv[k]];
269 llsq_steps[ipiv[k] ]=t1;
270 }
271 return 0;
272 }
273
llsq2(void)274 int llsq2(void)
275 {
276 float aux[2*LLSQ_MAXPAR];
277 int ipiv[2*LLSQ_MAXPAR];
278 int kpiv;
279 int i,j,k;
280 float t1,t2,sig,piv,beta;
281 if(llsq_npar > LLSQ_MAXPAR)
282 {
283 lirerr(1051);
284 return -1;
285 }
286 kpiv=piv=0;
287 for(k=0; k<llsq_npar; k++)
288 {
289 ipiv[k]=k;
290 t1=0;
291 for(i=0; i<llsq_neq; i++)
292 {
293 t1+=pow(llsq_derivatives[k*llsq_neq+i],2.0);
294 }
295 aux[k]=t1;
296 if(t1 > piv)
297 {
298 piv=t1;
299 kpiv=k;
300 }
301 }
302 if(piv == 0)return -1;
303 sig=sqrt(piv);
304 for(k=0; k<llsq_npar; k++)
305 {
306 if(kpiv>k)
307 {
308 t1=aux[k];
309 aux[k]=aux[kpiv];
310 aux[kpiv]=t1;
311 for(i=k; i<llsq_neq; i++)
312 {
313 t1=llsq_derivatives[k*llsq_neq+i];
314 llsq_derivatives[k*llsq_neq+i]=llsq_derivatives[kpiv*llsq_neq+i];
315 llsq_derivatives[kpiv*llsq_neq+i]=t1;
316 }
317 }
318 if(k > 0)
319 {
320 sig=0;
321 for(i=k; i<llsq_neq; i++)
322 {
323 sig+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[k*llsq_neq+i];
324 }
325 sig=sqrt(sig);
326 }
327 t1=llsq_derivatives[k*llsq_neq+k];
328 if(t1 < 0 )sig=-sig;
329 ipiv[kpiv]=ipiv[k];
330 ipiv[k]=kpiv;
331 beta=t1+sig;
332 llsq_derivatives[k*llsq_neq+k]=beta;
333 beta=1/(sig*beta);
334 j=llsq_npar+k;
335 aux[j]=-sig;
336 if(k<llsq_npar-1)
337 {
338 piv=0;
339 kpiv=k+1;
340 for(j=k+1; j<llsq_npar; j++)
341 {
342 t1=0;
343 for(i=k; i<llsq_neq; i++)
344 {
345 t1+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[j*llsq_neq+i];
346 }
347 t1=beta*t1;
348 for(i=k; i<llsq_neq; i++)
349 {
350 llsq_derivatives[j*llsq_neq+i]-=llsq_derivatives[k*llsq_neq+i]*t1;
351 }
352 t1=aux[j]-llsq_derivatives[j*llsq_neq+k]*llsq_derivatives[j*llsq_neq+k];
353 aux[j]=t1;
354 if(t1 > piv)
355 {
356 piv=t1;
357 kpiv=j;
358 }
359 }
360 }
361 t1=0;
362 t2=0;
363 for(i=k; i<llsq_neq; i++)
364 {
365 t1+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[2*i ];
366 t2+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[2*i+1];
367 }
368 t1*=beta;
369 t2*=beta;
370 for(i=k; i<llsq_neq; i++)
371 {
372 llsq_errors[2*i ]-=llsq_derivatives[k*llsq_neq+i]*t1;
373 llsq_errors[2*i+1]-=llsq_derivatives[k*llsq_neq+i]*t2;
374 }
375 }
376 llsq_steps[2*(llsq_npar-1) ]=
377 llsq_errors[2*(llsq_npar-1) ]/aux[2*llsq_npar-1];
378 llsq_steps[2*(llsq_npar-1)+1]=
379 llsq_errors[2*(llsq_npar-1)+1]/aux[2*llsq_npar-1];
380 if(llsq_npar == 1)return 0;
381 for(k=llsq_npar-2; k>=0; k--)
382 {
383 t1=llsq_errors[2*k ];
384 t2=llsq_errors[2*k+1];
385 for(i=k+1; i<llsq_npar; i++)
386 {
387 t1-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[2*i ];
388 t2-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[2*i+1];
389 }
390 t1/=aux[llsq_npar+k];
391 t2/=aux[llsq_npar+k];
392 llsq_steps[2*k ]=llsq_steps[2*ipiv[k] ];
393 llsq_steps[2*k+1]=llsq_steps[2*ipiv[k]+1];
394 llsq_steps[2*ipiv[k] ]=t1;
395 llsq_steps[2*ipiv[k]+1]=t2;
396 }
397 return 0;
398 }
399
400