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