libs/qm-dsp/dsp/signalconditioning/FiltFilt.cpp

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2
   3 /*
   4     QM DSP Library
   5
   6     Centre for Digital Music, Queen Mary, University of London.
   7     This file 2005-2006 Christian Landone.
   8
   9     This program is free software; you can redistribute it and/or
  10     modify it under the terms of the GNU General Public License as
  11     published by the Free Software Foundation; either version 2 of the
  12     License, or (at your option) any later version.  See the file
  13     COPYING included with this distribution for more information.
  14 */
  15
  16 #include "FiltFilt.h"
  17
  18 //////////////////////////////////////////////////////////////////////
  19 // Construction/Destruction
  20 //////////////////////////////////////////////////////////////////////
  21
  22 FiltFilt::FiltFilt(Filter::Parameters parameters) :
  23     m_filter(parameters)
  24 {
  25     m_ord = m_filter.getOrder();
  26 }
  27
  28 FiltFilt::~FiltFilt()
  29 {
  30 }
  31
  32 void FiltFilt::process(double *src, double *dst, unsigned int length)
  33 {
  34     unsigned int i;
  35
  36     if (length == 0) return;
  37
  38     if (length < 2) {
  39         for( i = 0; i < length; i++ ) {
  40             dst[i] = src [i];
  41         }
  42         return;
  43     }
  44
  45     unsigned int nFilt = m_ord + 1;
  46     unsigned int nFact = 3 * ( nFilt - 1);
  47     unsigned int nExt   = length + 2 * nFact;
  48
  49     double *filtScratchIn = new double[ nExt ];
  50     double *filtScratchOut = new double[ nExt ];
  51
  52     for( i = 0; i< nExt; i++ )
  53     {
  54         filtScratchIn[ i ] = 0.0;
  55         filtScratchOut[ i ] = 0.0;
  56     }
  57
  58     // Edge transients reflection
  59     double sample0 = 2 * src[ 0 ];
  60     double sampleN = 2 * src[ length - 1 ];
  61
  62     unsigned int index = 0;
  63     for( i = nFact; i > 0; i-- )
  64     {
  65         filtScratchIn[ index++ ] = sample0 - src[ i ];
  66     }
  67     index = 0;
  68     for( i = 0; i < nFact && i + 2 < length; i++ )
  69     {
  70         filtScratchIn[ (nExt - nFact) + index++ ] = sampleN - src[ (length - 2) - i ];
  71     }
  72
  73     for(; i < nFact; i++ )
  74     {
  75         filtScratchIn[ (nExt - nFact) + index++ ] = 0;
  76     }
  77
  78     index = 0;
  79     for( i = 0; i < length; i++ )
  80     {
  81         filtScratchIn[ i + nFact ] = src[ i ];
  82     }
  83
  84     ////////////////////////////////
  85     // Do  0Ph filtering
  86     m_filter.process( filtScratchIn, filtScratchOut, nExt);
  87
  88     // reverse the series for FILTFILT
  89     for ( i = 0; i < nExt; i++)
  90     {
  91         filtScratchIn[ i ] = filtScratchOut[ nExt - i - 1];
  92     }
  93
  94     // do FILTER again
  95     m_filter.process( filtScratchIn, filtScratchOut, nExt);
  96
  97     // reverse the series back
  98     for ( i = 0; i < nExt; i++)
  99     {
 100         filtScratchIn[ i ] = filtScratchOut[ nExt - i - 1 ];
 101     }
 102     for ( i = 0;i < nExt; i++)
 103     {
 104         filtScratchOut[ i ] = filtScratchIn[ i ];
 105     }
 106
 107     index = 0;
 108     for( i = 0; i < length; i++ )
 109     {
 110         dst[ index++ ] = filtScratchOut[ i + nFact ];
 111     }
 112
 113     delete [] filtScratchIn;
 114     delete [] filtScratchOut;
 115
 116 }
 117
 118 void FiltFilt::reset()
 119 {
 120
 121 }