DSP.CPP Digital Signal Processing C++

///////////////////////////////////////////////////////////////
// ÎÄ ¼þ Ãû :DSP.cpp
// Îļþ¹¦ÄÜ :
// ×÷    Õß : Taliux
// ´´½¨Ê±¼ä : 2005Äê3ÔÂ21ÈÕ
// ÏîÄ¿Ãû³Æ £ºWAVTransform
// ²Ù×÷ϵͳ :
// ±¸    ×¢ :
// ÀúÊ·¼Ç¼£º :
///////////////////////////////////////////////////////////////
#include "StdAfx.h"
#include ".\dsp.h"
#ifdef _USE_LSF_
#include "poly2lsf.h"
#endif

CDSP::CDSP(void)
{
}

CDSP::~CDSP(void)
{

}


///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : AutoCorrelate
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : calculate auto-correlation coefficients
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : float
// ²ÎÊý˵Ã÷ : const vector<short>& s, wave signals
// vector<float>& r, auto-correlation functions r(j)
// int p                      steps
///////////////////////////////////////////////////////////////
float CDSP::AutoCorrelate(const vector<short>& s, vector<float>& r, int p)
{
float sum;
int   i,j;
r.clear();
r.resize(p+1);

for ( i = 0; i <= p; i++)
{
sum = 0.0;
for ( j = 1; j < s.size()-i; j++ )
sum += s[j]*s[j+i];
r[i] = sum;
}
return r[0];
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : Durbin
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : Levinson-Durbin Algorithm
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : float
// ²ÎÊý˵Ã÷ : vector<float>& k,
// vector<float>& a,
// vector<float>& r,
// float E,
// int p
///////////////////////////////////////////////////////////////
float CDSP::Durbin(vector<float>& k, vector<float>& a, vector<float>& r, float E, int p)
{
vector<float> newA(p+1);
float ki;         /* Current Reflection Coefficient */
int i,j;
k.clear();
a.clear();
k.resize(p+1);
a.resize(p+1);
a[0] = 1;
for ( i = 1; i <= p; i++ )
{
ki = r[i];              /* Calc next reflection coef */
for ( j = 1; j < i; j++ )
ki = ki + a[j] * r[i - j];
ki = ki / E;  
if ( !k.empty() )  k[i] = ki;
E *= 1 - ki*ki;         /* Update Error */
newA[i] = -ki;          /* Calc new filter coef */
for ( j = 1 ; j < i; j++ )
newA[j] = a[j] - ki * a[i - j];
for ( j = 1; j <= i; j++ )  
a[j] = newA[j];
}
// for ( i = 0; i <= p; i++ )
// a[i] = -a[i];
return E;
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : Wave2LPC
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : calculate the lpc coefficients
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<short>& s,
// vector<float>& a,
// vector<float>& k,
// int p,
// float *re,
// float *te
///////////////////////////////////////////////////////////////
void CDSP::Wave2LPC(const vector<short>& s, vector<float>& a, vector<float>& k, int p, float *re, float *te)
{
vector<float> r(p);         /* AutoCorrelation Sequence */
float E; /* Prediction Error */

E = AutoCorrelate(s,r,p);
*te = E;
*re = Durbin(k,a,r,E,p);
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : LPC2RefC
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : calculate k[] from a[]
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<float>& a,
// vector<float>& k
///////////////////////////////////////////////////////////////
void CDSP::LPC2RefC(const vector<float>& a, vector<float>& k)
{
vector<float> thisA; /* Current LP filter coefficients */
vector<float> newA;  /* New LP filter coefficients */
int i,j,p;
float ki,x;

p=a.size();
thisA = a;
newA.resize(p);
k.clear();
k.resize(p);

for ( i = p; i >= 1; i-- )
{
ki = -thisA[i];
k[i] = ki;
x = 1 - ki*ki;
for ( j = 1; j < i; j++ )
newA[j] = (thisA[j] + ki * thisA[i - j]) / x;
for ( j = 1; j < i; j++ )
thisA[j] = newA[j];
}

}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : RefC2LPC
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : calculate a[] from k[]
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<float>& k,
// vector<float>& a
///////////////////////////////////////////////////////////////
void CDSP::RefC2LPC(const vector<float>& k, vector<float>& a)
{

int i,j,p;
float ki;
p=k.size();
vector<float> thisA(p); /* Current LP filter coefficients */
vector<float> newA(p);  /* New LP filter coefficients */
a.clear();
a.resize(p);

for ( i=1; i <= p; i++ )
{
ki = k[i];
newA[i] = -ki;
for ( j = 1; j < i; j++ )
newA[j] = thisA[j] - ki * thisA[i - j];
for ( j = 1; j <= i; j++ )  
thisA[j] = newA[j];
}
for ( i = 1; i <= p; i++ )  a[i] = thisA[i];
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : LPC2Cepstrum
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ :
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<float>& a,
// vector<float>& c
///////////////////////////////////////////////////////////////
void CDSP::LPC2Cepstrum(const vector<float>& a, vector<float>& c)
{
int i,n,p;
float sum;

p = c.size();

for ( n = 1; n <= p; n++ )
{
sum = 0.0;
for ( i = 1; i < n; i++ )
sum = sum + (n - i) * a[i] * c[n - i];
c[n] = -(a[n] + sum / n);
}
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : Cepstrum2LPC
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ :
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<float>& c,
// vector<float>& a
///////////////////////////////////////////////////////////////
void CDSP::Cepstrum2LPC(const vector<float>& c, vector<float>& a)
{
int i,n,p;
float sum;

p = a.size();
for ( n = 1; n <= p; n++ )
{
sum = 0.0;
for ( i = 1 ; i < n; i++ )
sum = sum + (n - i) * a[i] * c[n - i];
a[n] = -(c[n] + sum / n);
}
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : SpecModulus
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : 20*lg|G(f)|
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<complex<float> >& spec,
// vector<float>& m
///////////////////////////////////////////////////////////////
void CDSP::SpecModulus(const vector<complex<float> >& spec, vector<float>& m)
{
int i;
m.clear();
m.resize(spec.size());

for ( i = 0; i < spec.size(); i++ )
m[i] = 20*log10(abs(spec[i]));
}

/*----------------------------------------------------------------*
*  conversion from lsf coefficients to lpc coefficients
*---------------------------------------------------------------*/

void CDSP::LSF2LPC(  const vector<float>& _lsf, vector<float>& a_coef)
{
int i, j;
float hlp;
int order = _lsf.size();
vector<float> p(order/2,0), q(order/2,0);
vector<float> a(order/2 + 1,0), a1(order/2,0), a2(order/2,0);
vector<float> b(order/2 + 1,0), b1(order/2,0), b2(order/2,0);
a_coef.clear();
a_coef.resize(order+1);
vector<float> lsf = _lsf;

for ( i = 0; i < order; i++ )
{
lsf[i] = lsf[i] * PI / 2;
}

/* Check input for ill-conditioned cases.  This part is not

Andersen et. al.  Experimental - Expires Jan. 1st, 2003         194

Internet Low Bit Rate Codec            July 2002

found in the TIA standard.  It involves the following 2 IF
blocks. If "freq" is judged ill-conditioned, then we first
modify lsf[0] and lsf[HALFORDER-1] (normally HALFORDER =
10 for LPC applications), then we adjust the other "freq"
values slightly */

if ( (lsf[0] <= 0.0) || (lsf[order - 1] >= 0.5) )
{

if ( lsf[0] <= 0.0 )
{
lsf[0] = (float)0.022;
}

if ( lsf[order - 1] >= 0.5 )
{
lsf[order - 1] = (float)0.499;
}

hlp = (lsf[order - 1] - lsf[0]) /  (float) (order - 1);

for ( i = 1; i < order; i++ )
{
lsf[i] = lsf[i - 1] + hlp;
}
}

/* p[i] and q[i] compute cos(2*pi*omega_{2j}) and
cos(2*pi*omega_{2j-1} in eqs. 4.2.2.2-1 and 4.2.2.2-2.  
Note that for this code p[i] specifies the coefficients
used in .Q_A(z) while q[i] specifies the coefficients used
in .P_A(z) */

for ( i = 0; i < order/2; i++ )
{
p[i] = (float)cos(2*PI * lsf[2 * i]);
q[i] = (float)cos(2*PI * lsf[2 * i + 1]);
}

a[0] = 0.25;
b[0] = 0.25;

for ( i = 0; i < order/2; i++ )
{
a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i];
b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i];
a2[i] = a1[i];
a1[i] = a[i];
b2[i] = b1[i];
b1[i] = b[i];
}


for ( j = 0; j < order; j++ )
{
if ( j == 0 )
{
a[0] = 0.25;
b[0] = -0.25;
}
else
{
a[0] = b[0] = 0.0;
}

for (i = 0; i < order/2; i++){
a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i];
b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i];
a2[i] = a1[i];
a1[i] = a[i];
b2[i] = b1[i];
b1[i] = b[i];
}

a_coef[j + 1] = 2 * (a[order/2] + b[order/2]);
}

a_coef[0] = 1.0;
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : FFT
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ :
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<short>& sig,
// vector<complex<float> >& spec,
// bool invert
///////////////////////////////////////////////////////////////
void CDSP::FFT (const vector<short>& sig, vector<complex<float> >& spec, bool invert)
{

int n = 1024, m = 10;
int n1, n2, i, j, k, l;
float xt, yt, c, s;
double e, a;
n2 = n;

if ( !invert )
{
spec.clear();
spec.resize(n);
for ( i = 0; i < spec.size(); i++ )
{

if ( i < sig.size() )
spec[i] = complex<float>(sig[i],0);
else
spec[i] = complex<float>(0,0);
}
}

for ( k = 0; k < m; k++ )
{
n1 = n2;
n2 = n2 / 2;
e = PI*2 / n1;
for ( j = 0; j < n2; j++ )
{
a = j * e;
c = (float) cos (a);
s = (float) sin (a) * (invert ? -1:1);
for ( i = j; i < n; i += n1 )
{
l = i + n2;
xt = real(spec[i] - spec[l]);
yt = imag(spec[i] - spec[l]);
spec[i] = spec[i] + spec[l];
spec[l] = complex<float>(c * xt + s * yt,c * yt - s * xt);
}
}
}
j = 0;
for ( i = 0; i < n - 1; i++ )
{
if ( i < j )
swap(spec[i],spec[j]);
k = n / 2;
while ( k <= j )
{
j -= k;
k /= 2;
}
j += k;
}
if ( invert )
for ( i = 0; i < spec.size(); i++ )
spec[i] /= (float)n;
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : window
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : window functions
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : vector<float>& wgt,
// int len,
// string type
///////////////////////////////////////////////////////////////
void CDSP::window(vector<float>& wgt, int len, string type)
{
wgt.clear();
wgt.resize(len);

float a[4];
if ( type == "rectangular" )
{
a[0] = 1; a[1] = a[2] = a[3] = 0;
}
else if ( type == "hanning" )
{
a[0] = a[1] = 0.5;
a[2] = a[3] = 0;
}
else if ( type == "hamming" )
{
a[0] = 0.54; a[1] = 0.46; a[2] = a[3] = 0;
}
else if ( type == "blackman" )
{
a[0] = 0.42; a[1] = 0.5; a[2] = 0.08; a[3] = 0;
}
else
cerr<<"You have chosen an error type of window function!"<<endl;
int i,j;
for ( i = 0; i < len; i++ )
{
wgt[i] = 0;
for ( j = 0; j < 4; j++ )
wgt[i] += pow(-1,j) * a[j] * cos(2*3.14159*i*j/(len-1.0));
}
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : LPC2LSF
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : lpc to lsf
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ : to avoid the complex computation we convert
// "poly2lsf.m" in MatLab to "poly2lsf.h" with MatCom
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<float>& a,
// vector<float>& lsf
///////////////////////////////////////////////////////////////
#ifdef _USE_LSF_
void CDSP::LPC2LSF(const vector<float>& a, vector<float>& lsf)
{
lsf.clear();
lsf.resize(a.size()-1);

initM(MATCOM_VERSION);

Mm _a = zeros(1,a.size()), _lsf(lsf.size());
int i;
for ( i = 0; i < a.size(); i++ )
*_a.addr(1,i+1) = a[i];
_lsf = poly2lsf(_a);
for ( i = 0; i < lsf.size(); i++ )
lsf[i] = _lsf.r(i+1);

exitM();
}
#endif
///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : LPCSpec
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : LPC Spectrum
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<float>& a,
// int len,
// float G,
// vector<complex<float> >& spec
///////////////////////////////////////////////////////////////
void CDSP::LPCSpec(const vector<float>& a, int len, float G, vector<complex<float> >& spec)
{
spec.clear();
spec.resize(len);
int i,j,p = a.size()-1;
complex<float> tmp;
for ( i = 0; i < len; i++ )
{
tmp = complex<float>(0,0);
for ( j = 0; j <= p; j++)
tmp += a[j] * complex<float>(cos((float)i/len*2*PI*j),-sin((float)i/len*2*PI*j));
spec[i] = G / tmp;
}
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : GetG
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ :
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : float
// ²ÎÊý˵Ã÷ : const vector<short>& s,
// const vector<float>& a
///////////////////////////////////////////////////////////////
float CDSP::GetG(const vector<short>& s, const vector<float>& a)
{
float E = 0, e = 0, G;
int i,j,p = a.size()-1;
for ( i = p; i < s.size(); i++ )
{
e = 0;
for ( j = 0; j <= p; j++)
e += a[j] * s[i-j];
E += e*e;
}
G = sqrt(E);
return G;
}

///////////////////////////////////////////////////////////////
// º¯ Êý Ãû : ExcitationSpec
// ËùÊôÀàÃû : CDSP
// º¯Êý¹¦ÄÜ : excitation spectrum
// ´¦Àí¹ý³Ì :
// ±¸    ×¢ :
// ×÷    Õß : Taliux
// ʱ    ¼ä : 2005Äê3ÔÂ21ÈÕ
// ·µ »Ø Öµ : void
// ²ÎÊý˵Ã÷ : const vector<complex<float> >& WavSpec,
// const vector<complex<float> >& LpcSpec,
// vector<complex<float> >& ExcSpec
///////////////////////////////////////////////////////////////
void CDSP::ExcitationSpec(const vector<complex<float> >& WavSpec, const vector<complex<float> >& LpcSpec, vector<complex<float> >& ExcSpec)
{
int i;
ExcSpec.clear();
ExcSpec.resize(WavSpec.size());

for ( i = 0; i < WavSpec.size(); i++ )
ExcSpec[i] = WavSpec[i] / LpcSpec[i];

}