54{
55
56}
57
58FilterSavitzkyGolay::FilterSavitzkyGolay(
59 int nL, int nR, int m, int lD, bool convolveWithNr)
60{
61 m_params.nL = nL;
62 m_params.nR = nR;
63 m_params.m = m;
64 m_params.lD = lD;
65 m_params.convolveWithNr = convolveWithNr;
66}
67
68
69FilterSavitzkyGolay::FilterSavitzkyGolay(const SavGolParams sav_gol_params)
70{
71 m_params.nL = sav_gol_params.nL;
72 m_params.nR = sav_gol_params.nR;
73 m_params.m = sav_gol_params.m;
74 m_params.lD = sav_gol_params.lD;
75 m_params.convolveWithNr = sav_gol_params.convolveWithNr;
76}
77
78
79FilterSavitzkyGolay::FilterSavitzkyGolay(const QString ¶meters)
80{
81 buildFilterFromString(parameters);
82}
83
84
85FilterSavitzkyGolay::FilterSavitzkyGolay(const FilterSavitzkyGolay &other)
86{
87
88
89 m_params.nL = other.m_params.nL;
90 m_params.nR = other.m_params.nR;
91 m_params.m = other.m_params.m;
92 m_params.lD = other.m_params.lD;
93 m_params.convolveWithNr = other.m_params.convolveWithNr;
94}
95
96
97FilterSavitzkyGolay::~FilterSavitzkyGolay()
98{
99}
100
101FilterSavitzkyGolay &
102FilterSavitzkyGolay::operator=(const FilterSavitzkyGolay &other)
103{
104 if(&other == this)
105 return *this;
106
107
108
109 m_params.nL = other.m_params.nL;
110 m_params.nR = other.m_params.nR;
111 m_params.m = other.m_params.m;
112 m_params.lD = other.m_params.lD;
113 m_params.convolveWithNr = other.m_params.convolveWithNr;
114
115 return *this;
116}
117
118
119void
120FilterSavitzkyGolay::buildFilterFromString(const QString ¶meters)
121{
122
123 if(parameters.startsWith(QString("%1|").arg(name())))
124 {
125 QStringList params = parameters.split("|").back().split(";");
126
127 m_params.nL = params.at(0).toInt();
128 m_params.nR = params.at(1).toInt();
129 m_params.m = params.at(2).toInt();
130 m_params.lD = params.at(3).toInt();
131 m_params.convolveWithNr = (params.at(4) == "true" ? true : false);
132 }
133 else
134 {
136 QString("Building of FilterSavitzkyGolay from string %1 failed")
137 .arg(parameters));
138 }
139}
140
141
142Trace &
143FilterSavitzkyGolay::filter(Trace &data_points) const
144{
145
146
147
148
149 int data_point_count = data_points.size();
150
152 pappso_double *y_initial_data_p = dvector(1, data_point_count);
154
155 if(m_params.convolveWithNr)
156 y_filtered_data_p = dvector(1, 2 * data_point_count);
157 else
158 y_filtered_data_p = dvector(1, data_point_count);
159
160 for(int iter = 0; iter < data_point_count; ++iter)
161 {
162 x_data_p[iter] = data_points.at(iter).x;
163 y_initial_data_p[iter] = data_points.at(iter).y;
164 }
165
166
167
168 runFilter(y_initial_data_p, y_filtered_data_p, data_point_count);
169
170
171 auto iter_yf = y_filtered_data_p;
172 for(auto &data_point : data_points)
173 {
174 data_point.y = *iter_yf;
175 iter_yf++;
176 }
177
178 return data_points;
179}
180
181
182SavGolParams
183FilterSavitzkyGolay::getParameters() const
184{
185 return SavGolParams(
186 m_params.nL, m_params.nR, m_params.m, m_params.lD, m_params.convolveWithNr);
187}
188
189
190
191QString
192FilterSavitzkyGolay::toString() const
193{
194 return QString("%1|%2").arg(name()).arg(m_params.toString());
195}
196
197
198QString
199FilterSavitzkyGolay::name() const
200{
201 return "Savitzky-Golay";
202}
203
204
205int *
206FilterSavitzkyGolay::ivector(long nl, long nh) const
207{
208 int *v;
209 v = (int *)malloc((size_t)((nh - nl + 2) * sizeof(int)));
210 if(!v)
211 {
212 qFatal("Error: Allocation failure.");
213 }
214 return v - nl + 1;
215}
216
217
219FilterSavitzkyGolay::dvector(long nl, long nh) const
220{
222 long k;
223 v = (
pappso_double *)malloc((
size_t)((nh - nl + 2) *
sizeof(pappso_double)));
224 if(!v)
225 {
226 qFatal("Error: Allocation failure.");
227 }
228 for(k = nl; k <= nh; k++)
229 v[k] = 0.0;
230 return v - nl + 1;
231}
232
233
235FilterSavitzkyGolay::dmatrix(long nrl, long nrh, long ncl, long nch) const
236{
237 long i, nrow = nrh - nrl + 1, ncol = nch - ncl + 1;
239 m = (
pappso_double **)malloc((
size_t)((nrow + 1) *
sizeof(pappso_double *)));
240 if(!m)
241 {
242 qFatal("Error: Allocation failure.");
243 }
244 m += 1;
245 m -= nrl;
247 (size_t)((nrow * ncol + 1) * sizeof(pappso_double)));
248 if(!m[nrl])
249 {
250 qFatal("Error: Allocation failure.");
251 }
252 m[nrl] += 1;
253 m[nrl] -= ncl;
254 for(i = nrl + 1; i <= nrh; i++)
255 m[i] = m[i - 1] + ncol;
256 return m;
257}
258
259
260void
261FilterSavitzkyGolay::free_ivector(int *v,
262 long nl,
263 [[maybe_unused]] long nh) const
264{
265 free((char *)(v + nl - 1));
266}
267
268
269void
270FilterSavitzkyGolay::free_dvector(pappso_double *v,
271 long nl,
272 [[maybe_unused]] long nh) const
273{
274 free((char *)(v + nl - 1));
275}
276
277
278void
279FilterSavitzkyGolay::free_dmatrix(pappso_double **m,
280 long nrl,
281 [[maybe_unused]] long nrh,
282 long ncl,
283 [[maybe_unused]] long nch) const
284{
285 free((char *)(m[nrl] + ncl - 1));
286 free((char *)(m + nrl - 1));
287}
288
289
290void
291FilterSavitzkyGolay::lubksb(pappso_double **a,
292 int n,
293 int *indx,
294 pappso_double b[]) const
295{
296 int i, ii = 0, ip, j;
298
299 for(i = 1; i <= n; i++)
300 {
301 ip = indx[i];
304 if(ii)
305 for(j = ii; j <= i - 1; j++)
306 sum -= a[i][j] * b[j];
307 else if(sum)
308 ii = i;
310 }
311 for(i = n; i >= 1; i--)
312 {
314 for(j = i + 1; j <= n; j++)
315 sum -= a[i][j] * b[j];
316 b[i] =
sum /
a[i][i];
317 }
318}
319
320
321void
322FilterSavitzkyGolay::ludcmp(pappso_double **a,
323 int n,
324 int *indx,
325 pappso_double *d) const
326{
327 int i, imax = 0, j, k;
330
331 vv = dvector(1, n);
332 *d = 1.0;
333 for(i = 1; i <= n; i++)
334 {
335 big = 0.0;
336 for(j = 1; j <= n; j++)
337 if((temp = fabs(a[i][j])) > big)
338 big = temp;
339 if(big == 0.0)
340 {
341 qFatal("Error: Singular matrix found in routine ludcmp().");
342 }
343 vv[i] = 1.0 / big;
344 }
345 for(j = 1; j <= n; j++)
346 {
347 for(i = 1; i < j; i++)
348 {
350 for(k = 1; k < i; k++)
351 sum -= a[i][k] * a[k][j];
353 }
354 big = 0.0;
355 for(i = j; i <= n; i++)
356 {
358 for(k = 1; k < j; k++)
359 sum -= a[i][k] * a[k][j];
361 if((dum = vv[i] * fabs(sum)) >= big)
362 {
363 big = dum;
364 imax = i;
365 }
366 }
367 if(j != imax)
368 {
369 for(k = 1; k <= n; k++)
370 {
372 a[imax][k] =
a[j][k];
374 }
375 *d = -(*d);
376 vv[imax] = vv[j];
377 }
378 indx[j] = imax;
379 if(a[j][j] == 0.0)
380 a[j][j] = std::numeric_limits<pappso_double>::epsilon();
381 if(j != n)
382 {
383 dum = 1.0 / (
a[j][j]);
384 for(i = j + 1; i <= n; i++)
385 a[i][j] *= dum;
386 }
387 }
388 free_dvector(vv, 1, n);
389}
390
391
392void
393FilterSavitzkyGolay::four1(pappso_double data[], unsigned long nn, int isign)
394{
395 unsigned long n, mmax, m, j, istep, i;
398
399 n = nn << 1;
400 j = 1;
401 for(i = 1; i < n; i += 2)
402 {
403 if(j > i)
404 {
405 SWAP(data[j], data[i]);
406 SWAP(data[j + 1], data[i + 1]);
407 }
408 m = n >> 1;
409 while(m >= 2 && j > m)
410 {
411 j -= m;
412 m >>= 1;
413 }
414 j += m;
415 }
416 mmax = 2;
417 while(n > mmax)
418 {
419 istep = mmax << 1;
420 theta = isign * (6.28318530717959 / mmax);
421 wtemp = sin(0.5 * theta);
422 wpr = -2.0 * wtemp * wtemp;
423 wpi = sin(theta);
424 wr = 1.0;
425 wi = 0.0;
426 for(m = 1; m < mmax; m += 2)
427 {
428 for(i = m; i <= n; i += istep)
429 {
430 j = i + mmax;
431 tempr = wr * data[j] - wi * data[j + 1];
432 tempi = wr * data[j + 1] + wi * data[j];
433 data[j] = data[i] - tempr;
434 data[j + 1] = data[i + 1] - tempi;
435 data[i] += tempr;
436 data[i + 1] += tempi;
437 }
438 wr = (wtemp = wr) * wpr - wi * wpi + wr;
439 wi = wi * wpr + wtemp * wpi + wi;
440 }
441 mmax = istep;
442 }
443}
444
445
446void
447FilterSavitzkyGolay::twofft(pappso_double data1[],
448 pappso_double data2[],
449 pappso_double fft1[],
450 pappso_double fft2[],
451 unsigned long n)
452{
453 unsigned long nn3, nn2, jj, j;
455
456 nn3 = 1 + (nn2 = 2 + n + n);
457 for(j = 1, jj = 2; j <= n; j++, jj += 2)
458 {
459 fft1[jj - 1] = data1[j];
460 fft1[jj] = data2[j];
461 }
462 four1(fft1, n, 1);
463 fft2[1] = fft1[2];
464 fft1[2] = fft2[2] = 0.0;
465 for(j = 3; j <= n + 1; j += 2)
466 {
467 rep = 0.5 * (fft1[j] + fft1[nn2 - j]);
468 rem = 0.5 * (fft1[j] - fft1[nn2 - j]);
469 aip = 0.5 * (fft1[j + 1] + fft1[nn3 - j]);
470 aim = 0.5 * (fft1[j + 1] - fft1[nn3 - j]);
471 fft1[j] = rep;
472 fft1[j + 1] = aim;
473 fft1[nn2 - j] = rep;
474 fft1[nn3 - j] = -aim;
475 fft2[j] = aip;
476 fft2[j + 1] = -rem;
477 fft2[nn2 - j] = aip;
478 fft2[nn3 - j] = rem;
479 }
480}
481
482
483void
484FilterSavitzkyGolay::realft(pappso_double data[], unsigned long n, int isign)
485{
486 unsigned long i, i1, i2, i3, i4, np3;
489
491 if(isign == 1)
492 {
493 c2 = -0.5;
494 four1(data, n >> 1, 1);
495 }
496 else
497 {
498 c2 = 0.5;
499 theta = -theta;
500 }
501 wtemp = sin(0.5 * theta);
502 wpr = -2.0 * wtemp * wtemp;
503 wpi = sin(theta);
504 wr = 1.0 + wpr;
505 wi = wpi;
506 np3 = n + 3;
507 for(i = 2; i <= (n >> 2); i++)
508 {
509 i4 = 1 + (i3 = np3 - (i2 = 1 + (i1 = i + i - 1)));
510 h1r = c1 * (data[i1] + data[i3]);
511 h1i = c1 * (data[i2] - data[i4]);
512 h2r = -c2 * (data[i2] + data[i4]);
513 h2i = c2 * (data[i1] - data[i3]);
514 data[i1] = h1r + wr * h2r - wi * h2i;
515 data[i2] = h1i + wr * h2i + wi * h2r;
516 data[i3] = h1r - wr * h2r + wi * h2i;
517 data[i4] = -h1i + wr * h2i + wi * h2r;
518 wr = (wtemp = wr) * wpr - wi * wpi + wr;
519 wi = wi * wpr + wtemp * wpi + wi;
520 }
521 if(isign == 1)
522 {
523 data[1] = (h1r = data[1]) + data[2];
524 data[2] = h1r - data[2];
525 }
526 else
527 {
528 data[1] = c1 * ((h1r = data[1]) + data[2]);
529 data[2] = c1 * (h1r - data[2]);
530 four1(data, n >> 1, -1);
531 }
532}
533
534
535char
536FilterSavitzkyGolay::convlv(pappso_double data[],
537 unsigned long n,
538 pappso_double respns[],
539 unsigned long m,
540 int isign,
541 pappso_double ans[])
542{
543 unsigned long i, no2;
545
546 fft = dvector(1, n << 1);
547 for(i = 1; i <= (m - 1) / 2; i++)
548 respns[n + 1 - i] = respns[m + 1 - i];
549 for(i = (m + 3) / 2; i <= n - (m - 1) / 2; i++)
550 respns[i] = 0.0;
551 twofft(data, respns, fft, ans, n);
552 no2 = n >> 1;
553 for(i = 2; i <= n + 2; i += 2)
554 {
555 if(isign == 1)
556 {
557 ans[i - 1] =
558 (fft[i - 1] * (dum = ans[i - 1]) - fft[i] * ans[i]) / no2;
559 ans[i] = (fft[i] * dum + fft[i - 1] * ans[i]) / no2;
560 }
561 else if(isign == -1)
562 {
563 if((mag2 = ans[i - 1] * ans[i - 1] + ans[i] * ans[i]) == 0.0)
564 {
565 qDebug("Attempt of deconvolving at zero response in convlv().");
566 return (1);
567 }
568 ans[i - 1] =
569 (fft[i - 1] * (dum = ans[i - 1]) + fft[i] * ans[i]) / mag2 / no2;
570 ans[i] = (fft[i] * dum - fft[i - 1] * ans[i]) / mag2 / no2;
571 }
572 else
573 {
574 qDebug("No meaning for isign in convlv().");
575 return (1);
576 }
577 }
578 ans[2] = ans[n + 1];
579 realft(ans, n, -1);
580 free_dvector(fft, 1, n << 1);
581 return (0);
582}
583
584
585char
586FilterSavitzkyGolay::sgcoeff(
587 pappso_double c[], int np, int nl, int nr, int ld, int m) const
588{
589 int imj, ipj, j, k, kk, mm, *indx;
591
592 if(np < nl + nr + 1 || nl < 0 || nr < 0 || ld > m || nl + nr < m)
593 {
594 qDebug("Inconsistent arguments detected in routine sgcoeff.");
595 return (1);
596 }
597 indx = ivector(1, m + 1);
598 a = dmatrix(1, m + 1, 1, m + 1);
599 b = dvector(1, m + 1);
600 for(ipj = 0; ipj <= (m << 1); ipj++)
601 {
602 sum = (ipj ? 0.0 : 1.0);
603 for(k = 1; k <= nr; k++)
605 for(k = 1; k <= nl; k++)
607 mm = (ipj < 2 * m - ipj ? ipj : 2 * m - ipj);
608 for(imj = -mm; imj <= mm; imj += 2)
609 a[1 + (ipj + imj) / 2][1 + (ipj - imj) / 2] = sum;
610 }
611 ludcmp(a, m + 1, indx, &d);
612 for(j = 1; j <= m + 1; j++)
613 b[j] = 0.0;
615 lubksb(a, m + 1, indx, b);
616 for(kk = 1; kk <= np; kk++)
617 c[kk] = 0.0;
618 for(k = -nl; k <= nr; k++)
619 {
621 fac = 1.0;
622 for(mm = 1; mm <= m; mm++)
623 sum += b[mm + 1] * (fac *= k);
624 kk = ((np - k) % np) + 1;
626 }
627 free_dvector(b, 1, m + 1);
628 free_dmatrix(a, 1, m + 1, 1, m + 1);
629 free_ivector(indx, 1, m + 1);
630 return (0);
631}
632
633
634
635
636
637
638
639char
640FilterSavitzkyGolay::runFilter(double *y_data_p,
641 double *y_filtered_data_p,
642 int data_point_count) const
643{
644 int np = m_params.nL + 1 + m_params.nR;
646 char retval;
647
648#if CONVOLVE_WITH_NR_CONVLV
649 c = dvector(1, data_point_count);
650 retval = sgcoeff(c, np, m_nL, m_nR, m_lD, m_m);
651 if(retval == 0)
652 convlv(y_data_p, data_point_count, c, np, 1, y_filtered_data_p);
653 free_dvector(c, 1, data_point_count);
654#else
655 int j;
656 long int k;
657 c = dvector(1, m_params.nL + m_params.nR + 1);
658 retval = sgcoeff(c, np, m_params.nL, m_params.nR, m_params.lD, m_params.m);
659 if(retval == 0)
660 {
661 qDebug() << __FILE__ << __LINE__ << __FUNCTION__ << "()"
662 << "retval is 0";
663
664 for(k = 1; k <= m_params.nL; k++)
665 {
666 for(y_filtered_data_p[k] = 0.0, j = -m_params.nL; j <= m_params.nR;
667 j++)
668 {
669 if(k + j >= 1)
670 {
671 y_filtered_data_p[k] +=
672 c[(j >= 0 ? j + 1 : m_params.nR + m_params.nL + 2 + j)] *
673 y_data_p[k + j];
674 }
675 }
676 }
677 for(k = m_params.nL + 1; k <= data_point_count - m_params.nR; k++)
678 {
679 for(y_filtered_data_p[k] = 0.0, j = -m_params.nL; j <= m_params.nR;
680 j++)
681 {
682 y_filtered_data_p[k] +=
683 c[(j >= 0 ? j + 1 : m_params.nR + m_params.nL + 2 + j)] *
684 y_data_p[k + j];
685 }
686 }
687 for(k = data_point_count - m_params.nR + 1; k <= data_point_count; k++)
688 {
689 for(y_filtered_data_p[k] = 0.0, j = -m_params.nL; j <= m_params.nR;
690 j++)
691 {
692 if(k + j <= data_point_count)
693 {
694 y_filtered_data_p[k] +=
695 c[(j >= 0 ? j + 1 : m_params.nR + m_params.nL + 2 + j)] *
696 y_data_p[k + j];
697 }
698 }
699 }
700 }
701
702 free_dvector(c, 1, m_params.nR + m_params.nL + 1);
703#endif
704
705 return (retval);
706}
707
708
709}
excetion to use when an item type is not recognized
double pappso_double
A type definition for doubles.