void AccelerateFFT<float>::performFFT (float* buffer, float* real, float* imag)
{
vDSP_ctoz ((COMPLEX*)buffer, 2, &complexSplit, 1, fftSizeOver2);
vDSP_fft_zrip (fftSetupFloat, &complexSplit, 1, log2n, FFT_FORWARD);
complexSplit.realp[fftSizeOver2] = complexSplit.imagp[0];
complexSplit.imagp[fftSizeOver2] = 0.0;
complexSplit.imagp[0] = 0.0;
for (size_t i = 0; i <= fftSizeOver2; i++)
{
complexSplit.realp[i] *= 0.5;
complexSplit.imagp[i] *= 0.5;
}
for (size_t i = fftSizeOver2 - 1; i > 0; --i)
{
complexSplit.realp[2 * fftSizeOver2 - i] = complexSplit.realp[i];
complexSplit.imagp[2 * fftSizeOver2 - i] = -1 * complexSplit.imagp[i];
}
for (size_t i = 0; i < fftSize; i++)
{
real[i] = complexSplit.realp[i];
imag[i] = complexSplit.imagp[i];
}
}
void FFTAccelerate::doFFTReal(float samples[], float amp[], int numSamples)
{
int i;
vDSP_Length log2n = log2f(numSamples);
int nOver2 = numSamples/2;
//-- window
//vDSP_blkman_window(window, windowSize, 0);
vDSP_vmul(samples, 1, window, 1, in_real, 1, numSamples);
//Convert float array of reals samples to COMPLEX_SPLIT array A
vDSP_ctoz((COMPLEX*)in_real,2,&A,1,nOver2);
//Perform FFT using fftSetup and A
//Results are returned in A
vDSP_fft_zrip(fftSetup, &A, 1, log2n, FFT_FORWARD);
// scale by 1/2*n because vDSP_fft_zrip doesn't use the right scaling factors natively ("for better performances")
{
const float scale = 1.0f/(2.0f*(float)numSamples);
vDSP_vsmul( A.realp, 1, &scale, A.realp, 1, numSamples/2 );
vDSP_vsmul( A.imagp, 1, &scale, A.imagp, 1, numSamples/2 );
}
//Convert COMPLEX_SPLIT A result to float array to be returned
/*amp[0] = A.realp[0]/(numSamples*2);
for(i=1;i<numSamples/2;i++)
amp[i]=sqrt(A.realp[i]*A.realp[i]+A.imagp[i]*A.imagp[i]);*/
// collapse split complex array into a real array.
// split[0] contains the DC, and the values we're interested in are split[1] to split[len/2] (since the rest are complex conjugates)
vDSP_zvabs( &A, 1, amp, 1, numSamples/2 );
}
void CMPerformFFT( CMFFT *fft, float *input, float *output )
{
int i, n, nby2 ;
n = fft->size ;
nby2 = n/2 ;
switch ( fft->style ) {
case PowerSpectrum:
vDSP_ctoz( ( COMPLEX* )input, 2, &fft->z, 1, nby2 ) ;
if ( fft->window ) {
for ( i = 0; i < nby2; i++ ) {
fft->z.realp[i] *= fft->window[i] ;
fft->z.imagp[i] *= fft->window[i] ;
}
}
vDSP_fft_zript( fft->vfft, &fft->z, 1, &fft->tempBuf, fft->log2n, FFT_FORWARD ) ;
vDSP_vsq( fft->z.realp, 1, fft->z.realp, 1, nby2 ) ;
vDSP_vsq( fft->z.imagp, 1, fft->z.imagp, 1, nby2 ) ;
vDSP_vadd( fft->z.realp, 1, fft->z.imagp, 1, &output[0], 1, nby2 ) ;
break ;
default:
break ;
}
}
void ofxFFT::calc() {
//Generate a split complex vector from the real data
vDSP_ctoz((COMPLEX *)input, 2, &mDspSplitComplex, 1, mFFTLength);
//Take the fft and scale appropriately
vDSP_fft_zrip(mSpectrumAnalysis, &mDspSplitComplex, 1, log2n, FFT_FORWARD);
// vDSP_fft_zrip(mSpectrumAnalysis, &mDspSplitComplex, 1, log2n, FFT_INVERSE);
vDSP_vsmul(mDspSplitComplex.realp, 1, &mFFTNormFactor, mDspSplitComplex.realp, 1, mFFTLength);
vDSP_vsmul(mDspSplitComplex.imagp, 1, &mFFTNormFactor, mDspSplitComplex.imagp, 1, mFFTLength);
// /* The output signal is now in a split real form. Use the function
// * vDSP_ztoc to get a split real vector. */
// vDSP_ztoc(&mDspSplitComplex, 1, (COMPLEX *) output, 2, mFFTLength);
//
//Zero out the nyquist value
mDspSplitComplex.imagp[0] = 0.0;
//Convert the fft data to dB
vDSP_zvmags(&mDspSplitComplex, 1, amplitude, 1, mFFTLength);
//In order to avoid taking log10 of zero, an adjusting factor is added in to make the minimum value equal -128dB
float mAdjust0DB = ADJUST_0_DB;
vDSP_vsadd(amplitude, 1, &mAdjust0DB, power, 1, mFFTLength);
float one = 1;
vDSP_vdbcon(power, 1, &one, power, 1, mFFTLength, 0);
}
Cicm_Fft::Cicm_Fft(long aWindowSize)
{
m_window_size = Tools::clip_power_of_two(aWindowSize * 2);
m_array_size = m_window_size / 2;
m_order = Tools::log2(m_window_size);
m_scale = 1. / (double)(m_window_size * 2.);
#ifdef CICM_VDSP
vDSP_ctoz((Cicm_Complex *)aRealVector, 2, &m_input_complexes, 1, m_array_size);
vDSP_fft_zrip(m_fft_setup, &m_input_complexes, 1, m_log2_size, FFT_FORWARD);
#endif
#ifdef CICM_IPPS
Cicm_fft_get_size(m_order, &m_spec_size, &m_init_size, &m_buff_size);
m_fft_buff = Cicm_buffer_malloc(m_buff_size);
m_fft_init = Cicm_buffer_malloc(m_init_size);
m_fft_spec = Cicm_buffer_malloc(m_spec_size);
Cicm_fft_init_handle(&m_fft_handle, m_order, m_fft_spec, m_fft_init);
#endif
#ifdef CICM_FFTW_GSL
m_real_vector = (Cicm_Signal *)Cicm_signal_malloc(m_window_size);
m_input_complexes = (Cicm_Packed *)Cicm_packed_malloc(m_window_size);
m_output_complexes = (Cicm_Packed *)Cicm_packed_malloc(m_window_size);
m_handle_forward = Cicm_fft_init_handle_forward(m_window_size, m_real_vector, m_input_complexes);
m_handle_inverse = Cicm_fft_init_handle_inverse(m_window_size, m_input_complexes, m_real_vector);
#endif
}
/* Demonstrate using an FFT to detect telephone keys.
Setup is the result of creating an FFT setup.
F contains a pair of frequencies to inject into a signal.
*/
void Demonstrate(FFTSetup Setup, FrequencyPair F)
{
float *Signal = malloc(SampleLength * sizeof *Signal);
if (Signal == 0)
{
fprintf(stderr, "Error, unable to allocate memory.\n");
exit(EXIT_FAILURE);
}
printf("\tGenerating signal with noise and DTMF tones...\n");
// Initialize the signal with noise.
for (int i = 0; i < SampleLength; ++i)
Signal[i] = 4 * Random();
// Add one of the tones to the signal.
float Phase = Random(); // Start the tone at a pseudo-random time.
for (int i = 0; i < SampleLength; ++i)
Signal[i] += sin((i*F.Frequency[0] / SamplingFrequency + Phase)
* TwoPi);
// Add the other tone.
Phase = Random(); // Start the tone at a pseudo-random time.
for (int i = 0; i < SampleLength; ++i)
Signal[i] += sin((i*F.Frequency[1]/SamplingFrequency + Phase)
* TwoPi);
// Rearrange the signal for vDSP_fft_zrip, using an auxiliary buffer.
// Get enough memory for two halves.
float *BufferMemory = malloc(SampleLength * sizeof *BufferMemory);
if (BufferMemory == 0)
{
fprintf(stderr, "Error, unable to allocate memory.\n");
exit(EXIT_FAILURE);
}
// Assign half the memory to reals and half to imaginaries.
DSPSplitComplex Buffer
= { BufferMemory, BufferMemory + SampleLength/2 };
// Copy (and rearrange) the data to the buffer.
vDSP_ctoz((DSPComplex *) Signal, 2, &Buffer, 1, SampleLength / 2);
printf("\tAnalyzing signal...\n");
// Compute the DFT of the signal.
vDSP_fft_zrip(Setup, &Buffer, 1, Log2SampleLength, FFT_FORWARD);
// Use the DFT results to identify the tones in the signal.
int Tone0 = FindTone(Buffer, DTMF0, NumberOf(DTMF0));
int Tone1 = FindTone(Buffer, DTMF1, NumberOf(DTMF1));
printf("\tFound frequencies %g and %g for key %c.\n",
DTMF0[Tone0], DTMF1[Tone1], Keys[Tone1*4 + Tone0]);
// Release resources.
free(BufferMemory);
free(Signal);
}
/* Calculate the power spectrum */
void fft::powerSpectrum_vdsp(int start, float *data, float *window, float *magnitude,float *phase) {
uint32_t i;
//multiply by window
vDSP_vmul(data, 1, window, 1, in_real, 1, n);
//convert to split complex format - evens and odds
vDSP_ctoz((COMPLEX *) in_real, 2, &A, 1, half);
//calc fft
vDSP_fft_zrip(setupReal, &A, 1, log2n, FFT_FORWARD);
//scale by 2 (see vDSP docs)
static float scale=0.5 ;
vDSP_vsmul(A.realp, 1, &scale, A.realp, 1, half);
vDSP_vsmul(A.imagp, 1, &scale, A.imagp, 1, half);
//back to split complex format
vDSP_ztoc(&A, 1, (COMPLEX*) out_real, 2, half);
//convert to polar
vDSP_polar(out_real, 2, polar, 2, half);
for (i = 0; i < half; i++) {
magnitude[i]=polar[2*i]+1.0;
phase[i]=polar[2*i + 1];
}
}
std::shared_ptr<float> FftProcessorImplAccelerate::process( const float * inData )
{
mFftComplexBuffer.realp = (float *)memset( mFftComplexBuffer.realp, 0, mBandCount );
mFftComplexBuffer.imagp = (float *)memset( mFftComplexBuffer.imagp, 0, mBandCount );
vDSP_ctoz( (DSPComplex *)inData, 2 * sStride, &mFftComplexBuffer, 1, mBandCount );
vDSP_fft_zrip( mFftSetup, &mFftComplexBuffer, 1, mLog2Size, FFT_FORWARD );
float * outData = new float[mBandCount];
for( int i = 0; i < mBandCount; i++ ) {
outData[i] = sqrt( ( mFftComplexBuffer.realp[i] * mFftComplexBuffer.realp[i] ) + ( mFftComplexBuffer.imagp[i] * mFftComplexBuffer.imagp[i] ) );
}
return std::shared_ptr<float>( outData, deleteFftBuffer );
/*
// apply window to fft data
// vDSP_vmul( outData, 1, mWindow, 1, outData, 1, mBandCount );
float* windowedOutData = new float[mBandCount];
// for( int i = 0; i < mBandCount; i++ ) {
// windowedOutData[i] = 0.0f;
// }
float val = 0.0f;
vDSP_vsmul( outData, 1, &val, windowedOutData, 1, mBandCount );
delete outData;
return std::shared_ptr<float>( windowedOutData, deleteFftBuffer );
*/
}
void FFTFrame::doFFT(const float* data) {
AudioFloatArray scaledData(m_FFTSize);
// veclib fft returns a result that is twice as large as would be expected.
// Compensate for that by scaling the input by half so the FFT has the
// correct scaling.
float scale = 0.5f;
VectorMath::vsmul(data, 1, &scale, scaledData.data(), 1, m_FFTSize);
vDSP_ctoz((DSPComplex*)scaledData.data(), 2, &m_frame, 1, m_FFTSize / 2);
vDSP_fft_zrip(m_FFTSetup, &m_frame, 1, m_log2FFTSize, FFT_FORWARD);
}
static void PerformFFT(float * input, float * window, COMPLEX_SPLIT &fftData, FFTSetup &setup, size_t N)
{
// windowing
vDSP_vmul(input, 1, window, 1, input, 1, N);
// FFT
vDSP_ctoz((COMPLEX *) input, 2, &fftData, 1, N/2);
vDSP_fft_zrip(setup, &fftData, 1, (size_t)ceilf(log2f(N)), kFFTDirection_Forward);
// zero-ing out Nyquist freq
fftData.imagp[0] = 0.0f;
}
Pitch PitchDetector::process(float *input, unsigned int numberOfSamples)
{
assert(this->numberOfSamples==numberOfSamples);
int n = numberOfSamples;
float *originalReal = this->FFTBuffer;
// 1.1.时域上加窗
vDSP_vmul(input, 1, this->windowFunc, 1, originalReal, 1, n);
// 分前后加窗没什么不一样啊
// vDSP_vmul(input, 1, this->windowFunc, 1, originalReal+(n/2), 1, n/2);
// vDSP_vmul(input+(n/2), 1, this->windowFunc+(n/2), 1, originalReal, 1, n/2);
// 2.拆成复数形似{1+2i, 3+4i, ..., 1023+1024i} 原始数组可以认为是（COMPLEX*）交错存储 现在拆成COMPLEX_SPLIT非交错（分轨式）存储
vDSP_ctoz((COMPLEX*)originalReal, 2, &this->A, 1, n/2); // 读取originalReal以2的步长塞进A里面
// // 3.fft变换的预设
float originalRealInLog2 = log2f(n);
// FFTSetup setupReal = vDSP_create_fftsetup(originalRealInLog2, FFT_RADIX2);
// 4.傅里叶变换
vDSP_fft_zrip(this->setupReal, &this->A, 1, originalRealInLog2, FFT_FORWARD);
// 5.转换成能量值
vDSP_zvabs(&this->A, 1, this->magnitudes, 1, n/2);
Float32 one = 1;
vDSP_vdbcon(this->magnitudes, 1, &one, this->magnitudes, 1, n/2, 0);
// 6.获取基频f0
float maxValue;
vDSP_Length index;
vDSP_maxvi(this->magnitudes, 1, &maxValue, &index, n/2);
// 6.1.微调参数
double alpha = this->magnitudes[index - 1];
double beta = this->magnitudes[index];
double gamma = this->magnitudes[index + 1];
double p = 0.5 * (alpha - gamma) / (alpha - 2 * beta + gamma);
// 7.转换为频率 indexHZ = index * (SampleRate / (n/2))
float indexHZ = (index+p) * ((this->sampleRate*1.0) / n);
// 8.乐理信息生成
Pitch pitch;
pitch.frequency = indexHZ;
pitch.amplitude = beta;
pitch.key = 12 * log2(indexHZ / 127.09) + 28.5;
pitch.octave = (pitch.key - 3.0) / 12 + 1;
calStep(&pitch);
pitch.stepString = calStep(indexHZ);
return pitch;
}
shared_ptr<float> FftProcessorImplAccelerate::process( const float * inData )
{
mFftComplexBuffer.realp = (float *)memset( mFftComplexBuffer.realp, 0, mBandCount );
mFftComplexBuffer.imagp = (float *)memset( mFftComplexBuffer.imagp, 0, mBandCount );
vDSP_ctoz( (DSPComplex *)inData, 2 * sStride, &mFftComplexBuffer, 1, mBandCount );
vDSP_fft_zrip( mFftSetup, &mFftComplexBuffer, 1, mLog2Size, FFT_FORWARD );
float * outData = new float[mBandCount];
for( int i = 0; i < mBandCount; i++ ) {
outData[i] = sqrt( ( mFftComplexBuffer.realp[i] * mFftComplexBuffer.realp[i] ) + ( mFftComplexBuffer.imagp[i] * mFftComplexBuffer.imagp[i] ) );
}
return shared_ptr<float>( outData, deleteFftBuffer );;
}
Cicm_Fft::~Cicm_Fft()
{
#ifdef CICM_VDSP_FLOAT
vDSP_ctoz((Cicm_Complex *)aRealVector, 2, &m_input_complexes, 1, m_array_size);
vDSP_fft_zrip(m_fft_setup, &m_input_complexes, 1, m_log2_size, FFT_FORWARD);
#endif
#ifdef CICM_IPPS
Cicm_free(m_fft_buff);
Cicm_free(m_fft_init);
Cicm_free(m_fft_spec);
Cicm_fft_free_handle(m_fft_handle);
#endif
#ifdef CICM_FFTW_GSL
Cicm_free(m_real_vector);
Cicm_free(m_input_complexes);
Cicm_free(m_output_complexes);
Cicm_fft_free_handle(m_handle_forward);
Cicm_fft_free_handle(m_handle_inverse);
#endif
}
void fft(FFT_FRAME* frame, float* audioBuffer)
{
FFT* fft = frame->fft;
// Do some data packing stuff
vDSP_ctoz((COMPLEX*)audioBuffer, 2, &frame->buffer, 1, fft->sizeOverTwo);
// This applies the windowing
if (fft->window != NULL)
vDSP_vmul(audioBuffer, 1, fft->window, 1, audioBuffer, 1, fft->size);
// Actually perform the fft
vDSP_fft_zrip(fft->fftSetup, &frame->buffer, 1, fft->logTwo, FFT_FORWARD);
// Do some scaling
vDSP_vsmul(frame->buffer.realp, 1, &fft->normalize, frame->buffer.realp, 1, fft->sizeOverTwo);
vDSP_vsmul(frame->buffer.imagp, 1, &fft->normalize, frame->buffer.imagp, 1, fft->sizeOverTwo);
// Zero out DC offset
frame->buffer.imagp[0] = 0.0;
}
void FFTAccelerate::doFFTReal(float samples[], float amp[], int numSamples)
{
int i;
vDSP_Length log2n = log2f(numSamples);
int nOver2 = numSamples/2;
//-- window
//vDSP_blkman_window(window, windowSize, 0);
vDSP_vmul(samples, 1, window, 1, in_real, 1, numSamples);
//Convert float array of reals samples to COMPLEX_SPLIT array A
vDSP_ctoz((COMPLEX*)in_real,2,&A,1,nOver2);
//Perform FFT using fftSetup and A
//Results are returned in A
vDSP_fft_zrip(fftSetup, &A, 1, log2n, FFT_FORWARD);
//Convert COMPLEX_SPLIT A result to float array to be returned
amp[0] = A.realp[0]/(numSamples*2);
for(i=1;i<numSamples/2;i++)
amp[i]=sqrt(A.realp[i]*A.realp[i]+A.imagp[i]*A.imagp[i]);
}
void fft::inversePowerSpectrum_vdsp(int start, float *finalOut, float *window, float *magnitude,float *phase) {
uint32_t i;
for (i = 0; i < half; i++) {
// polar[2*i] = pow(10.0, magnitude[i] / 20.0) - 1.0;
polar[2*i] = magnitude[i] - 1.0;
polar[2*i + 1] = phase[i];
}
vDSP_rect(polar, 2, in_real, 2, half);
vDSP_ctoz((COMPLEX*) in_real, 2, &A, 1, half);
vDSP_fft_zrip(setupReal, &A, 1, log2n, FFT_INVERSE);
vDSP_ztoc(&A, 1, (COMPLEX*) out_real, 2, half);
static float scale = 1./n;
vDSP_vsmul(out_real, 1, &scale, out_real, 1, n);
//multiply by window
vDSP_vmul(out_real, 1, window, 1, finalOut, 1, n);
}
void fftIp(FFT* fftObject, float* audioBuffer)
{
// Creating pointer of COMPLEX_SPLIT to use in calculations (points to same data as audioBuffer)
COMPLEX_SPLIT* fftBuffer = (COMPLEX_SPLIT *)&audioBuffer[0];
// Apply windowing
if (fftObject->window != NULL)
vDSP_vmul(audioBuffer, 1, fftObject->window, 1, audioBuffer, 1, fftObject->size);
// This seems correct-ish TODO: check casting
vDSP_ctoz((COMPLEX*)audioBuffer, 2, fftBuffer, 1, fftObject->sizeOverTwo);
// Perform fft
vDSP_fft_zrip(fftObject->fftSetup, fftBuffer, 1, fftObject->logTwo, FFT_FORWARD);
// Do scaling
vDSP_vsmul(fftBuffer->realp, 1, &fftObject->normalize, fftBuffer->realp, 1, fftObject->sizeOverTwo);
vDSP_vsmul(fftBuffer->imagp, 1, &fftObject->normalize, fftBuffer->imagp, 1, fftObject->sizeOverTwo);
// zero out DC offset
fftBuffer->imagp[0] = 0.0;
}
void SignalProcessorAudioProcessor::computeFFT() {
// Reinterpret the signal in fftBuffer as an interleaved-data complex vector and use vDSP_ctoz to move the data to a separated-data complex vector. The stride is equal to 2 because the imaginary elements are skipped
vDSP_ctoz((DSPComplex *) fftBuffer, 2*Stride, &Buffer, 1, N/2);
// Perform a real-to-complex DFT.
vDSP_DFT_Execute(zrop_Setup,
Buffer.realp, Buffer.imagp,
Observed.realp, Observed.imagp);
// If the FFT is set to logarithmic, transform the linear result array in a new log one
if (logarithmicFFT) {
computeLogFFT();
}
// Send the FFT message over the network
sendFFTMsg();
// Reinitialize fftBufferIndex to start writing the temp data back from the start of the table
fftBufferIndex = 0;
}
void FFTHelper::ComputeFFT(Float32* inAudioData, Float32* outFFTData)
{
if (inAudioData == NULL || outFFTData == NULL) return;
//Generate a split complex vector from the real data
vDSP_ctoz((COMPLEX *)inAudioData, 2, &mDspSplitComplex, 1, mFFTLength);
//Take the fft and scale appropriately
vDSP_fft_zrip(mSpectrumAnalysis, &mDspSplitComplex, 1, mLog2N, kFFTDirection_Forward);
vDSP_vsmul(mDspSplitComplex.realp, 1, &mFFTNormFactor, mDspSplitComplex.realp, 1, mFFTLength);
vDSP_vsmul(mDspSplitComplex.imagp, 1, &mFFTNormFactor, mDspSplitComplex.imagp, 1, mFFTLength);
//Zero out the nyquist value
mDspSplitComplex.imagp[0] = 0.0;
//Convert the fft data to dB
vDSP_zvmags(&mDspSplitComplex, 1, outFFTData, 1, mFFTLength);
//In order to avoid taking log10 of zero, an adjusting factor is added in to make the minimum value equal -128dB
vDSP_vsadd(outFFTData, 1, &kAdjust0DB, outFFTData, 1, mFFTLength);
Float32 one = 1;
vDSP_vdbcon(outFFTData, 1, &one, outFFTData, 1, mFFTLength, 0);
Float32 max = -100;
int index = -1;
for(unsigned long i = 0; i < mFFTLength; i++){
if(outFFTData[i] > max){
max = outFFTData[i];
index = i;
}
}
if(max > -40){ // Filter out anything else, as it is unlikely to be the microwave beep
recentMaxIndex = index;
//if(index == 181){ // We found the microwave beep
//printf("%d %f\n", index, max);
}else{
recentMaxIndex = 0;
}
}
void FFTHelper::ComputeFFT(Float32* inAudioData, Float32* outFFTData)
{
if (inAudioData == NULL || outFFTData == NULL) return;
//Generate a split complex vector from the real data
vDSP_ctoz((COMPLEX *)inAudioData, 2, &mDspSplitComplex, 1, mFFTLength);
//Take the fft and scale appropriately
vDSP_fft_zrip(mSpectrumAnalysis, &mDspSplitComplex, 1, mLog2N, kFFTDirection_Forward);
vDSP_vsmul(mDspSplitComplex.realp, 1, &mFFTNormFactor, mDspSplitComplex.realp, 1, mFFTLength);
vDSP_vsmul(mDspSplitComplex.imagp, 1, &mFFTNormFactor, mDspSplitComplex.imagp, 1, mFFTLength);
//Zero out the nyquist value
mDspSplitComplex.imagp[0] = 0.0;
//Convert the fft data to dB
vDSP_zvmags(&mDspSplitComplex, 1, outFFTData, 1, mFFTLength);
//In order to avoid taking log10 of zero, an adjusting factor is added in to make the minimum value equal -128dB
vDSP_vsadd(outFFTData, 1, &kAdjust0DB, outFFTData, 1, mFFTLength);
Float32 one = 1;
vDSP_vdbcon(outFFTData, 1, &one, outFFTData, 1, mFFTLength, 0);
}
void FFTFrame::doFFT(const float* data)
{
vDSP_ctoz((DSPComplex*)data, 2, &m_frame, 1, m_FFTSize / 2);
vDSP_fft_zrip(m_FFTSetup, &m_frame, 1, m_log2FFTSize, FFT_FORWARD);
}
ITunesPixelFormat ivis_render( ITunesVis* plugin, short audio_data[][512], float freq_data[][512],
void* buffer, long buffer_size, bool idle )
{
ITunesPixelFormat format = ITunesPixelFormatUnknown;
/* make sure we have a plugin and a visual handler */
if ( !plugin || !plugin->imports.visual_handler )
return format;
int i=0, w=0;
RenderVisualData visual_data;
DSPSplitComplex splitComplex[2];
float *data[2];
/* perform FFT if we're not idling */
if ( ! idle )
{
/* allocate some complex vars */
for ( i = 0 ; i < 2 ; i++ )
{
splitComplex[i].realp = calloc( 512, sizeof(float) );
splitComplex[i].imagp = calloc( 512, sizeof(float) );
data[i] = calloc( 512, sizeof(float) );
}
/* 2 channels for spectrum and waveform data */
visual_data.numWaveformChannels = 2;
visual_data.numSpectrumChannels = 2;
/* copy spectrum audio data to visual data strucure */
for ( w = 0 ; w < 512 ; w++ )
{
/* iTunes visualizers expect waveform data from 0 - 255, with level 0 at 128 */
visual_data.waveformData[0][w] = (UInt8)( (long)(audio_data[0][w]) / 128 + 128 );
visual_data.waveformData[1][w] = (UInt8)( (long)(audio_data[1][w]) / 128 + 128 );
/* scale to -1, +1 */
*( data[0] + w ) = (float)(( audio_data[0][w]) / (2.0 * 8192.0) );
*( data[1] + w ) = (float)(( audio_data[1][w]) / (2.0 * 8192.0) );
}
/* FFT scaler */
float scale = ( 1.0 / 1024.0 ) ; /* scale by length of input * 2 (due to how vDSP does FFTs) */
float nyq=0, dc=0, freq=0;
for ( i = 0 ; i < 2 ; i++ )
{
/* pack data into format fft_zrip expects it */
vDSP_ctoz( (COMPLEX*)( data[i] ), 2, &( splitComplex[i] ), 1, 256 );
/* perform FFT on normalized audio data */
fft_zrip( plugin->fft_setup, &( splitComplex[i] ), 1, 9, FFT_FORWARD );
/* scale the values */
vDSP_vsmul( splitComplex[i].realp, 1, &scale, splitComplex[i].realp, 1, 256 );
vDSP_vsmul( splitComplex[i].imagp, 1, &scale, splitComplex[i].imagp, 1, 256 );
/* unpack data */
vDSP_ztoc( &splitComplex[i], 1, (COMPLEX*)( data[i] ), 2, 256 );
/* ignore phase */
dc = *(data[i]) = fabs( *(data[i]) );
nyq = fabs( *(data[i] + 1) );
for ( w = 1 ; w < 256 ; w++ )
{
/* don't use vDSP for this since there's some overflow */
freq = hypot( *(data[i] + w * 2), *(data[i] + w * 2 + 1) ) * 256 * 16;
freq = MAX( 0, freq );
freq = MIN( 255, freq );
visual_data.spectrumData[i][ w - 1 ] = (UInt8)( freq );
}
visual_data.spectrumData[i][256] = nyq;
}
/* deallocate complex vars */
for ( i = 0 ; i < 2 ; i++ )
{
free( splitComplex[i].realp );
free( splitComplex[i].imagp );
free( data[i] );
}
/* update the render message with the new visual data and timestamp */
plugin->visual_message.u.renderMessage.renderData = &visual_data;
plugin->visual_message.u.renderMessage.timeStampID++;
}
/* update time */
plugin->visual_message.u.renderMessage.currentPositionInMS =
ivis_current_time() - plugin->start_time; // FIXME: real time
/* save our GL context and send the vis a render message */
CGLContextObj currentContext = CGLGetCurrentContext();
if ( plugin->gl_context )
aglSetCurrentContext( (AGLContext)(plugin->gl_context ) );
/* call the plugin's render method */
if ( idle )
{
/* idle message */
if ( plugin->wants_idle )
plugin->imports.visual_handler( kVisualPluginIdleMessage,
&( plugin->visual_message ),
plugin->vis_ref );
}
else
{
/* render message */
plugin->imports.visual_handler( kVisualPluginRenderMessage,
&( plugin->visual_message ),
plugin->vis_ref );
/* set position message */
plugin->visual_message.u.setPositionMessage.positionTimeInMS
= plugin->visual_message.u.renderMessage.currentPositionInMS;
plugin->imports.visual_handler( kVisualPluginSetPositionMessage, &( plugin->visual_message ),
plugin->vis_ref );
}
/* update message */
plugin->imports.visual_handler( kVisualPluginUpdateMessage, NULL,
plugin->vis_ref );
/* read pixels and restore our GL context */
CGLLockContext( CGLGetCurrentContext() );
switch ( get_pixels( buffer, buffer_size, CGLGetCurrentContext() != currentContext ) )
{
case 3:
format = ITunesPixelFormatRGB24;
break;
case 4:
format = ITunesPixelFormatRGBA32;
break;
default:
break;
}
CGLUnlockContext ( CGLGetCurrentContext() );
/* restore our GL context */
CGLSetCurrentContext( currentContext );
return format;
}
void FFT::performFFT (float* samples)
{
vDSP_ctoz ((COMPLEX*) samples, 2, &bufferSplit, 1, properties.fftSizeHalved);
vDSP_fft_zrip (config, &bufferSplit, 1, properties.fftSizeLog2, FFT_FORWARD);
}
//get MVDR weighting matrix
void HiLight::get_MVDR_weighting(float grid_color_intensity_MVDR[grid_x*grid_x_MVDR][grid_y*grid_y_MVDR][MVDR_frame], float MVDR_weighting[grid_x*grid_x_MVDR][grid_y*grid_y_MVDR]){
float brightness_mean[grid_x*grid_x_MVDR][grid_y*grid_y_MVDR];
for (int j = 0; j < grid_x * grid_x_MVDR; j++){
for (int k = 0; k < grid_y * grid_y_MVDR; k++){
brightness_mean[j][k] = 0;
}
}
for (int j = 0; j < grid_x*grid_x_MVDR; j++){
for (int k = 0; k < grid_y*grid_y_MVDR; k++){
for(int i = 0; i < MVDR_frame; i++){
brightness_mean[j][k] = brightness_mean[j][k] + grid_color_intensity_MVDR[j][k][i];
}
brightness_mean[j][k] = brightness_mean[j][k]*1.0 / MVDR_frame;
for(int i = 0; i < MVDR_frame; i++){
grid_color_intensity_MVDR[j][k][i] = grid_color_intensity_MVDR[j][k][i] - brightness_mean[j][k];
}
}
}
// Set up a data structure with pre-calculated values for
// doing a very fast FFT.
float x[N];
for (int j = 0; j < grid_x; j++){
for (int k = 0; k < grid_y; k++){
float x_MVDR[grid_x_MVDR * grid_y_MVDR][MVDR_frame/N][N];
float r_MDVR[grid_x_MVDR * grid_y_MVDR][grid_x_MVDR * grid_y_MVDR];
for (int frame_index = 0; frame_index < MVDR_frame/N; frame_index++){
for (int j1 = 0; j1 < grid_x_MVDR; j1++){
for (int k1 = 0; k1 < grid_y_MVDR; k1++){
for (int i = 0 ; i < N; i++){
x[i] = grid_color_intensity_MVDR[j1][k1][frame_index*N + i];
}
float fft_sum_N_over_2 = 0;
for (int i = 0; i < N/2; i++){
fft_sum_N_over_2 = fft_sum_N_over_2 + x[2*i] - x[2*i+1];
}
// We need complex buffers in two different formats!
DSPSplitComplex tempSplitComplex;
tempSplitComplex.realp = new float[N/2];
tempSplitComplex.imagp = new float[N/2];
// ----------------------------------------------------------------
// Forward FFT
// Scramble-pack the real data into complex buffer in just the way that's
// required by the real-to-complex FFT function that follows.
vDSP_ctoz((DSPComplex*)x, 2, &tempSplitComplex, 1, N/2);
// Do real->complex forward FFT
vDSP_fft_zrip(fftSetup, &tempSplitComplex, 1, LOG_N, kFFTDirection_Forward);
x_MVDR[j1*grid_y_MVDR + k1][frame_index][N/2] = fft_sum_N_over_2;
for (int i = 0; i < N/2; i++)
{
x_MVDR[j1*grid_y_MVDR + k1][frame_index][i] = tempSplitComplex.realp[i];
if (i>0){
x_MVDR[j1*grid_y_MVDR + k1][frame_index][N-i] = tempSplitComplex.realp[i];
}
}
}
}
}
for (int r_j = 0; r_j < grid_x_MVDR * grid_y_MVDR; r_j++){
for (int r_k = 0; r_k < grid_x_MVDR * grid_y_MVDR; r_k++){
float x_MVDR_sum = 0;
for (int row = 0; row != MVDR_frame/N; ++row)
{
for (int col = 0; col != N; ++col)
{
x_MVDR_sum = x_MVDR_sum + x_MVDR[r_j][row][col] * x_MVDR[r_k][row][col];
}
}
r_MDVR[r_j][r_k] = x_MVDR_sum;
}
}
float w[grid_x_MVDR * grid_y_MVDR];
float r_MVDR_inv[grid_x_MVDR * grid_y_MVDR][grid_x_MVDR * grid_y_MVDR];
matrix <double> r_MDVR_tmp(grid_x_MVDR * grid_y_MVDR,grid_x_MVDR * grid_y_MVDR);
for (int r_i=0; r_i < r_MDVR_tmp.getactualsize(); r_i++){
for (int r_j=0; r_j < r_MDVR_tmp.getactualsize(); r_j++)
{
r_MDVR_tmp.setvalue(r_i , r_j, r_MDVR[r_i][r_j]);
}
}
r_MDVR_tmp.invert();
bool invert_check;
double inv_results;
float r_MVDR_inv_sum = 0;
float w_sum = 0;
for (int r_i=0; r_i < r_MDVR_tmp.getactualsize(); r_i++){
for (int r_j=0; r_j<r_MDVR_tmp.getactualsize(); r_j++)
{
r_MDVR_tmp.getvalue(r_i, r_j, inv_results, invert_check);
r_MVDR_inv[r_i][r_j] = inv_results;
r_MVDR_inv_sum = r_MVDR_inv_sum + inv_results;
}
}
for (int r_i = 0; r_i < grid_x_MVDR * grid_y_MVDR; r_i++){
w[r_i] = 0;
for (int r_j = 0; r_j < grid_x_MVDR * grid_y_MVDR; r_j++){
w[r_i] = w[r_i] + r_MVDR_inv[r_i][r_j];
}
w[r_i] = w[r_i]/r_MVDR_inv_sum;
if (w[r_i] < 0){
w[r_i] = 0;
}
w_sum = w_sum + w[r_i];
}
for (int r_i = 0; r_i < grid_x_MVDR * grid_y_MVDR; r_i++){
w[r_i] = w[r_i] / w_sum * grid_x_MVDR * grid_y_MVDR;
}
for(int w_i = 0; w_i < grid_x_MVDR; w_i++){
for(int w_j = 0; w_j < grid_y_MVDR; w_j++){
MVDR_weighting[ j * grid_x_MVDR + w_i][ k * grid_y_MVDR + w_j] = w[w_i * grid_y_MVDR + w_j];
}
}
}
}
}
//Processes a frame and returns output image
bool HiLight::processFrame(const cv::Mat& inputFrame, int* screen_position, double current_timestamp, char *results, bool* denoise_check, bool* first_bit_check, int* hilight_line_index, char* hilight_results_tmp )
{
*denoise_check = false;
*first_bit_check = false;
if(!initialized){
initialized = true;
for(int i = 0; i<grid_x; i++){
for(int j = 0; j<grid_y; j++ ){
for (int k = 0; k < first_bit_window; k++){
first_bit_color_window[i][j][k] = 0;
}
for (int k = 0; k < N; k++){
grid_color_intensity[i][j][k] = 0;
hilight_results_stack[i][j][k] = 0;
}
hilight_results[i][j] = 0;
}
}
if (isImage){
MVDR = true;
first_2_ratio = 0.6;//0.8
first_bit_voting = 0.5;//0.7
}else{
MVDR = false;
first_2_ratio = 0.8; //0.7
first_bit_voting = 0.7; //0.6
}
window_index = 0;
first_bit_detected = false;
first_bit_index = 0;
hilight_stack_index = 0;
start_time = current_timestamp;
current_time = current_timestamp;
denoise_start = false;
MVDR_index = 0;
bit_counter = 0;
results_stack_counter = 0;
fftSetup = vDSP_create_fftsetup(LOG_N, kFFTRadix2);
start_receiving = false;
counter_after_detect_1= 0;
first_bit_1_detected = false;
first_bit_2_detected = false;
hilight_first_bit_index = 0;
hilight_first_bit_counter = 0;
hilight_first_bit_position = 0;
if (screen_position[2] < screen_position[4]){
image_ROI_position[0].y = screen_position[2];
}else{
image_ROI_position[0].y = screen_position[4];
}
if (screen_position[1] < screen_position[7]){
image_ROI_position[0].x = screen_position[1];
}else{
image_ROI_position[0].x = screen_position[7];
}
if (screen_position[6] < screen_position[8]){
image_ROI_position[1].y = screen_position[8];
}else{
image_ROI_position[1].y = screen_position[6];
}
if (screen_position[5] < screen_position[3]){
image_ROI_position[1].x = screen_position[3];
}else{
image_ROI_position[1].x = screen_position[5];
}
if (isImage){
// Set grids points to calculate the grid position
for (int i = 0; i <= grid_x * grid_x_MVDR; i++){
grids_points_top_image[i].x = screen_position[1] + (screen_position[3] - screen_position[1]) / grid_x / grid_x_MVDR * i;
grids_points_top_image[i].y = screen_position[2] + (screen_position[4] - screen_position[2]) / grid_x / grid_x_MVDR * i;
grids_points_bottom_image[i].x = screen_position[7] + (screen_position[5] - screen_position[7]) / grid_x / grid_x_MVDR * i;
grids_points_bottom_image[i].y = screen_position[8] + (screen_position[6] - screen_position[8]) / grid_x / grid_x_MVDR * i;
}
for (int i = 0; i <= grid_y * grid_y_MVDR; i++){
grids_points_left_image[i].x = screen_position[1] + (screen_position[7] - screen_position[1]) / grid_y / grid_y_MVDR * i;
grids_points_left_image[i].y = screen_position[2] + (screen_position[8] - screen_position[2]) / grid_y / grid_y_MVDR * i;
grids_points_right_image[i].x = screen_position[3] + (screen_position[5] - screen_position[3]) / grid_y / grid_y_MVDR * i;
grids_points_right_image[i].y = screen_position[4] + (screen_position[6] - screen_position[4]) / grid_y / grid_y_MVDR * i;
}
for (int i = 0; i < grid_x * grid_x_MVDR; i++){
for (int j = 0; j < grid_y * grid_y_MVDR; j++){
cv::Point2f r1;
cv::Point2f r2;
cv::Point2f r3;
cv::Point2f r4;
intersection(grids_points_top_image[i], grids_points_bottom_image[i], grids_points_left_image[j], grids_points_right_image[j], r1);//top left
intersection(grids_points_top_image[i+1], grids_points_bottom_image[i+1], grids_points_left_image[j], grids_points_right_image[j], r2);//top right
intersection(grids_points_top_image[i+1], grids_points_bottom_image[i+1], grids_points_left_image[j+1], grids_points_right_image[j+1], r3);// bottom right
intersection(grids_points_top_image[i], grids_points_bottom_image[i], grids_points_left_image[j+1], grids_points_right_image[j+1], r4);//bottom left
//refine grid_position
if (r1.x <= r4.x){
grids_position_image[i][j][0].x = r4.x - image_ROI_position[0].x;
}else{
grids_position_image[i][j][0].x = r1.x - image_ROI_position[0].x;
}
if (r1.y <= r2.y){
grids_position_image[i][j][0].y = r2.y - image_ROI_position[0].y;
}else{
grids_position_image[i][j][0].y = r1.y - image_ROI_position[0].y;
}
if (r3.x <= r2.x){
grids_position_image[i][j][1].x = r3.x - image_ROI_position[0].x;
}else{
grids_position_image[i][j][1].x = r2.x - image_ROI_position[0].x;
}
if (r3.y <= r4.y){
grids_position_image[i][j][1].y = r3.y - image_ROI_position[0].y;
}else{
grids_position_image[i][j][1].y = r4.y - image_ROI_position[0].y;
}
}
}
}else{
// Set grids points to calculate the grid position
for (int i = 0; i <= grid_x; i++){
grids_points_top_video[i].x = screen_position[1] + (screen_position[3] - screen_position[1]) / grid_x * i;
grids_points_top_video[i].y = screen_position[2] + (screen_position[4] - screen_position[2]) / grid_x * i;
grids_points_bottom_video[i].x = screen_position[7] + (screen_position[5] - screen_position[7]) / grid_x * i;
grids_points_bottom_video[i].y = screen_position[8] + (screen_position[6] - screen_position[8]) / grid_x * i;
}
for (int i = 0; i <= grid_y; i++){
grids_points_left_video[i].x = screen_position[1] + (screen_position[7] - screen_position[1]) / grid_y * i;
grids_points_left_video[i].y = screen_position[2] + (screen_position[8] - screen_position[2]) / grid_y * i;
grids_points_right_video[i].x = screen_position[3] + (screen_position[5] - screen_position[3]) / grid_y * i;
grids_points_right_video[i].y = screen_position[4] + (screen_position[6] - screen_position[4]) / grid_y * i;
}
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
cv::Point2f r1;
cv::Point2f r2;
cv::Point2f r3;
cv::Point2f r4;
intersection(grids_points_top_video[i], grids_points_bottom_video[i], grids_points_left_video[j], grids_points_right_video[j], r1);//top left
intersection(grids_points_top_video[i+1], grids_points_bottom_video[i+1], grids_points_left_video[j], grids_points_right_video[j], r2);//top right
intersection(grids_points_top_video[i+1], grids_points_bottom_video[i+1], grids_points_left_video[j+1], grids_points_right_video[j+1], r3);// bottom right
intersection(grids_points_top_video[i], grids_points_bottom_video[i], grids_points_left_video[j+1], grids_points_right_video[j+1], r4);//bottom left
//refine grid_position
if (r1.x <= r4.x){
grids_position_video[i][j][0].x = r4.x - image_ROI_position[0].x;
}else{
grids_position_video[i][j][0].x = r1.x - image_ROI_position[0].x;
}
if (r1.y <= r2.y){
grids_position_video[i][j][0].y = r2.y - image_ROI_position[0].y;
}else{
grids_position_video[i][j][0].y = r1.y - image_ROI_position[0].y;
}
if (r3.x <= r2.x){
grids_position_video[i][j][1].x = r3.x - image_ROI_position[0].x;
}else{
grids_position_video[i][j][1].x = r2.x - image_ROI_position[0].x;
}
if (r3.y <= r4.y){
grids_position_video[i][j][1].y = r3.y - image_ROI_position[0].y;
}else{
grids_position_video[i][j][1].y = r4.y - image_ROI_position[0].y;
}
}
}
}
srcTri[0] = cv::Point2f( screen_position[2],screen_position[1] );
srcTri[1] = cv::Point2f( screen_position[4],screen_position[3] );
srcTri[2] = cv::Point2f( screen_position[6],screen_position[5] );
dist_top = sqrt(pow(screen_position[4]-screen_position[2],2.0) + pow(screen_position[3]-screen_position[1],2.0));
dstTri[0] = cv::Point2f( screen_position[2],screen_position[1] );
dstTri[1] = cv::Point2f( screen_position[2],screen_position[1] + dist_top );
dstTri[2] = cv::Point2f( screen_position[2] + (int)(dist_top*transmitter_screen_ratio),screen_position[1] + dist_top);
warp_mat = getAffineTransform( srcTri, dstTri );
}else{
current_time = current_timestamp;
}
//crop the transmitter screen from the captured image
cv::Mat image_ROI = inputFrame(cv::Range(image_ROI_position[0].y, image_ROI_position[1].y), cv::Range(image_ROI_position[0].x, image_ROI_position[1].x));
getGray_HiLight(image_ROI, grayImage);
image_ROI.release();
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++){
brightness[c][r] = 0;
}
}
if (isImage){
int brightness_c = 0;
int brightness_r = 0;
for (int c = 0; c < grid_x * grid_x_MVDR; c++){
for (int r = 0; r < grid_y * grid_y_MVDR; r++)
{
cv::Mat tile = grayImage(cv::Range(grids_position_image[c][r][0].y + grid_margin, grids_position_image[c][r][1].y - grid_margin)
, cv::Range(grids_position_image[c][r][0].x + grid_margin, grids_position_image[c][r][1].x - grid_margin));
cv::Scalar pixel_scalar = cv::mean(tile);
tile.release();
float brightness_tmp =pixel_scalar[0];
if (MVDR){
if(!denoise_start && MVDR_index < MVDR_frame){
grid_color_intensity_MVDR[c][r][MVDR_index] = brightness_tmp;
if ( c == grid_x * grid_x_MVDR -1 && r == grid_y * grid_y_MVDR - 1){
MVDR_index++;
}
}else if(!denoise_start && MVDR_index >= MVDR_frame){
denoise_start = true;
*denoise_check = true;
get_MVDR_weighting(grid_color_intensity_MVDR, MVDR_weighting);
//print MVDR matrix
if (isDebug){
for (int i = 0; i < grid_x * grid_x_MVDR; i++){
for (int j = 0; j < grid_y * grid_y_MVDR; j++){
printf("%f\t", MVDR_weighting[i][j]);
}
printf("\n");
}
}
}else{
brightness_c = floor(c*1.0/grid_x_MVDR);
brightness_r = floor(r*1.0/grid_y_MVDR);
brightness[brightness_c][brightness_r] = brightness[brightness_c][brightness_r] + brightness_tmp * MVDR_weighting[c][r];
}
}else{
brightness_c = floor(c*1.0/grid_x_MVDR);
brightness_r = floor(r*1.0/grid_y_MVDR);
brightness[brightness_c][brightness_r] = brightness[brightness_c][brightness_r] + brightness_tmp;
}
}
}
}else{
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++)
{
cv::Mat tile = grayImage(cv::Range(grids_position_video[c][r][0].y + grid_margin, grids_position_video[c][r][1].y - grid_margin)
, cv::Range(grids_position_video[c][r][0].x + grid_margin, grids_position_video[c][r][1].x - grid_margin));
cv::Scalar pixel_scalar = cv::mean(tile);
tile.release();
float brightness_tmp =pixel_scalar[0];
brightness[c][r] = brightness_tmp;
}
}
}
if (denoise_start || !MVDR){
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++){
if (window_index<(N-1)) {
grid_color_intensity[c][r][window_index] = brightness[c][r];
window_index++;
}else{
int i;
float fft_sum_N_over_2 = 0;
for (i = 1; i < N; i++){
grid_color_intensity[c][r][i-1] = grid_color_intensity[c][r][i];
}
grid_color_intensity[c][r][N-1] = brightness[c][r];
for (i = 0; i < N/2; i++){
fft_sum_N_over_2 = fft_sum_N_over_2 + grid_color_intensity[c][r][2*i] - grid_color_intensity[c][r][2*i+1];
}
// Initialize the input buffer with a sinusoid
// We need complex buffers in two different formats!
tempSplitComplex.realp = new float[N/2];
tempSplitComplex.imagp = new float[N/2];
// ----------------------------------------------------------------
// Forward FFT
// Scramble-pack the real data into complex buffer in just the way that's
// required by the real-to-complex FFT function that follows.
vDSP_ctoz((DSPComplex*)grid_color_intensity[c][r], 2, &tempSplitComplex, 1, N/2);
// Do real->complex forward FFT
vDSP_fft_zrip(fftSetup, &tempSplitComplex, 1, LOG_N, kFFTDirection_Forward);
int max_pulse_index = object_pulse_threashold;
float max_pulse = -100;
float object_pulse_power_1 = LinearToDecibel(pow (fft_sum_N_over_2, 2.0)/ N);
float object_pulse_power_2;
int object_pulse_force;
int object_pulse;
for (int k = object_pulse_threashold; k < N/2; k++)
{
float spectrum = LinearToDecibel((pow (tempSplitComplex.realp[k], 2.0) + pow (tempSplitComplex.imagp[k], 2.0))/ 4/ N);
if (max_pulse<spectrum){
max_pulse = spectrum;
max_pulse_index = k;
}
if(k == object_pulse_2){
object_pulse_power_2 = spectrum;
}
}
delete tempSplitComplex.realp;
delete tempSplitComplex.imagp;
if(max_pulse < object_pulse_power_1 && object_pulse_power_1 > -25){
object_pulse = 1;
}else if(max_pulse == object_pulse_power_2){
object_pulse = 2;
}else{
object_pulse = 0;
}
if(object_pulse_power_1 >= object_pulse_power_2){
object_pulse_force = 1;
}else{
object_pulse_force = 2;
}
//decode data by find the peak frequency power
if (first_bit_detected){
if (first_bit_1_detected && isCalibrate){
hilight_first_bit_stack[c][r][hilight_first_bit_index] = object_pulse_force;
}
if(current_time - start_time >= N / 60.0 * (bit_counter + 1)){
hilight_results[c][r] = get_hilight_results(hilight_results_stack[c][r], hilight_stack_index);
hilight_results_stack[c][r][0] = object_pulse_force;
}else{
hilight_results_stack[c][r][hilight_stack_index] = object_pulse_force;
}
}else{
first_bit_color_window[c][r][first_bit_index] = object_pulse;
}
}
}
}
if (first_bit_1_detected && isCalibrate){
hilight_first_bit_timestamp[hilight_first_bit_index++] = current_time;
}if(first_bit_2_detected && isCalibrate){
first_bit_2_detected = false;
hilight_first_bit_counter++;
if (hilight_first_bit_counter == N/2){
calibrate_first_bit_position();
}
}
if (first_bit_detected){
if(current_time - start_time >= N / 60.0 * (bit_counter + 1)){
int counter_for_2 = 0;
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
if (isDebug || start_receiving){
printf("%d ",hilight_results[i][j]);
}
if (hilight_results[i][j] == 2){
counter_for_2++;
}
}
}
if (isDebug || start_receiving){
printf("\n");
}
hilight_stack_index = 1;
bit_counter++;
if (!start_receiving){
counter_after_detect_1++;
if (counter_after_detect_1 > 3){
reset();
}else{
if(counter_for_2 >= (double)grid_x * grid_y * first_2_ratio){
first_bit_2_detected = true;
start_receiving = true;
*first_bit_check = true;
if (isDebug){
printf("\n");
}
}
}
return false;
}else{
int results_counter_tmp =0;
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
output_bit_stack[results_stack_counter++] = hilight_results[i][j];
hilight_results_tmp[results_counter_tmp++] = hilight_results[i][j] + '0' - 1;
}
}
hilight_line_index[0] = results_stack_counter/grid_x/grid_y;
if (results_stack_counter == output_bit_stck_length){
results_stack_counter = 0;
get_char_from_bits(output_bit_stack, results);
printf("%s\n",results);
if (demo){
debug_reset();
return true;
}else{
debug_reset();
return false;
}
}else{
return false;
}
}
}else{
hilight_stack_index++;
}
}else{
if(first_bit_index<first_bit_window-1){
first_bit_index++;
}else{
first_bit_detected = detect_first_bit(first_bit_color_window);
if (first_bit_detected){
if (isCalibrate){
first_bit_1_detected = true;
}
start_time = current_time;
}
}
}
}
return false;
}
/* Demonstrate the real-to-complex one-dimensional out-of-place FFT,
vDSP_fft_zrop.
The out-of-place FFT writes results into a different array than the
input. If you are using vDSP_ctoz to reformat the input, you do not
need vDSP_fft_zrop because you move the data from an input array to an
output array when you call vDSP_ctoz. vDSP_fft_zrop may be useful when
incoming data arrives in the format used by vDSP_fft_zrop, and you want
to simultaneously perform an FFT and store the results in another array.
*/
static void DemonstratevDSP_fft_zrop(FFTSetup Setup)
{
/* Define a stride for the array be passed to the FFT. In many
applications, the stride is one and is passed to the vDSP
routine as a constant.
*/
const vDSP_Stride Stride = 1;
// Define a variable for a loop iterator.
vDSP_Length i;
// Define some variables used to time the routine.
ClockData t0, t1;
double Time;
printf("\n\tOne-dimensional real FFT of %lu elements.\n",
(unsigned long) N);
// Allocate memory for the arrays.
float *Signal = malloc(N * Stride * sizeof Signal);
float *BufferMemory = malloc(N * sizeof *BufferMemory);
float *ObservedMemory = malloc(N * sizeof *ObservedMemory);
if (ObservedMemory == NULL || BufferMemory == NULL || Signal == NULL)
{
fprintf(stderr, "Error, failed to allocate memory.\n");
exit(EXIT_FAILURE);
}
// Assign half of BufferMemory to reals and half to imaginaries.
DSPSplitComplex Buffer = { BufferMemory, BufferMemory + N/2 };
// Assign half of ObservedMemory to reals and half to imaginaries.
DSPSplitComplex Observed = { ObservedMemory, ObservedMemory + N/2 };
/* Generate an input signal. In a real application, data would of
course be provided from an image file, sensors, or other source.
*/
const float Frequency0 = 48, Frequency1 = 243, Frequency2 = 300;
const float Phase0 = 1.f/3, Phase1 = .82f, Phase2 = .5f;
for (i = 0; i < N; ++i)
Signal[i*Stride] =
cos((i * Frequency0 / N + Phase0) * TwoPi)
+ cos((i * Frequency1 / N + Phase1) * TwoPi)
+ cos((i * Frequency2 / N + Phase2) * TwoPi);
/* Reinterpret the real signal as an interleaved-data complex
vector and use vDSP_ctoz to move the data to a separated-data
complex vector. This puts the even-indexed elements of Signal
in Observed.realp and the odd-indexed elements in
Observed.imagp.
Note that we pass vDSP_ctoz two times Signal's normal stride,
because ctoz skips through a complex vector from real to real,
skipping imaginary elements. Considering this as a stride of
two real-sized elements rather than one complex element is a
legacy use.
In the destination array, a stride of one is used regardless of
the source stride. Since the destination is a buffer allocated
just for this purpose, there is no point in replicating the
source stride.
*/
vDSP_ctoz((DSPComplex *) Signal, 2*Stride, &Buffer, 1, N/2);
// Perform a real-to-complex FFT.
vDSP_fft_zrop(Setup, &Buffer, 1, &Observed, 1, Log2N, FFT_FORWARD);
/* Prepare expected results based on analytical transformation of
the input signal.
*/
float *ExpectedMemory = malloc(N * sizeof *ExpectedMemory);
if (ExpectedMemory == NULL)
{
fprintf(stderr, "Error, failed to allocate memory.\n");
exit(EXIT_FAILURE);
}
// Assign half of ExpectedMemory to reals and half to imaginaries.
DSPSplitComplex Expected = { ExpectedMemory, ExpectedMemory + N/2 };
for (i = 0; i < N/2; ++i)
Expected.realp[i] = Expected.imagp[i] = 0;
// Add the frequencies in the signal to the expected results.
Expected.realp[(int) Frequency0] = N * cos(Phase0 * TwoPi);
Expected.imagp[(int) Frequency0] = N * sin(Phase0 * TwoPi);
Expected.realp[(int) Frequency1] = N * cos(Phase1 * TwoPi);
Expected.imagp[(int) Frequency1] = N * sin(Phase1 * TwoPi);
Expected.realp[(int) Frequency2] = N * cos(Phase2 * TwoPi);
Expected.imagp[(int) Frequency2] = N * sin(Phase2 * TwoPi);
// Compare the observed results to the expected results.
CompareComplexVectors(Expected, Observed, N/2);
// Release memory.
free(ExpectedMemory);
/* The above shows how to use the vDSP_fft_zrop routine. Now we
will see how fast it is.
*/
/* Zero the signal before timing because repeated FFTs on non-zero
data can cause abnormalities such as infinities, NaNs, and
subnormal numbers.
*/
for (i = 0; i < N; ++i)
Signal[i] = 0;
vDSP_ctoz((DSPComplex *) Signal, 2*Stride, &Observed, 1, N/2);
// Time vDSP_fft_zrop by itself.
t0 = Clock();
for (i = 0; i < Iterations; ++i)
vDSP_fft_zrop(Setup, &Buffer, 1, &Observed, 1, Log2N,
FFT_FORWARD);
t1 = Clock();
// Average the time over all the loop iterations.
Time = ClockToSeconds(t1, t0) / Iterations;
printf("\tvDSP_fft_zrop on %lu elements takes %g microseconds.\n",
(unsigned long) N, Time * 1e6);
/* Unlike the vDSP_fft_zrip example, we do not time vDSP_fft_zrop
in conjunction with vDSP_ctoz and vDSP_ztoc. If your data
arrangement requires you to use vDSP_ctoz, then you are already
making a copy of the input data. So you would just do the FFT
in-place in that copy; you would call vDSP_fft_zrip and not
vDSP_fft_zrop.
*/
// Release resources.
free(ObservedMemory);
free(BufferMemory);
free(Signal);
}
Boolean FFTBufferManager::ComputeFFT(int32_t *outFFTData)
{
if (HasNewAudioData())
{
// for(int i=0;i<mFFTLength;i++)
// {
// if(mAudioBuffer[i]>0.15)
// printf("%f\n",mAudioBuffer[i]);
// }
//Generate a split complex vector from the real data
# define NOISE_FILTER 0.01;
for(int i=0;i<4096;i++)
{
//DENOISE
if(mAudioBuffer[i]>0)
{
mAudioBuffer[i] -= NOISE_FILTER;
if(mAudioBuffer[i] < 0)
{
mAudioBuffer[i] = 0;
}
}
else if(mAudioBuffer[i]<0)
{
mAudioBuffer[i] += NOISE_FILTER;
if(mAudioBuffer[i]>0)
{
mAudioBuffer[i] = 0;
}
}
else
{
mAudioBuffer[i] = 0;
}
}
vDSP_ctoz((COMPLEX *)mAudioBuffer, 2, &mDspSplitComplex, 1, mFFTLength);
//Take the fft and scale appropriately
vDSP_fft_zrip(mSpectrumAnalysis, &mDspSplitComplex, 1, mLog2N, kFFTDirection_Forward);
vDSP_vsmul(mDspSplitComplex.realp, 1, &mFFTNormFactor, mDspSplitComplex.realp, 1, mFFTLength);
vDSP_vsmul(mDspSplitComplex.imagp, 1, &mFFTNormFactor, mDspSplitComplex.imagp, 1, mFFTLength);
//Zero out the nyquist value
mDspSplitComplex.imagp[0] = 0.0;
//Convert the fft data to dB
Float32 tmpData[mFFTLength];
vDSP_zvmags(&mDspSplitComplex, 1, tmpData, 1, mFFTLength);
//In order to avoid taking log10 of zero, an adjusting factor is added in to make the minimum value equal -128dB
vDSP_vsadd(tmpData, 1, &mAdjust0DB, tmpData, 1, mFFTLength);
Float32 one = 1;
vDSP_vdbcon(tmpData, 1, &one, tmpData, 1, mFFTLength, 0);
//Convert floating point data to integer (Q7.24)
vDSP_vsmul(tmpData, 1, &m24BitFracScale, tmpData, 1, mFFTLength);
for(UInt32 i=0; i<mFFTLength; ++i)
outFFTData[i] = (SInt32) tmpData[i];
for(int i=0;i<mFFTLength/4;i++)
{
printf("%i:::%i\n",i,outFFTData[i]/16777216+120);
}
OSAtomicDecrement32Barrier(&mHasAudioData);
OSAtomicIncrement32Barrier(&mNeedsAudioData);
mAudioBufferCurrentIndex = 0;
return true;
}
else if (mNeedsAudioData == 0)
OSAtomicIncrement32Barrier(&mNeedsAudioData);
return false;
}
