int main()
{
int n, ip[NMAXSQRT + 2];
double a[NMAX + 1], w[NMAX * 5 / 4], t[NMAX / 2 + 1], err;
printf("data length n=? (must be 2^m)\n");
scanf("%d", &n);
ip[0] = 0;
/* check of CDFT */
putdata(0, n - 1, a);
cdft(n, 1, a, ip, w);
cdft(n, -1, a, ip, w);
err = errorcheck(0, n - 1, 2.0 / n, a);
printf("cdft err= %g \n", err);
/* check of RDFT */
putdata(0, n - 1, a);
rdft(n, 1, a, ip, w);
rdft(n, -1, a, ip, w);
err = errorcheck(0, n - 1, 2.0 / n, a);
printf("rdft err= %g \n", err);
/* check of DDCT */
putdata(0, n - 1, a);
ddct(n, 1, a, ip, w);
ddct(n, -1, a, ip, w);
a[0] *= 0.5;
err = errorcheck(0, n - 1, 2.0 / n, a);
printf("ddct err= %g \n", err);
/* check of DDST */
putdata(0, n - 1, a);
ddst(n, 1, a, ip, w);
ddst(n, -1, a, ip, w);
a[0] *= 0.5;
err = errorcheck(0, n - 1, 2.0 / n, a);
printf("ddst err= %g \n", err);
/* check of DFCT */
putdata(0, n, a);
a[0] *= 0.5;
a[n] *= 0.5;
dfct(n, a, t, ip, w);
a[0] *= 0.5;
a[n] *= 0.5;
dfct(n, a, t, ip, w);
err = errorcheck(0, n, 2.0 / n, a);
printf("dfct err= %g \n", err);
/* check of DFST */
putdata(1, n - 1, a);
dfst(n, a, t, ip, w);
dfst(n, a, t, ip, w);
err = errorcheck(1, n - 1, 2.0 / n, a);
printf("dfst err= %g \n", err);
return 0;
}
// 音声コールバック関数
LRESULT CALLBACK CSpectrumAnalyzer::AudioCallback(short *pData,DWORD Samples,int Channels,void *pClientData)
{
CSpectrumAnalyzer *pThis=static_cast<CSpectrumAnalyzer*>(pClientData);
DWORD BufferUsed=pThis->m_BufferUsed;
DWORD j=pThis->m_BufferPos;
for (DWORD i=0;i<Samples;i++) {
if (j==FFT_SIZE)
j=0;
pThis->m_pBuffer[j]=pData[i*Channels];
j++;
BufferUsed++;
if (BufferUsed==FFT_SIZE) {
pThis->m_FFTLock.Lock();
// doubleに変換
for (int k=0;k<FFT_SIZE;k++) {
pThis->m_pFFTBuffer[k]=
(double)pThis->m_pBuffer[(j+k)%FFT_SIZE]/32768.0;
}
// FFT処理実行
rdft(FFT_SIZE,1,pThis->m_pFFTBuffer,
pThis->m_pFFTWorkBuffer,pThis->m_pFFTSinTable);
pThis->m_FFTLock.Unlock();
BufferUsed=0;
}
}
pThis->m_BufferUsed=BufferUsed;
pThis->m_BufferPos=j;
return 0;
}
void aubio_fft_rdo_complex(aubio_fft_t * s, fvec_t * compspec, fvec_t * output) {
uint_t i;
#ifdef HAVE_FFTW3
const smpl_t renorm = 1./(smpl_t)s->winsize;
#ifdef HAVE_COMPLEX_H
s->specdata[0] = compspec->data[0];
for (i=1; i < s->fft_size - 1; i++) {
s->specdata[i] = compspec->data[i] +
I * compspec->data[compspec->length - i];
}
s->specdata[s->fft_size - 1] = compspec->data[s->fft_size - 1];
#else
for (i=0; i < s->fft_size; i++) {
s->specdata[i] = compspec->data[i];
}
#endif
fftw_execute(s->pbw);
for (i = 0; i < output->length; i++) {
output->data[i] = s->out[i]*renorm;
}
#else /* HAVE_FFTW3 */
smpl_t scale = 2.0 / s->winsize;
s->out[0] = compspec->data[0];
s->out[1] = compspec->data[s->winsize / 2];
for (i = 1; i < s->fft_size - 1; i++) {
s->out[2 * i] = compspec->data[i];
s->out[2 * i + 1] = - compspec->data[s->winsize - i];
}
rdft(s->winsize, -1, s->out, s->ip, s->w);
for (i=0; i < s->winsize; i++) {
output->data[i] = s->out[i] * scale;
}
#endif /* HAVE_FFTW3 */
}
void rfft(int n,int isign,REAL *x)
{
static int ipsize = 0,wsize=0;
static int *ip = NULL;
static REAL *w = NULL;
int newipsize,newwsize;
if (n == 0) {
free(ip); ip = NULL; ipsize = 0;
free(w); w = NULL; wsize = 0;
return;
}
n = 1 << n;
newipsize = 2+sqrt(n/2);
if (newipsize > ipsize) {
ipsize = newipsize;
ip = (int *)realloc(ip,sizeof(int)*ipsize);
ip[0] = 0;
}
newwsize = n/2;
if (newwsize > wsize) {
wsize = newwsize;
w = (REAL *)realloc(w,sizeof(REAL)*wsize);
}
rdft(n,isign,x,ip,w);
}
void aubio_fft_do_complex(aubio_fft_t * s, fvec_t * input, fvec_t * compspec) {
uint_t i;
for (i=0; i < s->winsize; i++) {
s->in[i] = input->data[i];
}
#ifdef HAVE_FFTW3
fftw_execute(s->pfw);
#ifdef HAVE_COMPLEX_H
compspec->data[0] = REAL(s->specdata[0]);
for (i = 1; i < s->fft_size -1 ; i++) {
compspec->data[i] = REAL(s->specdata[i]);
compspec->data[compspec->length - i] = IMAG(s->specdata[i]);
}
compspec->data[s->fft_size-1] = REAL(s->specdata[s->fft_size-1]);
#else /* HAVE_COMPLEX_H */
for (i = 0; i < s->fft_size; i++) {
compspec->data[i] = s->specdata[i];
}
#endif /* HAVE_COMPLEX_H */
#else /* HAVE_FFTW3 */
rdft(s->winsize, 1, s->in, s->ip, s->w);
compspec->data[0] = s->in[0];
compspec->data[s->winsize / 2] = s->in[1];
for (i = 1; i < s->fft_size - 1; i++) {
compspec->data[i] = s->in[2 * i];
compspec->data[s->winsize - i] = - s->in[2 * i + 1];
}
#endif /* HAVE_FFTW3 */
}
void do_thresher(t_thresher *x)
{
int i;
t_fftease *fft = x->fft;
t_float *channel = fft->channel;
t_float damping_factor = x->damping_factor;
int max_hold_frames = x->max_hold_frames;
int *frames_left = x->frames_left;
t_float *composite_frame = x->composite_frame;
int N = fft->N;
t_float move_threshold = x->move_threshold;
fold(fft);
rdft(fft,FFT_FORWARD);
convert(fft);
if( x->first_frame ){
for ( i = 0; i < N+2; i++ ){
composite_frame[i] = channel[i];
frames_left[i] = max_hold_frames;
}
x->first_frame = 0;
} else {
for( i = 0; i < N+2; i += 2 ){
if(fabs( composite_frame[i] - channel[i] ) > move_threshold || frames_left[i] <= 0 ){
composite_frame[i] = channel[i];
composite_frame[i+1] = channel[i+1];
frames_left[i] = max_hold_frames;
} else {
--(frames_left[i]);
composite_frame[i] *= damping_factor;
}
}
}
// try memcpy here
for ( i = 0; i < N+2; i++ ){
channel[i] = composite_frame[i];
}
if(fft->obank_flag){
oscbank(fft);
} else {
unconvert(fft);
rdft(fft,FFT_INVERSE);
overlapadd(fft);
}
}
void do_leaker(t_leaker *x)
{
int i,odd,even;
t_float a1,a2,b1,b2;
t_fftease *fft = x->fft;
t_fftease *fft2 = x->fft2;
int N2 = fft->N2;
t_float *buffer1 = fft->buffer;
t_float *buffer2 = fft2->buffer;
t_float *channel1 = fft->channel;
int *sieve = x->sieve;
t_float fade_value = x->fade_value;
fold(fft);
fold(fft2);
rdft(fft,1);
rdft(fft2,1);
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
if( fade_value <= 0 || fade_value < sieve[i] ){
a1 = ( i == N2 ? *(buffer1+1) : *(buffer1+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(buffer1+odd) );
*(channel1+even) = hypot( a1, b1 ) ;
*(channel1+odd) = -atan2( b1, a1 );
*(buffer1+even) = *(channel1+even) * cos(*(channel1+odd));
if ( i != N2 ){
*(buffer1+odd) = -(*(channel1+even)) * sin(*(channel1+odd));
}
} else {
a2 = ( i == N2 ? *(buffer2+1) : *(buffer2+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(buffer2+odd) );
*(channel1+even) = hypot( a2, b2 ) ;
*(channel1+odd) = -atan2( b2, a2 );
*(buffer1+even) = *(channel1+even) * cos(*(channel1+odd) );
if ( i != N2 ){
*(buffer1+odd) = -(*(channel1+even)) * sin( *(channel1+odd) );
}
}
}
rdft(fft,-1);
overlapadd(fft);
}
Var* ff_realfft2(vfuncptr func, Var* arg)
{
Var* obj = NULL;
int i, j, n, x;
double* in;
Alist alist[3];
alist[0] = make_alist("obj", ID_VAL, NULL, &obj);
alist[1].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (obj == NULL) {
parse_error("%s: No object specified\n", func->name);
return (NULL);
}
n = V_DSIZE(obj);
for (x = n; (x & 1) == 0; x >>= 1)
;
if (x != 1) {
parse_error("dimension not a power of 2. Use version 0.\n");
return (NULL);
}
in = (double*)calloc(n, sizeof(double));
for (i = 0; i < n; i++) {
in[i] = extract_double(obj, i);
}
if (func->fdata == (void*)1) {
rdft(n, cos(M_PI / n), sin(M_PI / n), in);
} else {
rdft(n, cos(M_PI / n), -sin(M_PI / n), in);
for (j = 0; j <= n - 1; j++) {
in[j] *= 2.0 / n;
}
}
return (newVal(BSQ, 1, n, 1, DV_DOUBLE, in));
}
// input : Signal *x
// output: energy spectrum *erg
void
PowSpec256 ( const float* x, float* erg )
{
int i;
// windowing
for( i = 0; i < 256; ++i )
a[i] = x[i] * Hann_256[i];
rdft(256, a, ip, w); // perform FFT
// calculate power
for( i = 0; i < 128; ++i)
erg[i] = a[i*2] * a[i*2] + a[i*2+1] * a[i*2+1];
}
// input : Signal *x
// output: energy spectrum *erg
void
PowSpec1024 ( const float* x, float* erg )
{
int i;
// windowing
for( i = 0; i < 1024; ++i )
a[i] = x[i] * Hann_1024[i];
rdft(1024, a, ip, w); // perform FFT
// calculate power
for( i = 0; i < 512; ++i)
erg[i] = a[i*2] * a[i*2] + a[i*2+1] * a[i*2+1];
}
// input : logarithmized energy spectrum *cep
// output: Cepstrum *cep (in-place)
void
Cepstrum2048 ( float* cep, const int MaxLine )
{
int i, j;
// generate real, even spectrum (symmetric around 1024, cep[2048-i] = cep[i])
for( i = 0, j = 1024; i < 1024; ++i, --j )
cep[1024 + j] = cep[i];
rdft(2048, cep, ip, w);
// only real part as outcome (all even indexes of cep[])
for( i = 0; i < MaxLine + 1; ++i )
cep[i] = cep[i*2] * (float) (0.9888 / 2048);
}
// input : Signal *x
// output: energy spectrum *erg
void
PowSpec2048 ( const float* x, float* erg )
{
int i;
// windowing (only 1600 samples available -> centered in 2048!)
memset(a, 0, 224 * sizeof *a);
for( i = 0; i < 1600; ++i )
a[i+224] = x[i] * Hann_1600[i];
memset(a + 1824, 0, 224 * sizeof *a);
rdft(2048, a, ip, w); // perform FFT
// calculate power
for( i = 0; i < 1024; ++i)
erg[i] = a[i*2] * a[i*2] + a[i*2+1] * a[i*2+1];
}
// input : Signal *x
// output: energy spectrum *erg and phase spectrum *phs
void
PolarSpec1024 ( const float* x, float* erg, float* phs )
{
int i;
for( i = 0; i < 1024; i++ )
a[i] = x[i] * Hann_1024[i];
rdft( 1024, a, ip, w); // perform FFT
// calculate power and phase
for( i = 0; i < 512; ++i )
{
erg[i] = a[i*2] * a[i*2] + a[i*2+1] * a[i*2+1];
phs[i] = ATAN2F( a[i*2+1], a[i*2] );
}
}
void OouraFFT::fft(float* in, std::complex<float>* out)
{
size_t nfft = buffer_.size();
// copy input data to buffer used by ooura
for (size_t i = 0; i < nfft; ++i)
buffer_[i] = in[i];
// perform forward DFT
rdft((int)nfft, +1, buffer_.data(), ip_.data(), sineTable_.data());
// copy to output from the ooura format
for (size_t i = 0; i < nfft / 2; ++i)
{
out[i].real((float)buffer_[i * 2]); //real part
out[i].imag((float)buffer_[i * 2 + 1]); // imag part
}
out[nfft / 2].real((float)buffer_[1]); // a[1] = R[n/2]
}
void OouraFFT::ifft(std::complex<float>* in, float* out)
{
size_t nfft = buffer_.size();
// copy to the ooura format
for (size_t i = 0; i < nfft / 2; ++i)
{
buffer_[i * 2] = in[i].real(); // real part
buffer_[i * 2 + 1] = in[i].imag(); // imag part
}
buffer_[1] = in[nfft / 2].real(); // a[1] = R[n/2]
// perform inverse DFT
rdft((int)nfft, -1, buffer_.data(), ip_.data(), sineTable_.data());
// copy to output
for (size_t i = 0; i < nfft; ++i)
out[i] = (float)buffer_[i];
}
void PCM::getPCM(float *PCMdata, int samples, int channel, int freq, float smoothing, int derive)
{
int i,index;
index=start-1;
if (index<0) index=maxsamples+index;
PCMdata[0]=PCMd[channel][index];
for(i=1;i<samples;i++)
{
index=start-1-i;
if (index<0) index=maxsamples+index;
PCMdata[i]=(1-smoothing)*PCMd[channel][index]+smoothing*PCMdata[i-1];
}
//return derivative of PCM data
if(derive)
{
for(i=0;i<samples-1;i++)
{
PCMdata[i]=PCMdata[i]-PCMdata[i+1];
}
PCMdata[samples-1]=0;
}
//return frequency data instead of PCM (perform FFT)
if (freq)
{
double temppcm[1024];
for (int i=0;i<samples;i++)
{temppcm[i]=(double)PCMdata[i];}
rdft(samples, 1, temppcm, ip, w);
for (int j=0;j<samples;j++)
{PCMdata[j]=(float)temppcm[j];}
}
}
virtual void ifft(float* data, const float* re, const float* im) override
{
// Convert into the format as required by the Ooura FFT
{
double* b = &_buffer[0];
double* bEnd = b + _size;
const float *r = re;
const float *i = im;
while (b != bEnd)
{
*(b++) = static_cast<double>(*(r++));
*(b++) = -static_cast<double>(*(i++));
}
_buffer[1] = re[_size / 2];
}
rdft(static_cast<int>(_size), -1, _buffer.data(), _ip.data(), _w.data());
// Convert back to split-complex
ScaleBuffer(data, &_buffer[0], 2.0 / static_cast<double>(_size), _size);
}
virtual void fft(const float* data, float* re, float* im) override
{
// Convert into the format as required by the Ooura FFT
ConvertBuffer(&_buffer[0], data, _size);
rdft(static_cast<int>(_size), +1, _buffer.data(), _ip.data(), _w.data());
// Convert back to split-complex
{
double* b = &_buffer[0];
double* bEnd = b + _size;
float *r = re;
float *i = im;
while (b != bEnd)
{
*(r++) = static_cast<float>(*(b++));
*(i++) = static_cast<float>(-(*(b++)));
}
}
const size_t size2 = _size / 2;
re[size2] = -im[0];
im[0] = 0.0;
im[size2] = 0.0;
}
KstObject::UpdateType KstPSDCurve::update(int update_counter) {
int i_subset, i_samp;
int n_subsets;
int v_len;
int copyLen;
double mean;
double y;
bool force = false;
KstVectorPtr iv = _inputVectors[INVECTOR];
double *psd;
if (KstObject::checkUpdateCounter(update_counter))
return NO_CHANGE;
if (update_counter <= 0) {
force = true;
} else {
iv->update(update_counter);
}
v_len = iv->sampleCount();
n_subsets = v_len/PSDLen+1;
last_n_new += iv->numNew();
if ((last_n_new < PSDLen/16) && (n_subsets - last_n_subsets < 1) && !force) {
return NO_CHANGE;
}
psd = (*_sVector)->value();
for (i_samp = 0; i_samp < PSDLen; i_samp++) {
psd[i_samp] = 0;
}
for (i_subset = 0; i_subset < n_subsets; i_subset++) {
/* copy each chunk into a[] and find mean */
if (i_subset*PSDLen + ALen <= v_len) {
copyLen = ALen;
} else {
copyLen = v_len - i_subset*PSDLen;
}
mean = 0;
for (i_samp = 0; i_samp < copyLen; i_samp++) {
mean += (
a[i_samp] =
iv->interpolate(i_samp + i_subset*PSDLen, v_len)
);
}
if (copyLen>1) mean/=(double)copyLen;
/* Remove Mean and apodize */
if (removeMean() && appodize()) {
for (i_samp=0; i_samp<copyLen; i_samp++) {
a[i_samp]= (a[i_samp]-mean)*w[i_samp];
}
} else if (removeMean()) {
for (i_samp=0; i_samp<copyLen; i_samp++) {
a[i_samp] -= mean;
}
} else if (appodize()) {
for (i_samp=0; i_samp<copyLen; i_samp++) {
a[i_samp] *= w[i_samp];
}
}
for (;i_samp < ALen; i_samp++) a[i_samp] = 0.0;
/* fft a */
rdft(ALen, 1, a);
/* sum each bin into psd[] */
psd[0]+=a[0];
psd[PSDLen-1] += a[1];
for (i_samp=1; i_samp<PSDLen-1; i_samp++) {
psd[i_samp]+= cabs(a[i_samp*2], a[i_samp*2+1]);
}
}
last_f0 = 0;
last_n_subsets = n_subsets;
last_n_new = 0;
norm_factor = 1.0/(sqrt(double(Freq)*double(PSDLen))*double(n_subsets));
psd[0]*=norm_factor;
MaxY = MinY = mean = psd[0];
if (psd[0]>0)
MinPosY = psd[0];
else (MinPosY = 1.0e300);
/* normalize psd */
for (i_samp=1; i_samp<PSDLen; i_samp++) {
y = (psd[i_samp]*=norm_factor);
if (y>MaxY)
MaxY=y;
if (y<MinY)
MinY=y;
if ((y>0) && (y<MinPosY))
MinPosY = y;
mean +=y;
}
if (PSDLen > 0)
MeanY = mean/PSDLen;
else MeanY = 0; // should never ever happen...
NS = PSDLen;
if (Freq <= 0)
Freq = 1.0;
MaxX = Freq/2.0;
MinX = 0;
MinPosX = 1.0/double(NS) * MaxX;
MeanX = MaxX/2.0;
double *f = (*_fVector)->value();
f[0] = 0;
f[1] = Freq/2.0;
(*_sVector)->update(update_counter);
(*_fVector)->update(update_counter);
return UPDATE;
}
void do_cross(t_cross *x)
{
t_fftease *fft = x->fft;
t_fftease *fft2 = x->fft2;
int i;
int N2 = fft->N2;
t_float a1, b1, a2, b2;
t_float *buffer1 = fft->buffer;
t_float *buffer2 = fft2->buffer;
t_float *channel1 = fft->channel;
short autonorm = x->autonorm;
int N = fft->N;
t_float mult = fft->mult;
int even, odd;
t_float gainer;
t_float threshie = x->threshie;
t_float ingain = 0;
t_float outgain, rescale;
t_float mymult;
fold(fft);
fold(fft2);
rdft(fft,1);
rdft(fft2,1);
/* changing algorithm for window flexibility */
if(autonorm){
ingain = 0;
for(i = 0; i < N; i+=2){
ingain += hypot(buffer1[i], buffer1[i+1]);
}
}
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
a1 = ( i == N2 ? *(buffer1+1) : *(buffer1+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(buffer1+odd) );
a2 = ( i == N2 ? *(buffer2+1) : *(buffer2+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(buffer2+odd) );
gainer = hypot(a2, b2);
if( gainer > threshie )
*(channel1+even) = hypot( a1, b1 ) * gainer;
*(channel1+odd) = -atan2( b1, a1 );
*(buffer1+even) = *(channel1+even) * cos( *(channel1+odd) );
if ( i != N2 )
*(buffer1+odd) = -(*(channel1+even)) * sin( *(channel1+odd) );
}
if(autonorm){
outgain = 0;
for(i = 0; i < N; i+=2){
outgain += hypot(buffer1[i], buffer1[i+1]);
}
if(ingain <= .0000001){
// post("gain emergency!");
rescale = 1.0;
} else {
rescale = ingain / outgain;
}
//post("ingain %f outgain %f rescale %f",ingain, outgain, rescale);
x->normult = mult * rescale;
} else {
x->normult = mult;
//post("mymult: %f", mymult);
}
rdft(fft, -1);
overlapadd(fft);
}
void do_ether(t_ether *x)
{
t_fftease *fft = x->fft;
t_fftease *fft2 = x->fft2;
int i;
int N2 = fft->N2;
float a1, b1, a2, b2;
int even, odd;
int invert = x->invert;
t_float threshMult = x->threshMult;
t_float *bufferOne = fft->buffer;
t_float *bufferTwo = fft2->buffer;
t_float *channelOne = fft->channel;
t_float *channelTwo = fft2->channel;
fold(fft);
fold(fft2);
rdft(fft,1);
rdft(fft2,1);
if (invert) {
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
a1 = ( i == N2 ? *(bufferOne+1) : *(bufferOne+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(bufferOne+odd) );
a2 = ( i == N2 ? *(bufferTwo+1) : *(bufferTwo+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(bufferTwo+odd) );
*(channelOne+even) = hypot( a1, b1 );
*(channelOne+odd) = -atan2( b1, a1 );
*(channelTwo+even) = hypot( a2, b2 );
*(channelTwo+odd) = -atan2( b2, a2 );
if ( *(channelOne+even) > *(channelTwo+even) * threshMult )
*(channelOne+even) = *(channelTwo+even);
if ( *(channelOne+odd) == 0. )
*(channelOne+odd) = *(channelTwo+odd);
}
}
else {
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
a1 = ( i == N2 ? *(bufferOne+1) : *(bufferOne+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(bufferOne+odd) );
a2 = ( i == N2 ? *(bufferTwo+1) : *(bufferTwo+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(bufferTwo+odd) );
*(channelOne+even) = hypot( a1, b1 );
*(channelOne+odd) = -atan2( b1, a1 );
*(channelTwo+even) = hypot( a2, b2 );
*(channelTwo+odd) = -atan2( b2, a2 );
if ( *(channelOne+even) < *(channelTwo+even) * threshMult )
*(channelOne+even) = *(channelTwo+even);
if ( *(channelOne+odd) == 0. )
*(channelOne+odd) = *(channelTwo+odd);
}
}
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
*(bufferOne+even) = *(channelOne+even) * cos( *(channelOne+odd) );
if ( i != N2 )
*(bufferOne+odd) = -(*(channelOne+even)) * sin( *(channelOne+odd) );
}
rdft(fft, -1);
overlapadd(fft);
}
//---------------------------------------------------------------------------
void tRisaPhaseVocoderDSP::ProcessCore_sse(int ch)
{
unsigned int framesize_d2 = FrameSize / 2;
float * analwork = AnalWork[ch];
float * synthwork = SynthWork[ch];
// 丸めモードを設定
SetRoundingModeToNearest_SSE();
// FFT を実行する
rdft(FrameSize, 1, analwork, FFTWorkIp, FFTWorkW); // Real DFT
analwork[1] = 0.0; // analwork[1] = nyquist freq. power (どっちみち使えないので0に)
__m128 exact_time_scale = _mm_load1_ps(&ExactTimeScale);
__m128 over_sampling_radian_v = _mm_load1_ps(&OverSamplingRadian);
if(FrequencyScale != 1.0)
{
// ここでは 4 複素数 (8実数) ごとに処理を行う。
__m128 over_sampling_radian_recp = _mm_load1_ps(&OverSamplingRadianRecp);
__m128 frequency_per_filter_band = _mm_load1_ps(&FrequencyPerFilterBand);
__m128 frequency_per_filter_band_recp = _mm_load1_ps(&FrequencyPerFilterBandRecp);
for(unsigned int i = 0; i < framesize_d2; i += 4)
{
// インターリーブ解除 + 直交座標系→極座標系
__m128 aw3120 = *(__m128*)(analwork + i*2 );
__m128 aw7654 = *(__m128*)(analwork + i*2 + 4);
__m128 re3210 = _mm_shuffle_ps(aw3120, aw7654, _MM_SHUFFLE(2,0,2,0));
__m128 im3210 = _mm_shuffle_ps(aw3120, aw7654, _MM_SHUFFLE(3,1,3,1));
__m128 mag = _mm_sqrt_ps(_mm_add_ps(_mm_mul_ps(re3210,re3210), _mm_mul_ps(im3210,im3210)));
__m128 ang = VFast_arctan2_F4_SSE(im3210, re3210);
// 前回の位相との差をとる
__m128 lastp = *(__m128*)(LastAnalPhase[ch] + i);
*(__m128*)(LastAnalPhase[ch] + i) = ang;
ang = _mm_sub_ps(lastp, ang);
// over sampling の影響を考慮する
__m128 i_3210;
i_3210 = _mm_cvtsi32_ss(i_3210, i);
i_3210 = _mm_shuffle_ps(i_3210, i_3210, _MM_SHUFFLE(0,0,0,0));
i_3210 = _mm_add_ps( i_3210, PM128(PFV_INIT) );
__m128 phase_shift = _mm_mul_ps(i_3210, over_sampling_radian_v);
ang = _mm_sub_ps( ang, phase_shift );
// unwrapping をする
ang = Wrap_Pi_F4_SSE(ang);
// -M_PI～+M_PIを-1.0～+1.0の変位に変換
ang = _mm_mul_ps( ang, over_sampling_radian_recp );
// tmp をフィルタバンド中央からの周波数の変位に変換し、
// それにフィルタバンドの中央周波数を加算する
__m128 freq = _mm_mul_ps( _mm_add_ps(ang, i_3210), frequency_per_filter_band );
// analwork に値を格納する
re3210 = mag;
im3210 = freq;
__m128 im10re10 = _mm_movelh_ps(re3210, im3210);
__m128 im32re32 = _mm_movehl_ps(im3210, re3210);
__m128 im1re1im0re0 = _mm_shuffle_ps(im10re10, im10re10, _MM_SHUFFLE(3,1,2,0));
__m128 im3re3im2re2 = _mm_shuffle_ps(im32re32, im32re32, _MM_SHUFFLE(3,1,2,0));
*(__m128*)(analwork + i*2 ) = im1re1im0re0;
*(__m128*)(analwork + i*2 + 4) = im3re3im2re2;
}
//------------------------------------------------
// 変換
//------------------------------------------------
// 周波数軸方向のリサンプリングを行う
float FrequencyScale_rcp = 1.0f / FrequencyScale;
for(unsigned int i = 0; i < framesize_d2; i ++)
{
// i に対応するインデックスを得る
float fi = i * FrequencyScale_rcp;
// floor(x) と floor(x) + 1 の間でバイリニア補間を行う
unsigned int index = static_cast<unsigned int>(fi); // floor
float frac = fi - index;
if(index + 1 < framesize_d2)
{
synthwork[i*2 ] =
analwork[index*2 ] +
frac * (analwork[index*2+2]-analwork[index*2 ]);
synthwork[i*2+1] =
FrequencyScale * (
analwork[index*2+1] +
frac * (analwork[index*2+3]-analwork[index*2+1]) );
}
else if(index < framesize_d2)
{
synthwork[i*2 ] = analwork[index*2 ];
synthwork[i*2+1] = analwork[index*2+1] * FrequencyScale;
}
else
{
synthwork[i*2 ] = 0.0;
synthwork[i*2+1] = 0.0;
}
}
//------------------------------------------------
// 合成
//------------------------------------------------
// 各フィルタバンドごとに変換
// 基本的には解析の逆変換である
for(unsigned int i = 0; i < framesize_d2; i += 4)
{
// インターリーブ解除
__m128 sw3120 = *(__m128*)(synthwork + i*2 );
__m128 sw7654 = *(__m128*)(synthwork + i*2 + 4);
__m128 mag = _mm_shuffle_ps(sw3120, sw7654, _MM_SHUFFLE(2,0,2,0));
__m128 freq = _mm_shuffle_ps(sw3120, sw7654, _MM_SHUFFLE(3,1,3,1));
// i+3 i+2 i+1 i+0 を準備
__m128 i_3210;
i_3210 = _mm_cvtsi32_ss(i_3210, i);
i_3210 = _mm_shuffle_ps(i_3210, i_3210, _MM_SHUFFLE(0,0,0,0));
i_3210 = _mm_add_ps(i_3210, PM128(PFV_INIT));
// 周波数から各フィルタバンドの中央周波数を減算し、
// フィルタバンドの中央周波数からの-1.0～+1.0の変位
// に変換する
__m128 ang = _mm_sub_ps(_mm_mul_ps(freq, frequency_per_filter_band_recp), i_3210);
// -1.0～+1.0の変位を-M_PI～+M_PIの位相に変換
ang = _mm_mul_ps( ang, over_sampling_radian_v );
// OverSampling による位相の補正
ang = _mm_add_ps( ang, _mm_mul_ps( i_3210, over_sampling_radian_v ) );
// TimeScale による位相の補正
ang = _mm_mul_ps( ang, exact_time_scale );
// 前回の位相と加算する
// ここでも虚数部の符号が逆になるので注意
ang = _mm_sub_ps( *(__m128*)(LastSynthPhase[ch] + i), ang );
*(__m128*)(LastSynthPhase[ch] + i) = ang;
// 極座標系→直交座標系
__m128 sin, cos;
VFast_sincos_F4_SSE(ang, sin, cos);
__m128 re3210 = _mm_mul_ps( mag, cos );
__m128 im3210 = _mm_mul_ps( mag, sin );
// インターリーブ
__m128 im10re10 = _mm_movelh_ps(re3210, im3210);
__m128 im32re32 = _mm_movehl_ps(im3210, re3210);
__m128 im1re1im0re0 = _mm_shuffle_ps(im10re10, im10re10, _MM_SHUFFLE(3,1,2,0));
__m128 im3re3im2re2 = _mm_shuffle_ps(im32re32, im32re32, _MM_SHUFFLE(3,1,2,0));
*(__m128*)(synthwork + i*2 ) = im1re1im0re0;
*(__m128*)(synthwork + i*2 + 4) = im3re3im2re2;
}
}
else
{
// 周波数軸方向にシフトがない場合
// ここでも 4 複素数 (8実数) ごとに処理を行う。
for(unsigned int i = 0; i < framesize_d2; i += 4)
{
// インターリーブ解除 + 直交座標系→極座標系
__m128 aw3120 = *(__m128*)(analwork + i*2 );
__m128 aw7654 = *(__m128*)(analwork + i*2 + 4);
__m128 re3210 = _mm_shuffle_ps(aw3120, aw7654, _MM_SHUFFLE(2,0,2,0));
__m128 im3210 = _mm_shuffle_ps(aw3120, aw7654, _MM_SHUFFLE(3,1,3,1));
__m128 mag = _mm_sqrt_ps( _mm_add_ps(_mm_mul_ps(re3210,re3210), _mm_mul_ps(im3210,im3210)) );
__m128 ang = VFast_arctan2_F4_SSE(im3210, re3210);
// 前回の位相との差をとる
__m128 lastp = *(__m128*)(LastAnalPhase[ch] + i);
*(__m128*)(LastAnalPhase[ch] + i) = ang;
ang = _mm_sub_ps( lastp, ang );
// over sampling の影響を考慮する
__m128 i_3210;
i_3210 = _mm_cvtsi32_ss(i_3210, i);
i_3210 = _mm_shuffle_ps(i_3210, i_3210, _MM_SHUFFLE(0,0,0,0));
i_3210 = _mm_add_ps( i_3210, PM128(PFV_INIT) );
__m128 phase_shift = _mm_mul_ps( i_3210, over_sampling_radian_v );
ang = _mm_sub_ps( ang, phase_shift );
// unwrapping をする
ang = Wrap_Pi_F4_SSE(ang);
// OverSampling による位相の補正
ang = _mm_add_ps( ang, phase_shift );
// TimeScale による位相の補正
ang = _mm_mul_ps( ang, exact_time_scale );
// 前回の位相と加算する
// ここでも虚数部の符号が逆になるので注意
ang = _mm_sub_ps( *(__m128*)(LastSynthPhase[ch] + i), ang );
*(__m128*)(LastSynthPhase[ch] + i) = ang;
// 極座標系→直交座標系
__m128 sin, cos;
VFast_sincos_F4_SSE(ang, sin, cos);
re3210 = _mm_mul_ps( mag, cos );
im3210 = _mm_mul_ps( mag, sin );
// インターリーブ
__m128 im10re10 = _mm_movelh_ps(re3210, im3210);
__m128 im32re32 = _mm_movehl_ps(im3210, re3210);
__m128 im1re1im0re0 = _mm_shuffle_ps(im10re10, im10re10, _MM_SHUFFLE(3,1,2,0));
__m128 im3re3im2re2 = _mm_shuffle_ps(im32re32, im32re32, _MM_SHUFFLE(3,1,2,0));
*(__m128*)(synthwork + i*2 ) = im1re1im0re0;
*(__m128*)(synthwork + i*2 + 4) = im3re3im2re2;
}
}
// FFT を実行する
synthwork[1] = 0.0; // synthwork[1] = nyquist freq. power (どっちみち使えないので0に)
rdft_sse(FrameSize, -1, synthwork, FFTWorkIp, FFTWorkW); // Inverse Real DFT
}
//---------------------------------------------------------------------------
void tRisaPhaseVocoderDSP::ProcessCore(int ch)
{
unsigned int framesize_d2 = FrameSize / 2;
float * analwork = AnalWork[ch];
float * synthwork = SynthWork[ch];
// FFT を実行する
rdft(FrameSize, 1, analwork, FFTWorkIp, FFTWorkW); // Real DFT
analwork[1] = 0.0; // analwork[1] = nyquist freq. power (どっちみち使えないので0に)
if(FrequencyScale != 1.0)
{
// 各フィルタバンドごとに変換
//-- 各フィルタバンドごとの音量と周波数を求める。
//-- FFT を実行すると各フィルタバンドごとの値が出てくるが、
//-- フィルタバンドというバンドパスフィルタの幅の中で
//-- 周波数のピークが本当はどこにあるのかは、前回計算した
//-- 位相との差をとってみないとわからない。
for(unsigned int i = 0; i < framesize_d2; i ++)
{
// 直交座標系→極座標系
float re = analwork[i*2 ];
float im = analwork[i*2+1];
float mag = sqrt(re*re + im*im); // mag = √(re^2+im^2)
float ang = VFast_arctan2(im, re); // ang = atan(im/re)
// 前回の位相との差をとる
// --注意: ここで使用しているFFTパッケージは、
// -- ソース先頭の参考資料などで示しているFFTと
// -- 出力される複素数の虚数部の符号が逆なので
// -- (共役がでてくるので)注意が必要。ここでも符号を
// -- 逆の物として扱う。
float tmp = LastAnalPhase[ch][i] - ang;
LastAnalPhase[ch][i] = ang; // 今回の値を保存
// over sampling の影響を考慮する
// -- 通常、FrameSize で FFT の１周期であるところを、
// -- 精度を補うため、OverSampling 倍の周期で演算をしている。
// -- そのために生じる位相のずれを修正する。
tmp -= i * OverSamplingRadian;
// unwrapping をする
// -- tmp が -M_PI ～ +M_PI の範囲に収まるようにする
tmp = WrapPi_F1(tmp);
// -M_PI～+M_PIを-1.0～+1.0の変位に変換
tmp = tmp * OverSamplingRadianRecp;
// tmp をフィルタバンド中央からの周波数の変位に変換し、
// それにフィルタバンドの中央周波数を加算する
// -- i * FrequencyPerFilterBand はフィルタバンドの中央周波数を
// -- 表し、tmp * FrequencyPerFilterBand は フィルタバンド中央から
// -- の周波数の変位を表す。これらをあわせた物が、そのフィルタ
// -- バンド内での「真」の周波数である。
float freq = (i + tmp) *FrequencyPerFilterBand;
// analwork に値を格納する
analwork[i*2 ] = mag;
analwork[i*2+1] = freq;
}
//------------------------------------------------
// 変換
//------------------------------------------------
// 周波数軸方向のリサンプリングを行う
float FrequencyScale_rcp = 1.0f / FrequencyScale;
for(unsigned int i = 0; i < framesize_d2; i ++)
{
// i に対応するインデックスを得る
float fi = i * FrequencyScale_rcp;
// floor(x) と floor(x) + 1 の間でバイリニア補間を行う
unsigned int index = static_cast<unsigned int>(fi); // floor
float frac = fi - index;
if(index + 1 < framesize_d2)
{
synthwork[i*2 ] =
analwork[index*2 ] +
frac * (analwork[index*2+2]-analwork[index*2 ]);
synthwork[i*2+1] =
FrequencyScale * (
analwork[index*2+1] +
frac * (analwork[index*2+3]-analwork[index*2+1]) );
}
else if(index < framesize_d2)
{
synthwork[i*2 ] = analwork[index*2 ];
synthwork[i*2+1] = analwork[index*2+1] * FrequencyScale;
}
else
{
synthwork[i*2 ] = 0.0;
synthwork[i*2+1] = 0.0;
}
}
//------------------------------------------------
// 合成
//------------------------------------------------
// 各フィルタバンドごとに変換
// 基本的には解析の逆変換である
for(unsigned int i = 0; i < framesize_d2; i ++)
{
float mag = synthwork[i*2 ];
float freq = synthwork[i*2+1];
// 周波数から各フィルタバンドの中央周波数を減算し、
// フィルタバンドの中央周波数からの-1.0～+1.0の変位
// に変換する
float tmp = freq * FrequencyPerFilterBandRecp - (float)i;
// -1.0～+1.0の変位を-M_PI～+M_PIの位相に変換
tmp = tmp * OverSamplingRadian;
// OverSampling による位相の補正
tmp += i * OverSamplingRadian;
// TimeScale による位相の補正
// TimeScale で出力が時間軸方向にのびれば(あるいは縮めば)、
// 位相の差分もそれに伴ってのびる(縮む)
tmp *= ExactTimeScale;
// 前回の位相と加算する
// ここでも虚数部の符号が逆になるので注意
LastSynthPhase[ch][i] -= tmp;
float ang = LastSynthPhase[ch][i];
// 極座標系→直交座標系
float c, s;
VFast_sincos(ang, s, c);
synthwork[i*2 ] = mag * c;
synthwork[i*2+1] = mag * s;
}
}
else
{
// 周波数軸方向にシフトがない場合
// 各フィルタバンドごとに変換
//-- 各フィルタバンドごとの音量と周波数を求める。
//-- FFT を実行すると各フィルタバンドごとの値が出てくるが、
//-- フィルタバンドというバンドパスフィルタの幅の中で
//-- 周波数のピークが本当はどこにあるのかは、前回計算した
//-- 位相との差をとってみないとわからない。
for(unsigned int i = 0; i < framesize_d2; i ++)
{
// 直交座標系→極座標系
float re = analwork[i*2 ];
float im = analwork[i*2+1];
float mag = sqrt(re*re + im*im); // mag = √(re^2+im^2)
float ang = VFast_arctan2(im, re); // ang = atan(im/re)
// 前回の位相との差をとる
// --注意: ここで使用しているFFTパッケージは、
// -- ソース先頭の参考資料などで示しているFFTと
// -- 出力される複素数の虚数部の符号が逆なので
// -- (共役がでてくるので)注意が必要。ここでも符号を
// -- 逆の物として扱う。
float tmp = LastAnalPhase[ch][i] - ang;
LastAnalPhase[ch][i] = ang; // 今回の値を保存
// phase shift
float phase_shift = i * OverSamplingRadian;
// over sampling の影響を考慮する
// -- 通常、FrameSize で FFT の１周期であるところを、
// -- 精度を補うため、OverSampling 倍の周期で演算をしている。
// -- そのために生じる位相のずれを修正する。
tmp -= phase_shift;
// unwrapping をする
// -- tmp が -M_PI ～ +M_PI の範囲に収まるようにする
tmp = WrapPi_F1(tmp);
//--
// OverSampling による位相の補正
tmp += phase_shift;
// TimeScale による位相の補正
// TimeScale で出力が時間軸方向にのびれば(あるいは縮めば)、
// 位相の差分もそれに伴ってのびる(縮む)
tmp *= ExactTimeScale;
// 前回の位相と加算する
// ここでも虚数部の符号が逆になるので注意
LastSynthPhase[ch][i] -= tmp;
ang = LastSynthPhase[ch][i];
// 極座標系→直交座標系
float c, s;
VFast_sincos(ang, s, c);
synthwork[i*2 ] = mag * c;
synthwork[i*2+1] = mag * s;
}
}
// FFT を実行する
synthwork[1] = 0.0; // synthwork[1] = nyquist freq. power (どっちみち使えないので0に)
rdft(FrameSize, -1, SynthWork[ch], FFTWorkIp, FFTWorkW); // Inverse Real DFT
}
t_int *bthresher_perform(t_int *w)
{
float sample, outsamp ;
int i, j, on;
t_bthresher *x = (t_bthresher *) (w[1]);
float *in = (t_float *)(w[2]);
float *inthresh = (t_float *)(w[3]);
float *damping = (t_float *)(w[4]);
float *out = (t_float *)(w[5]);
t_int n = w[6];
int *bitshuffle = x->bitshuffle;
float *trigland = x->trigland;
float mult = x->mult;
int in_count = x->in_count;
int R = x->R;
int N = x->N;
int N2 = x->N2;
int D = x->D;
int Nw = x->Nw;
float *Wanal = x->Wanal;
float *Wsyn = x->Wsyn;
float *damping_factor = x->damping_factor;
float *move_threshold = x->move_threshold;
float *input = x->input;
float *output = x->output;
float *buffer = x->buffer;
float *channel = x->channel;
float *composite_frame = x->composite_frame;
int max_hold_frames = x->max_hold_frames;
int *frames_left = x->frames_left;
float thresh_scalar = x->thresh_scalar;
float damp_scalar = x->damp_scalar;
short inf_hold = x->inf_hold;
if( x->mute ) {
for( j = 0; j < D; j++) {
*out++ = 0.0 ;
}
} else if ( x->bypass ) {
for( j = 0; j < D; j++) {
*out++ = *in++ * 0.5;
}
} else {
#if MSP
if( x->thresh_connected ) {
thresh_scalar = *inthresh++;
}
if( x->damping_connected ) {
damp_scalar = *damping++;
}
#endif
#if PD
thresh_scalar = *inthresh++;
damp_scalar = *damping++;
#endif
in_count += D;
for ( j = 0 ; j < Nw - D ; j++ )
input[j] = input[j+D];
for ( j = Nw-D; j < Nw; j++ ) {
input[j] = *in++;
}
fold( input, Wanal, Nw, buffer, N, in_count );
rdft( N, 1, buffer, bitshuffle, trigland );
convert( buffer, channel, N2, x->c_lastphase_in, x->c_fundamental, x->c_factor_in );
if( x->first_frame ){
for ( i = 0; i < N+2; i++ ){
composite_frame[i] = channel[i];
x->frames_left[i] = max_hold_frames;
}
x->first_frame = 0;
} else {
if( thresh_scalar < .999 || thresh_scalar > 1.001 || damp_scalar < .999 || damp_scalar > 1.001 ) {
for(i = 0, j = 0; i < N+2; i += 2, j++ ){
if( fabs( composite_frame[i] - channel[i] ) > move_threshold[j] * thresh_scalar|| frames_left[j] <= 0 ){
composite_frame[i] = channel[i];
composite_frame[i+1] = channel[i+1];
frames_left[j] = max_hold_frames;
} else {
if(!inf_hold){
--(frames_left[j]);
}
composite_frame[i] *= damping_factor[j] * damp_scalar;
}
}
} else {
for( i = 0, j = 0; i < N+2; i += 2, j++ ){
if( fabs( composite_frame[i] - channel[i] ) > move_threshold[j] || frames_left[j] <= 0 ){
composite_frame[i] = channel[i];
composite_frame[i+1] = channel[i+1];
frames_left[j] = max_hold_frames;
} else {
if(!inf_hold){
--(frames_left[j]);
}
composite_frame[i] *= damping_factor[j];
}
}
}
}
unconvert(x->composite_frame, buffer, N2, x->c_lastphase_out, x->c_fundamental, x->c_factor_out);
rdft(N, -1, buffer, bitshuffle, trigland);
overlapadd(buffer, N, Wsyn, output, Nw, in_count);
for ( j = 0; j < D; j++ )
*out++ = output[j] * mult;
for ( j = 0; j < Nw - D; j++ )
output[j] = output[j+D];
for ( j = Nw - D; j < Nw; j++ )
output[j] = 0.;
}
x->in_count = in_count % Nw;
x->thresh_scalar = thresh_scalar;
x->damp_scalar = damp_scalar;
return (w+7);
}
t_int *ether_perform(t_int *w)
{
int i,j,
inCount,
R,
N,
N2,
D,
Nw,
invert = 1,
even, odd,
*bitshuffle;
float maxamp,
threshMult = 1.,
mult,
a1, b1,
a2, b2,
*inputOne,
*inputTwo,
*bufferOne,
*bufferTwo,
*output,
*Wanal,
*Wsyn,
*channelOne,
*channelTwo,
*trigland;
/* get our inlets and outlets */
t_ether *x = (t_ether *) (w[1]);
t_float *inOne = (t_float *)(w[2]);
t_float *inTwo = (t_float *)(w[3]);
t_float *vec_threshMult = (t_float *)(w[4]);
t_float *out = (t_float *)(w[5]);
t_int n = w[6];
short *connected = x->connected;
/* dereference structure */
inputOne = x->inputOne;
inputTwo = x->inputTwo;
bufferOne = x->bufferOne;
bufferTwo = x->bufferTwo;
inCount = x->inCount;
R = x->R;
N = x->N;
N2 = x->N2;
D = x->D;
Nw = x->Nw;
Wanal = x->Wanal;
Wsyn = x->Wsyn;
output = x->output;
channelOne = x->channelOne;
channelTwo = x->channelTwo;
bitshuffle = x->bitshuffle;
trigland = x->trigland;
mult = x->mult;
invert = x->invert;
if(connected[2]){
threshMult = *vec_threshMult;
}
else if ( x->threshMult != 0. ){
threshMult = x->threshMult;
}
else {
threshMult = 1.0;
}
if(x->mute){
while(n--)
*out++ = 0.0;
return w+7;
}
/* fill our retaining buffers */
inCount += D;
for ( j = 0 ; j < Nw - D ; j++ ) {
inputOne[j] = inputOne[j+D];
inputTwo[j] = inputTwo[j+D];
}
for ( j = Nw - D; j < Nw; j++ ) {
inputOne[j] = *inOne++;
inputTwo[j] = *inTwo++;
}
/* apply hamming window and fold our window buffer into the fft buffer */
fold( inputOne, Wanal, Nw, bufferOne, N, inCount );
fold( inputTwo, Wanal, Nw, bufferTwo, N, inCount );
/* do an fft */
rdft( N, 1, bufferOne, bitshuffle, trigland );
rdft( N, 1, bufferTwo, bitshuffle, trigland );
/* use slow fft */
/* use redundant coding for speed, even though moving the invert variable
comparison outside of the for loop will give us only a minimal performance
increase (hypot and atan2 are the most intensive portions of this code).
consider adding a table lookup for atan2 instead.
*/
if (invert) {
/* convert to polar coordinates from complex values */
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
a1 = ( i == N2 ? *(bufferOne+1) : *(bufferOne+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(bufferOne+odd) );
a2 = ( i == N2 ? *(bufferTwo+1) : *(bufferTwo+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(bufferTwo+odd) );
*(channelOne+even) = hypot( a1, b1 );
*(channelOne+odd) = -atan2( b1, a1 );
*(channelTwo+even) = hypot( a2, b2 );
*(channelTwo+odd) = -atan2( b2, a2 );
/* use simple threshold for inverse compositing */
if ( *(channelOne+even) > *(channelTwo+even) * threshMult )
*(channelOne+even) = *(channelTwo+even);
if ( *(channelOne+odd) == 0. )
*(channelOne+odd) = *(channelTwo+odd);
}
}
else {
/* convert to polar coordinates from complex values */
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
a1 = ( i == N2 ? *(bufferOne+1) : *(bufferOne+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(bufferOne+odd) );
a2 = ( i == N2 ? *(bufferTwo+1) : *(bufferTwo+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(bufferTwo+odd) );
*(channelOne+even) = hypot( a1, b1 );
*(channelOne+odd) = -atan2( b1, a1 );
*(channelTwo+even) = hypot( a2, b2 );
*(channelTwo+odd) = -atan2( b2, a2 );
/* use simple threshold for compositing */
if ( *(channelOne+even) < *(channelTwo+even) * threshMult )
*(channelOne+even) = *(channelTwo+even);
if ( *(channelOne+odd) == 0. )
*(channelOne+odd) = *(channelTwo+odd);
}
}
/* convert back to complex form, read for the inverse fft */
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
*(bufferOne+even) = *(channelOne+even) * cos( *(channelOne+odd) );
if ( i != N2 )
*(bufferOne+odd) = -(*(channelOne+even)) * sin( *(channelOne+odd) );
}
/* do an inverse fft */
rdft( N, -1, bufferOne, bitshuffle, trigland );
/* dewindow our result */
overlapadd( bufferOne, N, Wsyn, output, Nw, inCount);
/* set our output and adjust our retaining output buffer */
for ( j = 0; j < D; j++ )
*out++ = output[j] * mult;
for ( j = 0; j < Nw - D; j++ )
output[j] = output[j+D];
for ( j = Nw - D; j < Nw; j++ )
output[j] = 0.;
/* restore state variables */
x->inCount = inCount % Nw;
return (w+7);
}
int PSDCalculator::calculatePowerSpectrum(
double *input, int inputLen,
double *output, int outputLen,
bool removeMean, bool interpolateHoles,
bool average, int averageLen,
bool apodize, ApodizeFunction apodizeFxn, double gaussianSigma,
PSDType outputType, double inputSamplingFreq) {
if (outputLen != calculateOutputVectorLength(inputLen, average, averageLen)) {
KstDebug::self()->log(QObject::tr("in PSDCalculator::calculatePowerSpectrum: received output array with wrong length."), KstDebug::Error);
return -1;
}
if (outputLen != _prevOutputLen) {
delete[] _a;
delete[] _w;
_awLen = outputLen*2;
_prevOutputLen = outputLen;
_a = new double[_awLen];
_w = new double[_awLen];
updateWindowFxn(apodizeFxn, gaussianSigma);
}
if ( (_prevApodizeFxn != apodizeFxn) || (_prevGaussianSigma != gaussianSigma) ) {
updateWindowFxn(apodizeFxn, gaussianSigma);
}
int currentCopyLen, nsamples = 0;
int i_samp, i_subset, ioffset;
memset(output, 0, sizeof(double) * outputLen);
bool done = false;
for (i_subset = 0; !done; i_subset++) {
//
// overlapping average => i_subset*outputLen...
//
ioffset = i_subset * outputLen;
//
// only zero pad if we really have to.
// It is better to adjust the last chunk's overlap...
//
if (ioffset + _awLen*5/4 < inputLen) {
currentCopyLen = _awLen; // will copy a complete window.
} else if (_awLen<inputLen) { // count the last one from the end.
ioffset = inputLen-_awLen - 1;
currentCopyLen = _awLen; // will copy a complete window.
done = true;
} else {
currentCopyLen = inputLen - ioffset; // will copy a partial window.
memset(&_a[currentCopyLen], 0, sizeof(double)*(_awLen - currentCopyLen)); // zero the leftovers.
done = true;
}
double mean = 0.0;
if (removeMean) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
mean += input[i_samp + ioffset];
}
mean /= (double)currentCopyLen;
}
//
// apply the PSD options (removeMean, apodize, etc.)
// separate cases for speed- although this shouldn't really matter -
// the rdft should be the most time consuming step by far for any large data set...
//
if (removeMean && apodize && interpolateHoles) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = (kstInterpolateNoHoles(input, inputLen, i_samp + ioffset, inputLen) - mean)*_w[i_samp];
}
} else if (removeMean && apodize) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = (input[i_samp + ioffset] - mean)*_w[i_samp];
}
} else if (removeMean && interpolateHoles) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = kstInterpolateNoHoles(input, inputLen, i_samp + ioffset, inputLen) - mean;
}
} else if (apodize && interpolateHoles) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = kstInterpolateNoHoles(input, inputLen, i_samp + ioffset, inputLen)*_w[i_samp];
}
} else if (removeMean) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = input[i_samp + ioffset] - mean;
}
} else if (apodize) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = input[i_samp + ioffset]*_w[i_samp];
}
} else if (interpolateHoles) {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = kstInterpolateNoHoles(input, inputLen, i_samp + ioffset, inputLen);
}
} else {
for (i_samp = 0; i_samp < currentCopyLen; i_samp++) {
_a[i_samp] = input[i_samp + ioffset];
}
}
nsamples += currentCopyLen;
//
// real discrete fourier transorm on _a.
//
rdft(_awLen, 1, _a);
output[0] += _a[0] * _a[0];
output[outputLen-1] += _a[1] * _a[1];
for (i_samp = 1; i_samp < outputLen - 1; i_samp++) {
output[i_samp] += cabs2(_a[i_samp * 2], _a[i_samp * 2 + 1]);
}
}
double frequencyStep = 2.0*(double)inputSamplingFreq/(double)nsamples;
double norm = 2.0/(double)nsamples*2.0/(double)nsamples;
switch (outputType) {
default:
case PSDAmplitudeSpectralDensity: // amplitude spectral density (default) [V/Hz^1/2]
norm /= frequencyStep;
for (i_samp = 0; i_samp < outputLen; i_samp++) {
output[i_samp] = sqrt(output[i_samp]*norm);
}
break;
case PSDPowerSpectralDensity: // power spectral density [V^2/Hz]
norm /= frequencyStep;
for (i_samp = 0; i_samp < outputLen; i_samp++) {
output[i_samp] *= norm;
}
break;
case PSDAmplitudeSpectrum: // amplitude spectrum [V]
for (i_samp = 0; i_samp < outputLen; i_samp++) {
output[i_samp] = sqrt(output[i_samp]*norm);
}
break;
case PSDPowerSpectrum: // power spectrum [V^2]
for (i_samp = 0; i_samp < outputLen; i_samp++) {
output[i_samp] *= norm;
}
break;
}
return 0;
}
t_int *pvcompand_perform(t_int *w)
{
float sample, outsamp ;
int i,j;
float maxamp ;
float fmult;
float cutoff;
float avr, new_avr, rescale;
t_pvcompand *x = (t_pvcompand *) (w[1]);
t_float *in = (t_float *)(w[2]);
t_float *in2 = (t_float *)(w[3]);
t_float *out = (t_float *)(w[4]);
int n = (int)(w[5]);
int inCount = x->inCount;
float *Wanal = x->Wanal;
float *Wsyn = x->Wsyn;
float *input = x->input;
float *Hwin = x->Hwin;
float *buffer = x->buffer;
float *channel = x->channel;
float *output = x->output;
int D = x->D;
int I = D;
int R = x->R;
int Nw = x->Nw;
int N = x->N ;
int N2 = x-> N2;
int Nw2 = x->Nw2;
int *bitshuffle = x->bitshuffle;
float *trigland = x->trigland;
float mult = x->mult;
int count = x->count;
float *atten = x->atten;
float *curthresh = x->curthresh;
float *thresh = x->thresh;
float max_atten = x->max_atten;
if( x->mute ){
while( n-- ){
*out++ = 0.0;
}
return (w+6);
}
if( x->bypass ){
while( n-- ){
*out++ = *in++ * 0.5; // gain compensation
}
return (w+6);
}
#if MSP
if( x->connected[1] ){
max_atten = *in2++ ;
if(max_atten != x->last_max_atten) {
x->last_max_atten = x->max_atten = max_atten;
update_thresholds(x);
}
}
#endif
#if PD
max_atten = *in2++ ;
if(max_atten != x->last_max_atten) {
x->last_max_atten = x->max_atten = max_atten;
update_thresholds(x);
}
#endif
inCount += D;
for ( j = 0 ; j < Nw - D ; j++ ){
input[j] = input[j+D];
}
for ( j = Nw - D; j < Nw; j++ ) {
input[j] = *in++;
}
fold( input, Wanal, Nw, buffer, N, inCount );
rdft( N, 1, buffer, bitshuffle, trigland );
leanconvert(buffer, channel, N2);
maxamp = 0.;
avr = 0;
for( i = 0; i < N; i+= 2 ){
avr += channel[i];
if( maxamp < channel[i] ){
maxamp = channel[i] ;
}
}
if(count <= 1){
// post("count too low!");
count = 1;
}
for( i = 0; i < count; i++ ){
curthresh[i] = thresh[i]*maxamp ;
}
cutoff = curthresh[count-1];
new_avr = 0;
for( i = 0; i < N; i += 2){
if( channel[i] > cutoff ){
j = count-1;
while( channel[i] > curthresh[j] ){
j--;
if( j < 0 ){
j = 0;
break;
}
}
channel[i] *= atten[j];
}
new_avr += channel[i] ;
}
leanunconvert( channel,buffer, N2);
rdft( N, -1, buffer, bitshuffle, trigland );
overlapadd( buffer, N, Wsyn, output, Nw, inCount);
if( x->norml ) {
if( new_avr <= 0 ){
new_avr = .0001;
}
rescale = avr / new_avr ;
mult *= rescale ;
} else {
mult *= pvcompand_ampdb( max_atten * -.5); ;
}
for ( j = 0; j < D; j++ ){
*out++ = output[j] * mult;
}
for ( j = 0; j < Nw - D; j++ )
output[j] = output[j+D];
for ( j = Nw - D; j < Nw; j++ )
output[j] = 0.;
/* restore state variables */
x->inCount = inCount % Nw;
return (w+6);
}
t_int *thresher_perform(t_int *w)
{
float sample, outsamp ;
int i,j;
t_thresher *x = (t_thresher *) (w[1]);
float *in = (t_float *)(w[2]);
float *inthresh = (t_float *)(w[3]);
float *damping = (t_float *)(w[4]);
float *out = (t_float *)(w[5]);
int n = (int)(w[6]);
float *input = x->input;
float *output = x->output;
float *buffer = x->buffer;
float *Wanal = x->Wanal;
float *Wsyn = x->Wsyn;
float *channel = x->channel;
float damping_factor = x->damping_factor;
int max_hold_frames = x->max_hold_frames;
int *frames_left = x->frames_left;
float *composite_frame = x->composite_frame;
float *c_lastphase_in = x->c_lastphase_in;
float *c_lastphase_out = x->c_lastphase_out;
float c_fundamental = x->c_fundamental;
float c_factor_in = x->c_factor_in;
float c_factor_out = x->c_factor_out;
int *bitshuffle = x->bitshuffle;
float *trigland = x->trigland;
float mult = x->mult;
int in_count = x->in_count;
int R = x->R;
int N = x->N;
int N2 = x->N2;
int D = x->D;
int Nw = x->Nw;
float move_threshold = x->move_threshold;
if( x->mute ) {
for( j = 0; j < D; j++) {
*out++ = 0.0 ;
}
return (w+7);
}
if ( x->bypass ) {
for( j = 0; j < D; j++) {
*out++ = *in++ ;
}
return (w+7);
}
if( x->thresh_connected ) {
move_threshold = *inthresh ;
}
if( x->damping_connected ) {
damping_factor = *damping ;
}
in_count += D;
for ( j = 0 ; j < Nw - D ; j++ )
input[j] = input[j+D];
for ( j = Nw - D; j < Nw; j++ ) {
input[j] = *in++;
}
fold( input, Wanal, Nw, buffer, N, in_count );
rdft( N, 1, buffer, bitshuffle, trigland );
convert( buffer, channel, N2, c_lastphase_in, c_fundamental, c_factor_in );
if( x->first_frame ){
for ( i = 0; i < N+2; i++ ){
composite_frame[i] = channel[i];
frames_left[i] = max_hold_frames;
}
x->first_frame = 0;
} else {
for( i = 0; i < N+2; i += 2 ){
if(fabs( composite_frame[i] - channel[i] ) > move_threshold ||
frames_left[i] <= 0 ){
composite_frame[i] = channel[i];
composite_frame[i+1] = channel[i+1];
frames_left[i] = max_hold_frames;
} else {
--(frames_left[i]);
composite_frame[i] *= damping_factor;
}
}
}
unconvert( composite_frame, buffer, N2, c_lastphase_out, c_fundamental, c_factor_out );
rdft( N, -1, buffer, bitshuffle, trigland );
overlapadd( buffer, N, Wsyn, output, Nw, in_count );
for ( j = 0; j < D; j++ )
*out++ = output[j] * mult;
for ( j = 0; j < Nw - D; j++ )
output[j] = output[j+D];
for ( j = Nw - D; j < Nw; j++ )
output[j] = 0.;
x->in_count = in_count;
x->damping_factor = damping_factor;
return (w+7);
}
t_int *crossx_perform(t_int *w)
{
int i, j;
float a1, a2, b1, b2;
int even, odd;
int amp, freq;
float gainer, threshie;
float ingain = 0;
float outgain, rescale;
float mymult;
t_crossx *x = (t_crossx *) (w[1]);
t_float *in1 = (t_float *)(w[2]);
t_float *in2 = (t_float *)(w[3]);
t_float *in3 = (t_float *)(w[4]);
t_float *out = (t_float *)(w[5]);
t_int n = w[6];
/* dereference struncture */
float *input1 = x->input1;
float *input2 = x->input2;
float *buffer1 = x->buffer1;
float *buffer2 = x->buffer2;
int inCount = x->inCount;
int R = x->R;
int N = x->N;
int N2 = x->N2;
int D = x->D;
int Nw = x->Nw;
float *Wanal = x->Wanal;
float *Wsyn = x->Wsyn;
float *output = x->output;
float *channel1 = x->channel1;
float *channel2 = x->channel2;
float *last_channel = x->last_channel;
int *bitshuffle = x->bitshuffle;
float *trigland = x->trigland;
float mult = x->mult;
float *c_lastphase_in1 = x->c_lastphase_in1;
float *c_lastphase_in2 = x->c_lastphase_in2;
float *c_lastphase_out = x->c_lastphase_out;
float c_fundamental = x->c_fundamental;
float c_factor_in = x->c_factor_in;
float c_factor_out = x->c_factor_out;
short autonorm = x->autonorm;
if(x->mute){
while(n--){
*out++ = 0;
}
return w+7;
}
if( x->thresh_connected ){
threshie = *in3++;
} else {
threshie = x->threshie;
}
inCount += D;
for ( j = 0 ; j < Nw - D ; j++ ){
input1[j] = input1[j+D];
input2[j] = input2[j+D];
}
for ( j = Nw - D; j < Nw; j++ ) {
input1[j] = *in1++;
input2[j] = *in2++;
}
fold( input1, Wanal, Nw, buffer1, N, inCount );
fold( input2, Wanal, Nw, buffer2, N, inCount );
rdft( N, 1, buffer1, bitshuffle, trigland );
rdft( N, 1, buffer2, bitshuffle, trigland );
/* changing algorithm for window flexibility */
if(autonorm){
ingain = 0;
for(i = 0; i < N; i+=2){
ingain += hypot(buffer1[i], buffer1[i+1]);
}
}
for ( i = 0; i <= N2; i++ ) {
odd = ( even = i<<1 ) + 1;
a1 = ( i == N2 ? *(buffer1+1) : *(buffer1+even) );
b1 = ( i == 0 || i == N2 ? 0. : *(buffer1+odd) );
a2 = ( i == N2 ? *(buffer2+1) : *(buffer2+even) );
b2 = ( i == 0 || i == N2 ? 0. : *(buffer2+odd) );
gainer = hypot(a2, b2);
if( gainer > threshie )
*(channel1+even) = hypot( a1, b1 ) * gainer;
*(channel1+odd) = -atan2( b1, a1 );
*(buffer1+even) = *(channel1+even) * cos( *(channel1+odd) );
if ( i != N2 )
*(buffer1+odd) = -(*(channel1+even)) * sin( *(channel1+odd) );
}
if(autonorm){
outgain = 0;
for(i = 0; i < N; i+=2){
outgain += hypot(buffer1[i], buffer1[i+1]);
}
if(ingain <= .0000001){
// post("gain emergency!");
rescale = 1.0;
} else {
rescale = ingain / outgain;
}
// post("ingain %f outgain %f rescale %f",ingain, outgain, rescale);
mymult = mult * rescale;
} else {
mymult = mult;
}
rdft( N, -1, buffer1, bitshuffle, trigland );
overlapadd( buffer1, N, Wsyn, output, Nw, inCount);
for ( j = 0; j < D; j++ )
*out++ = output[j] * mymult;
for ( j = 0; j < Nw - D; j++ )
output[j] = output[j+D];
for ( j = Nw - D; j < Nw; j++ )
output[j] = 0.;
/* restore state variables */
x->inCount = inCount % Nw;
return (w+7);
}
int WBPMDetect1::PutSamples(short *pSamples, unsigned int nSampleNum)
{
// create samples array
unsigned int i;
if(c_nChannel == 2)
{
// convert to mono
for(i = 0; i < nSampleNum; i++)
c_Samples[i] = ((BPM_DETECT1_REAL) pSamples[2 * i] + (BPM_DETECT1_REAL) pSamples[2 * i + 1]) * c_pflWindow[i] / 2.0;
}
else
{
for(i = 0; i < nSampleNum; i++)
c_Samples[i] = (BPM_DETECT1_REAL) pSamples[i] * c_pflWindow[i];
}
// make fft
rdft(BPM_DETECT_FFT_POINTS1, 1, c_Samples, c_pnIp, c_pflw);
BPM_DETECT1_REAL amp;
BPM_DETECT1_REAL re = c_Samples[1] ;
BPM_DETECT1_REAL im = 0;
amp = sqrt(re * re + im * im) / c_sqrtFFTPoints;
c_Amplitudes[BPM_DETECT_FFT_POINTS1 / 2 - 1] = amp * amp;
for(i = 1; i < BPM_DETECT_FFT_POINTS1 / 2; i++)
{
re = c_Samples[i * 2] ;
im = c_Samples[i * 2 + 1];
amp = sqrt(re * re + im * im) / c_sqrtFFTPoints;
c_Amplitudes[i - 1] = amp * amp;
}
unsigned int j;
unsigned int nSubbandSize = BPM_DETECT_FFT_POINTS1 / ( 2 * BPM_NUMBER_OF_SUBBANDS);
//for(i = 0; i < BPM_NUMBER_OF_SUBBANDS; i++)
for(i = c_nLowLimitIndex; i < c_nHighLimitIndex; i++)
{
SUBBAND1 *subband = &c_pSubBand[i];
if(subband->BPM) // we have BPM, don't compute anymore
continue;
// === CREATE SUBBANDS =================================
BPM_DETECT1_REAL InstantAmplitude = 0;
for(j = i * nSubbandSize; j < (i + 1) * nSubbandSize; j++)
InstantAmplitude += c_Amplitudes[j];
InstantAmplitude /= (BPM_DETECT1_REAL) nSubbandSize;
// =====================================================
// FILL BUFFER
subband->Buffer[subband->BufferLoad] = InstantAmplitude;
subband->BufferLoad++;
if(subband->BufferLoad < c_nBufferSize)
continue;
unsigned int j;
unsigned int bpm = 0;
BPM_DETECT1_REAL max_correlation = 0;
// calculate auto correlation of each BPM in range
unsigned int k;
for(j = BPM_DETECT_MIN_BPM - BPM_DETECT_MIN_MARGIN - 1; j <= BPM_DETECT_MAX_BPM + BPM_DETECT_MAX_MARGIN; j++)
{
BPM_DETECT1_REAL correlation = 0;
for(k = 0; k < c_nWindowSize; k++)
correlation += (subband->Buffer[k] * subband->Buffer[k + c_pnOffset[j]]);
if(correlation >= max_correlation)
{
max_correlation = correlation;
bpm = j;
}
}
// mark bpm in history
if(bpm >= BPM_DETECT_MIN_BPM && bpm <= BPM_DETECT_MAX_BPM)
{
unsigned int HistoryHit = subband->BPMHistoryHit[bpm] + subband->BPMHistoryHit[bpm - 1] + subband->BPMHistoryHit[bpm + 1];
if(HistoryHit)
{
if(HistoryHit >= BPM_HISTORY_HIT)
{
unsigned int Avg = (bpm * subband->BPMHistoryHit[bpm] + (bpm - 1) * subband->BPMHistoryHit[bpm - 1] + (bpm + 1) * subband->BPMHistoryHit[bpm + 1]) / HistoryHit;
subband->BPM = Avg;
c_nSubbandBPMDetected++;
}
}
subband->BPMHistoryHit[bpm]++;
unsigned int j;
for(j = 0 ; j < c_nOverlapSuccessSize; j++)
subband->Buffer[j] = subband->Buffer[j + c_nOverlapSuccessStart];
subband->BufferLoad = c_nOverlapSuccessSize;
}
else
{
unsigned int j;
for(j = 0 ; j < c_nOverlapFailSize; j++)
subband->Buffer[j] = subband->Buffer[j + c_nOverlapFailStart];
subband->BufferLoad = c_nOverlapFailSize;
}
}
if(c_nSubbandBPMDetected == c_nBandSizeIndex)
return 1;
return 0;
}