static void ao_vdivs(vec *c, vec v, double n)
{
vDSP_vsdivD(v.co, 1, &n, c->co, 1, 3);
/*
c->x = v.x / n;
c->y = v.y / n;
c->z = v.z / n;
*/
}
void AVFEqualizerBand::SetupHighShelfFilter(double omega, double bw, double gain) {
double egm = gain - 1.0;
double egp = gain + 1.0;
double alpha = tan(bw / 2.0);
double delta = 2.0 * sqrt(gain) * alpha;
double cosF = cos(omega);
mCoefficients[IND_A0] = (egp + egm * cosF + delta) * gain;
mCoefficients[IND_A1] = (egm + egp * cosF) * -2.0 * gain;
mCoefficients[IND_A2] = (egp + egm * cosF - delta) * gain;
double b0 = egp - egm * cosF + delta;
mCoefficients[IND_B1] = (egm - egp * cosF) * 2.0;
mCoefficients[IND_B2] = egp - egm * cosF - delta;
// pre-scale coefficients
vDSP_vsdivD(mCoefficients, 1, &b0, mCoefficients, 1, 5);
}
/*
* vDSP_deq22:
* https://developer.apple.com/library/mac/documentation/Accelerate/Reference/vDSPRef/Reference/reference.html#//apple_ref/c/func/vDSP_deq22
*
* The biquadratic filter equation for the nth sample is:
* D[n] = S[n] * a0 + S[n-1] * a1 + S[n-2] * a2 - D[n-1] * b1 - D[n-2] * b2
*
* vDSP_deq22 stuffs all coefficients in vector B and uses:
* for p in [0,2]:
* A(n-p)i * B(p) -> A(n)*B[0] + A(n-1)*B[1] + A(n-2)*B[2]
*
* for p in [3,4]:
* C(n-p+2)k * B(p) -> C(n-1)*B[3] + C(n-2)*B[4]
*
* where A and C are vectors of at least size N+2
* so B[0..2] is a0 to a2 respectively and B[3..4] is b1 and b2 respectively
*
* The formulae used to calculate the coefficients are taken from GStreamer so
* we can match the behavior of the GStreamer pipeline (and they work well enough)
* though modified for SIMD operations using vDSP_deq22.
*
* Note that the GStreamer coefficient names (a0-a2,b0-b2) are swapped from other
* examples, but the use is the same.
*/
void AVFEqualizerBand::SetupPeakFilter(double omega, double bw, double gain) {
double cosF = cos(omega);
double alpha = tan(bw / 2.0);
double alpha1 = alpha * gain;
double alpha2 = alpha / gain;
// set up peak filter coefficients
mCoefficients[IND_A0] = 1.0 + alpha1;
mCoefficients[IND_A1] = -2.0 * cosF;
mCoefficients[IND_A2] = 1.0 - alpha1;
double b0 = 1.0 + alpha2;
mCoefficients[IND_B1] = -2.0 * cosF;
mCoefficients[IND_B2] = 1.0 - alpha2;
// pre-scale coefficients
vDSP_vsdivD(mCoefficients, 1, &b0, mCoefficients, 1, 5);
}
int xtract_autocorrelation_fft(const double *data, const int N, const void *argv, double *result)
{
double *rfft = NULL;
int n = 0;
int M = 0;
#ifndef USE_OOURA
DSPDoubleSplitComplex *fft = NULL;
double M_double = 0.0;
#endif
M = N << 1;
/* Zero pad the input vector */
rfft = (double *)calloc(M, sizeof(double));
memcpy(rfft, data, N * sizeof(double));
#ifdef USE_OOURA
rdft(M, 1, rfft, ooura_data_autocorrelation_fft.ooura_ip,
ooura_data_autocorrelation_fft.ooura_w);
for(n = 2; n < M; ++n)
{
rfft[n*2] = XTRACT_SQ(rfft[n*2]) + XTRACT_SQ(rfft[n*2+1]);
rfft[n*2+1] = 0.0;
}
rfft[0] = XTRACT_SQ(rfft[0]);
rfft[1] = XTRACT_SQ(rfft[1]);
rdft(M, -1, rfft, ooura_data_autocorrelation_fft.ooura_ip,
ooura_data_autocorrelation_fft.ooura_w);
#else
/* vDSP has its own autocorrelation function, but it doesn't fit the
* LibXtract model, e.g. we can't guarantee it's going to use
* an FFT for all values of N */
fft = &vdsp_data_autocorrelation_fft.fft;
vDSP_ctozD((DSPDoubleComplex *)data, 2, fft, 1, N);
vDSP_fft_zripD(vdsp_data_autocorrelation_fft.setup, fft, 1,
vdsp_data_autocorrelation_fft.log2N, FFT_FORWARD);
for(n = 0; n < N; ++n)
{
fft->realp[n] = XTRACT_SQ(fft->realp[n]) + XTRACT_SQ(fft->imagp[n]);
fft->imagp[n] = 0.0;
}
vDSP_fft_zripD(vdsp_data_autocorrelation_fft.setup, fft, 1,
vdsp_data_autocorrelation_fft.log2N, FFT_INVERSE);
#endif
/* Normalisation factor */
M = M * N;
#ifdef USE_OOURA
for(n = 0; n < N; n++)
result[n] = rfft[n] / (double)M;
free(rfft);
#else
M_double = (double)M;
vDSP_ztocD(fft, 1, (DOUBLE_COMPLEX *)result, 2, N);
vDSP_vsdivD(result, 1, &M_double, result, 1, N);
#endif
return XTRACT_SUCCESS;
}
/* Q: problem data, 1-dim array of length n*n
* wneg: slope of negative part, 1-dim array of length n
* wpos: slope of positive part, 1-dim array of length n
* sigma: initial penalty parameter
* maxIter: max number of iterations
* d: initial feasible vector d, 1-dim array of length n
* iter_arr, obj_arr and time_arr are output variables, CURRENTLY NOT USED!!! */
int CDlogdet_nonsmooth(int n, double* Q, double* wneg, double* wpos, double sigma,
int maxIter, double* d, double* Vinit,
int* iter_arr, double* obj_arr, double* time_arr)
{
int iter = 0;
int k = 0;
double Vkk = 0;
double dk = 0;
double dchange = 0;
clock_t iterTimer = 0;
double GRAD_TOL = 3E-2;
double MIN_TAU = 1E-5;
double TERM_PROG = 1E-4;
double relgrad = 1E17;
double tmpScalar1 = 0;
double VERYSMALLNUM = 1E-9;
double obj = 0;
double prevobj = 1E17;
double nrmW = 0;
double TAU_UPDATE = 0.8;
int nsq = n*n;
vDSP_Length tmpPosition = 0;
double* V = NULL;
double* vk = NULL;
double* tmpVec1 = NULL;
double* tmpVec2 = NULL;
double* tmpVec3 = NULL;
double* tmpVec4 = NULL;
double* tmpVec5 = NULL;
double* tmpVec6 = NULL;
double* subg = NULL;
V = (double*) malloc(nsq*sizeof(double));
vk = (double*) malloc(n*sizeof(double));
subg = (double*) malloc(n*sizeof(double));
tmpVec1 = (double*) malloc(n*sizeof(double));
tmpVec2 = (double*) malloc(n*sizeof(double));
tmpVec3 = (double*) malloc(n*sizeof(double));
tmpVec4 = (double*) malloc(n*sizeof(double));
tmpVec5 = (double*) malloc(n*sizeof(double));
tmpVec6 = (double*) malloc(n*sizeof(double));
char UPLO = 'L';
int LDA = n;
int INFO = 0;
if (PRINTLEVEL)
{
PRINT("Entering CDlogdet_nonsmooth: n=%d, sigma=%f, maxIter=%d\n",n,sigma,maxIter);
}
iterTimer = clock();
/*Compute V = inv(Q+d), only stores the lower triangular part*/
if (NULL==Vinit)
{
cblas_dcopy(nsq, Q, 1, V, 1);
cblas_daxpy(n, 1, d, 1, V, n+1);
UPLO = 'L';
LDA = n;
INFO = 0;
dpotrf_(&UPLO, &n, V, &LDA, &INFO);
dpotri_(&UPLO, &n, V, &LDA, &INFO);
}
else
{
cblas_dcopy(nsq, Vinit, 1, V, 1);
}
//printMat(V,n);
//printMat(Vinit,n);
nrmW = MAX(cblas_dnrm2(n, wneg, 1), cblas_dnrm2(n, wpos, 1));
vDSP_vmulD(wneg,1,d,1,tmpVec1,1,n);
vDSP_vmulD(wpos,1,d,1,tmpVec2,1,n);
vDSP_vmaxD(tmpVec1, 1, tmpVec2, 1, tmpVec3, 1, n);
vDSP_sveD(tmpVec3,1,&prevobj,n);
while(iter<maxIter)
{
/*Compute sub-gradient*/
/*tmpVec1 <= wneg*/
cblas_dcopy(n, wneg, 1, tmpVec1, 1);
/*tmpVec1 <= wneg-sigma*diag(V)*/
cblas_daxpy(n, -sigma, V, n+1, tmpVec1,1);
/*tmpVec2 <= wpos*/
cblas_dcopy(n, wpos, 1, tmpVec2, 1);
/*tmpVec2 <= wpos-sigma*diag(V)*/
cblas_daxpy(n, -sigma, V, n+1, tmpVec2,1);
/*tmpVec3 <= max(0,wneg-sigma*diag(V)) */
tmpScalar1 = 0;
vDSP_vthresD(tmpVec1,1,&tmpScalar1,tmpVec3,1,n);
/*tmpVec4 <= -min(0,wpos-sigma*diag(V))*/
vDSP_vnegD(tmpVec2,1,tmpVec4,1,n);
tmpScalar1 = 0;
vDSP_vthresD(tmpVec4,1,&tmpScalar1,tmpVec4,1,n);
/*tmpVec3 <= max(0,wneg-sigma*diag(V)) + min(0,wpos-sigma*diag(V))*/
cblas_daxpy(n,-1,tmpVec4,1,tmpVec3,1);
/*If d<0, use wneg-sigma*diag(V)*/
/*tmpVec4_i = 1 <==> d_i < -VERYSMALLNUM */
/*tmpVec4 <= (-d)*/
vDSP_vnegD(d,1,tmpVec4,1,n);
tmpScalar1 = 1;
vDSP_vlimD(tmpVec4,1, &VERYSMALLNUM, &tmpScalar1,tmpVec4,1,n);
tmpScalar1 = 1;
vDSP_vsaddD(tmpVec4,1,&tmpScalar1, tmpVec4, 1, n);
tmpScalar1 = 2;
vDSP_vsdivD(tmpVec4,1,&tmpScalar1, tmpVec4, 1, n);
/*tmpVec5[i] = 1 <==> d_i > VERYSMALLNUM*/
cblas_dcopy(n,d,1,tmpVec5,1);
tmpScalar1 = 1;
vDSP_vlimD(tmpVec5, 1, &VERYSMALLNUM, &tmpScalar1, tmpVec5, 1, n);
tmpScalar1 = 1;
vDSP_vsaddD(tmpVec5, 1, &tmpScalar1, tmpVec5, 1, n);
tmpScalar1 = 2;
vDSP_vsdivD(tmpVec5,1,&tmpScalar1, tmpVec5, 1, n);
/*tmpVec6[i] = 1 <==> abs(d_i) < VERYSMALLNUM*/
vDSP_vaddD(tmpVec4, 1, tmpVec5, 1, tmpVec6, 1, n);
tmpScalar1 = -1;
vDSP_vsaddD(tmpVec6,1, &tmpScalar1, tmpVec6, 1, n);
vDSP_vnegD(tmpVec6,1,tmpVec6,1,n);
/*Multiply, Multiply, Multiply, then add */
vDSP_vmmaD(tmpVec1, 1, tmpVec4, 1, tmpVec2, 1, tmpVec5, 1, subg,1, n);
vDSP_vmaD(tmpVec3, 1, tmpVec6, 1, subg, 1, subg, 1, n);
/*Choose the index with largest abs(subg)*/
vDSP_maxmgviD(subg,1,&tmpScalar1,&tmpPosition,n);
/*k is the selected index*/
k = (int) tmpPosition;
Vkk = V[k*n+k];
dk = d[k];
if (1-dk*Vkk<=0 || (sigma*Vkk/(1-dk*Vkk)>wpos[k]) )
{
dchange = sigma/wpos[k] - 1/Vkk;
}
else if(sigma*Vkk/(1-dk*Vkk)<wneg[k])
{
dchange = sigma/wneg[k] - 1/Vkk;
}
else
{
dchange = -dk;
}
d[k] += dchange;
cblas_dcopy(k+1, &V[k], n, vk, 1);
cblas_dcopy(n-k-1, &V[k*n+k+1], 1, &vk[k+1], 1);
tmpScalar1 = -(dchange/(1+dchange*Vkk));
cblas_dsyr(CblasColMajor, CblasLower, n, tmpScalar1, vk, 1, V, n);
iter ++;
relgrad = cblas_dnrm2(n,subg,1)/nrmW;
vDSP_vmulD(wneg,1,d,1,tmpVec1,1,n);
vDSP_vmulD(wpos,1,d,1,tmpVec2,1,n);
vDSP_vmaxD(tmpVec1, 1, tmpVec2, 1, tmpVec3, 1, n);
vDSP_sveD(tmpVec3,1,&obj,n);
if (relgrad<GRAD_TOL)
{
sigma = MAX(MIN_TAU, sigma*TAU_UPDATE);
if (PRINTLEVEL)
{
PRINT("Iter = %5d, sig=%1.2e(#) , obj = %1.5e, relgrad = %1.3f\n",
iter, sigma, obj, relgrad);
}
}
else if (iter==1 || iter%n==0)
{
if (PRINTLEVEL)
{
PRINT("Iter = %5d, sig=%1.2e , obj = %1.5e, relgrad = %1.3f, t=%0.3f\n",
iter, sigma, obj, relgrad,((double)(clock()-iterTimer))/CLOCKS_PER_SEC);
}
}
if (iter%n==0)
{
if (obj<prevobj && ABS(prevobj-obj)<TERM_PROG*ABS(obj))
{
if(PRINTLEVEL)
{
PRINT("Terminate due to small progress.\n");
}
break;
}
else
{
prevobj = obj;
}
}
}
free(V);
free(vk);
free(tmpVec1);
free(tmpVec2);
free(tmpVec3);
free(tmpVec4);
free(tmpVec5);
free(tmpVec6);
free(subg);
return 0;
}
