view sv/filter/DSP.cpp @ 282:d9319859a4cf tip

(none)

/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

/*	Sound Access	
		EASAIER client application.	
		Dublin Institute of Technology - Audio Research Group 2007
		www.audioresearchgroup.com
		Author: Dan Barry

	This program is free software; you can redistribute it and/or    
	modify it under the terms of the GNU General Public License as    
	published by the Free Software Foundation; either version 2 of the    
	License, or (at your option) any later version.  See the file    
	COPYING included with this distribution for more information.
*/

//#include  "stdafx.h"
#include  "DSP.h"
#include  "math.h"
#include	<cassert>
#include	<cmath>
extern int currentposition;
extern float lastfactor;
extern float hopfactor;
//These variables are only required for find peaks...the same variables are passed in to other functions
extern int numpeaks;
extern float *peak_locations; 

extern float *L_mags;		///CURRENT FRAME MAGNITUDES
extern float *R_mags;		///CURRENT FRAME MAGNITUDES
extern float *pL_mags;		///PREVIOUS FRAME MAGNITUDES
extern float *pR_mags;		///PREVIOUS FRAME MAGNITUDES

void intobyte(int num, char* pbyte1 ,char* pbyte2)
{

		
		char firstByte = (num & 0xff);
		int secondByteInt = (num & 0xff00);
		secondByteInt = secondByteInt>>8;
		char secondByte = secondByteInt;

		/*int thirdByteInt = (array[i] & 0xff0000);
		thirdByteInt = thirdByteInt>>16;
		char thirdByte = thirdByteInt;

		int fourthByteInt = (array[i] & 0xff000000);
		fourthByteInt = fourthByteInt>>24;
		char fourthByte = fourthByteInt;*/


		*pbyte1 = firstByte;
		*pbyte2 = secondByte;
}


void cart2pol(float* cart, float* mags, float* phases, int framesize)
{
	int length=framesize/2;
	for (int f = 0; f<length; f++)
	{
	
	mags[f]=sqrt((cart[f]*cart[f])+(cart[length+f]*cart[length+f]));
	

	phases[f]=atan2((float)(-1*cart[f+length]),(float)(cart[f]));
	
	

	}


}

void pol2cart(float* cart, float* mags, float* phases, int framesize)
{
	int length=framesize/2;
	for (int f = 0; f<length; f++)
	{
		
		cart[f]=mags[f]*cos(phases[f]);
		cart[length+f]=-(mags[f]*sin(phases[f]));
	
	
	}


}

void hanning(float* window, int framesize)
{
	
	for (int f = 0; f<framesize; f++)
	{
		
		window[f]= (float) (0.5*(1-cos(2*PI*(f+1)/(framesize+1))));
	
	}


}

void updatephases(float* c_phase,float* p_phase,float* c_synthphase,float* p_synthphase, int framesize,float hopfactor,float interpfactor)
{

	float synth_hopsize=((float) framesize) /4;
	float actual_anhop=floor(hopfactor*synth_hopsize);
	float anhop = floor((hopfactor*synth_hopsize)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;

	for (int f = 0; f<(framesize/2); f++)
	{
		
		//float bin = (f/(float)framesize);

		interpfactor;

		omega_k=(float) ((2*PI*f)/float(framesize));			//This is the predicted angular freqency rads/sec of the centre frequency
		
		delta_phi=(c_phase[f]-p_phase[f])-(anhop*omega_k);

		a = (float) (delta_phi/(2*PI));
		
		k =  (float) (floor(a+0.5));
		
		delta_phi = (float) (delta_phi-k*2*PI);
		
		inst_freq=omega_k+delta_phi/anhop; 
	
		
		
		c_synthphase[f] = p_synthphase[f]+(inst_freq*synth_hopsize); 

		if (currentposition<actual_anhop){c_synthphase[f] = c_phase[f];}
		
		p_synthphase[f] = c_synthphase[f];

		p_phase[f] = c_phase[f];

		
		
		

	
	}
}

void updatephases2(float* c_phase,float* p_phase,float* c_synthphase,float* p_synthphase, int framesize,float hopfactor,float interpfactor)
{

	float synth_hopsize=((float) framesize)/4;
	float actual_anhop=actual_anhop=floor(hopfactor*synth_hopsize);
	float anhop = floor((hopfactor*synth_hopsize)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;

	for (int f = 0; f<(framesize/2); f++)
	{
		
		//float bin = (f/(float)framesize);

		interpfactor;

		omega_k=(float) ((2*PI*f)/float(framesize));			//This is the predicted angular freqency rads/sec of the centre frequency
		
		delta_phi=(c_phase[f]-p_phase[f])-(anhop*omega_k);

		a = (float) (delta_phi/(2*PI));
		
		k = (float) (floor(a+0.5));
		
		delta_phi = (float) (delta_phi-k*2*PI);
		
		inst_freq=omega_k+delta_phi/anhop; 
	
		
		
		c_synthphase[f] = p_synthphase[f]+(inst_freq*synth_hopsize); 

		

		if (currentposition<actual_anhop){c_synthphase[f] = c_phase[f];}
		
		if (!(anhop==lastfactor)){c_synthphase[f]=c_phase[f];}
		
		
		
		p_synthphase[f] = c_synthphase[f];
		

		p_phase[f] = c_phase[f];

		

		

	
	}
	lastfactor=anhop;
}

void rotatephases(float* c_phase, float* p_phase, float* c_synthphase, float* p_synthphase, int framesize, float interpfactor)
{
	float phase_diff;
	int diffhop = framesize/4;

	float anhop = floor((diffhop)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;
	
	//findpeaks(c_mags, p_mags, framesize, peak_locations, numpeaks);
	
	for (int i=0; i<framesize/2; i++)
	{
		
		phase_diff=(c_phase[i]-p_phase[i]);
		
		omega_k=(float) ((2*PI*i)/float(framesize));

		delta_phi=(phase_diff)-(anhop*omega_k);

		a = (float) (delta_phi/(2*PI));
		
		k = (float) (floor(a+0.5));
		
		delta_phi = (float) (delta_phi-k*2*PI);
		
		inst_freq=omega_k+delta_phi/anhop; 
					
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);

		//c_synthphase[i]=p_synthphase[i]+(phase_diff);

		//c_synthphase[i]=p_synthphase[i]+((phase_diff*diffhop)*interpfactor);
		
		//if (currentposition<framesize/2 || (!(lastfactor==hopfactor))){c_synthphase[i] = c_phase[i];}
		if (currentposition<framesize/2){c_synthphase[i] = c_phase[i];}
		
		p_synthphase[i]=c_synthphase[i];
	}

	lastfactor=hopfactor;
}

void rotatephases_peaklocked(float* c_phase, float* p_phase, float* c_synthphase, float* p_synthphase, float* c_mags, int framesize, float interpfactor)
{
	float phase_diff;
	int diffhop = framesize/4;

	float anhop = floor((diffhop)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;
	int start, end, i;
	float original_peak_phase;
	float cutoff;

	cutoff = 10000;

	cutoff = floor(cutoff/(44100/framesize));

	numpeaks = findpeaks(c_mags, pL_mags,(float) framesize, peak_locations);
	
	int p=1;
	//while(peak_locations[p]<cutoff)
		while(p<=numpeaks)
	{
		
		i=(int) peak_locations[p];

		original_peak_phase=c_phase[i];
		
		//start=i-2;
		//end=i+2;
		
		
		if (p==1){
			start=0;
			end=(int) floor(peak_locations[p]+((peak_locations[p+1]-peak_locations[p])/2));
		}

		else{
		if (p==numpeaks){
			start=(int) floor(peak_locations[p-1]+((peak_locations[p]-peak_locations[p-1])/2));
			end=(framesize/2);
		}
		else {
			start=(int) floor(peak_locations[p-1]+((peak_locations[p]-peak_locations[p-1])/2));
			end=(int) floor(peak_locations[p]+((peak_locations[p+1]-peak_locations[p])/2));
		}
		}
			
		phase_diff=(c_phase[i]-p_phase[i]);
		omega_k=(float) ((2*PI*i)/float(framesize));
		delta_phi=(phase_diff)-(anhop*omega_k);
		a = (float) (delta_phi/(2*PI));
		k = (float) (floor(a+0.5));
		delta_phi = (float) (delta_phi-k*2*PI);
		inst_freq=omega_k+delta_phi/anhop; 
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);
		//c_synthphase[i]=p_synthphase[i]+(phase_diff);
		//c_synthphase[i]=p_synthphase[i]+((phase_diff*diffhop)*interpfactor);
		//if (currentposition<framesize/2 || (!(lastfactor==hopfactor))){c_synthphase[i] = c_phase[i];}
		if (currentposition<framesize/2){c_synthphase[i] = c_phase[i];}
		p_synthphase[i]=c_synthphase[i];

		int j;
		for (j=start; j<i; j++)
		{
			c_synthphase[j]=c_synthphase[i]-(original_peak_phase-c_phase[j]);
			p_synthphase[j]=c_synthphase[j];
		}

		for (j=i+1; j<end; j++)
		{
			c_synthphase[j]=c_synthphase[i]-(original_peak_phase-c_phase[j]);
			p_synthphase[j]=c_synthphase[j];
		}


		 p++;
	}

	/*for (i=peak_locations[p]+1; i<(framesize/2); i++)
	{
		phase_diff=(c_phase[i]-p_phase[i]);
		
		omega_k=(2*PI*i)/float(framesize);

		delta_phi=(phase_diff)-(anhop*omega_k);

		a = delta_phi/(2*PI);
		
		k = floor(a+0.5);
		
		delta_phi = delta_phi-k*2*PI;
		
		inst_freq=omega_k+delta_phi/anhop; 
					
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);

		p_synthphase[i]=c_synthphase[i];
	}  */

	lastfactor=hopfactor;
}

void cur2last(float* c_phase, float* c_synthphase, float* p_synthphase, int framesize)
{

for (int i=0; i<framesize/2; i++)
	{
		
		c_synthphase[i]=c_phase[i];
						
		p_synthphase[i]=c_synthphase[i];
	}


}


int findpeaks(float* c_mags, float* p_mags, float framesize, float* peak_locations)

{
	numpeaks=0;
	peak_locations[0]=0;
	float peakthreshold=0;

	for (int i=2; i<(framesize/4)-10; i++)
	{
		
		if(c_mags[i]>c_mags[i-1] && c_mags[i]>c_mags[i+1] && c_mags[i]>c_mags[i-2] && c_mags[i]>c_mags[i+2] && c_mags[i]>peakthreshold)
		{
			numpeaks++;
			peak_locations[numpeaks]=(float) i;
			
		}
		
		
	}
	return numpeaks;
}

bool transient_detect(float* L_mags, float* R_mags, float* pL_mags, float* pR_mags, float drumthresh, float framesize)
{
	
	extern float *L_phase, *R_phase;
	int trans = 0;
	
	for (int f = 0; f<(framesize/2); f++)
	{
		
			if	((R_mags[f]-pR_mags[f])>0 ||
				(L_mags[f]-pL_mags[f])>0){trans++;}
				
				
				pR_mags[f]=R_mags[f];
				pL_mags[f]=L_mags[f];
				

	}
	drumthresh=(drumthresh/100)*(framesize/2);
	

	if (trans>=drumthresh)
	{

	return true;
	}
	else{return false;}
}





void log10plot(float* L_mags, float* plotFFTarray, int framesize, int plotsize)
{
	float interpfactor1 = 10/100;	//1 - 100hz		bin 1 - 10
	float interpfactor2 = 90/100;	//100 - 1000hz		bin 10 - 100
	float interpfactor3 = 900/100;	//1000 - 10000hz		bin 100 - 1000
	float interpfactor4 = 1048/100;	//10000 - 20000hz			bin 1000 - 2048
	int temp;
	int i;
	float p;
	float ratio,dec;
	
	for (i=0 ; i<plotsize/4; i++)
	{
		p=(float) i;
		
		temp=(int) floor((((1+p)/100)*9)+1);
		ratio=((((1+p)/100)*9)+1)-floor((float) temp);
		
		plotFFTarray[i]=log((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp])/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp])/1000);
	}

	for (i=plotsize/4 ; i<(plotsize/4)*2; i++)
	{
		p=(float) i;
		temp= (int) floor((((1+p-plotsize/4)/100)*90)+10);
		plotFFTarray[i]=log(L_mags[temp]/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*2 ; i<(plotsize/4)*3; i++)
	{
		p=(float) i;
		temp=(int) floor((((1+p-(plotsize/4)*2)/100)*900)+100);
		
		plotFFTarray[i]=log(L_mags[temp]/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*3 ; i<(plotsize/4)*4; i++)
	{
		p=(float) i;
		temp=(int) floor((((1+p-(plotsize/4)*3)/100)*1048)+1000);
		
		plotFFTarray[i]=log(L_mags[temp]/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}
}


void log10plot2(QVector<float>& L_mags, float* plotFFTarray, int framesize, int plotsize)
{
	float interpfactor1 = 10/100;	//1 - 100hz		bin 1 - 10
	float interpfactor2 = 90/100;	//100 - 1000hz		bin 10 - 100
	float interpfactor3 = 900/100;	//1000 - 10000hz		bin 100 - 1000
	float interpfactor4 = 1048/100;	//10000 - 20000hz			bin 1000 - 2048
	int temp;
	int i;
	float p;
	float ratio,dec;

	
		
	for (i=0 ; i<plotsize/4; i++)
	{
		p=(float) i;
		
		temp=(int) floor((((1+p)/100)*9)+1);
		ratio=((((1+p)/100)*9)+1)-floor((float) temp);
		
		
		plotFFTarray[i]=((((L_mags.at(temp+1)-L_mags.at(temp))*ratio)+L_mags.at(temp)));
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp])/1000);
	}

	for (i=plotsize/4 ; i<(plotsize/4)*2; i++)
	{
		p=i;
		temp=floor((((1+p-plotsize/4)/100)*90)+10);
		plotFFTarray[i]=(L_mags.at(temp));
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*2 ; i<(plotsize/4)*3; i++)
	{
		p=i;
		temp=floor((((1+p-(plotsize/4)*2)/100)*900)+100);
		
		plotFFTarray[i]=(L_mags.at(temp));
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*3 ; i<(plotsize/4)*4-1; i++)
	{
		p=i;
		temp=floor((((1+p-(plotsize/4)*3)/100)*1048)+1000);
		
		plotFFTarray[i]=(L_mags.at(temp));
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}
}

void applyEQ(float* L_mags, float* R_mags, int framesize, int plotsize, QVector<float>& eqcurve)
{
	//(80,85,400,120,0) Screen Coordinates
	float gain, interpfactor, p,result;
	int i, ind;

	for(i=0; i<10; i++)
	{
		gain=(119-(eqcurve.at(i*10)))/120;  // eqcurve is the array filled when the user draws on the canvas
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
	}

	for(i=10; i<100; i++)
	{
		
		p=i;
		interpfactor=1.1;
		result=100+((interpfactor*(p-10)));
		ind=int(result);
		gain=(119-(eqcurve.at(ind)))/120;
		//gain=(110-(eqcurve[i+90]-85))/120;
		
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
			

	}

	for(i=100; i<1000; i++)
	{
		p=i;
		interpfactor=0.11;
		result=200+((interpfactor*(p-100)));
		ind=int(result);
		gain=(119-(eqcurve.at(ind)))/120;
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
			

	}
	
	for(i=1000; i<2048; i++)
	{
		p=i;
		interpfactor=.09;
		result=300+((interpfactor*(p-1000)));
		ind=int(result);
		gain=(119-(eqcurve.at(ind)))/120;
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
			

	}
}



void stereo2ms(float* left, float* right, int framesize)
{
	float m, s;
	for (int i=0;i<framesize;i++)
	{
		m=(left[i]+right[i])/2;
		s=(left[i]-right[i])/2;
		left[i]=m;
		right[i]=s;
	}
}

void ms2stereo(float* left, float* right, int framesize)
{
	float m, s;
	for (int i=0;i<framesize;i++)
	{
		m=(left[i]+right[i]);
		s=(left[i]-right[i]);
		left[i]=m;
		right[i]=s;
	}
}


void bandeq(float* L_mags, float* R_mags, QVector<float>& eqcurve, int framesize)
{
	
	for (int i=0;i<framesize/2;i++)
	{
		L_mags[i] *= eqcurve.at(i);
		R_mags[i] *= eqcurve.at(i);
	}

}