File content as of revision 0:2076b4d80327:

#include <math.h>
#include "Mixer3D.h"
//#include "World.h"
#include "fft.h"
#include "mit_hrtf_lib.h"
#include <sys/time.h>

Mixer3D::Mixer3D(int bufSize, int smpRate, int bitD) :
bufferSize(bufSize), sampleRate(smpRate), bitDepth(bitD))
{
    cout << "BUF_SIZE: " << bufSize << endl;
    cout << "SAMPLE_RATE: " << smpRate << endl;
    cout << "BIT_DEPTH: " << bitD << endl;
    inputAO = new Complex[2*bufferSize];
    overlapInput = new Complex[2 * bufferSize];
    fInput = new Complex[2 * bufferSize];
    fFilter = new Complex[2 * bufferSize];
    
    azimuths = new int[World::MAX_OBJ];
    elevations = new int[World::MAX_OBJ];
    prevAzimuths = new int[World::MAX_OBJ];
    prevElevations = new int[World::MAX_OBJ];

    updateAngles(); // initialize azimuths and elevations

    outputLeft = new Complex*[World::MAX_OBJ];
    outputRight = new Complex*[World::MAX_OBJ];
    overlapLeft = new Complex*[World::MAX_OBJ];
    overlapRight = new Complex*[World::MAX_OBJ];

    leftFilter = new short[2 * bufferSize];
    rightFilter = new short[2 * bufferSize];
   
      
}

Mixer3D::~Mixer3D()
{
    delete [] inputAO;
    delete [] outputLeft;
    delete [] outputLeft;
    delete [] leftFilter;
    delete [] rightFilter;
    delete [] complexLeftFilter;
    delete [] complexRightFilter;
    delete [] overlapLeft;
    delete [] overlapRight;
    delete [] overlapInput;
    delete [] prevAzimuths;
    delete [] prevElevations;
    delete [] azimuths;
    delete [] elevations;
}

void Mixer3D::updateAngles() {
    AudioObj* iAudioObj;
    Location iLocation;
    for (int i = 0; i < myWorld->getNumObj(); i++) {
        iAudioObj = myWorld->getAudioObj(i);
        iLocation = iAudioObj->getLocation();
        
        if (iAudioObj->isGpsObject()) {
            azimuths[i] = player.computeAzimuthFromGpsCoordinates(&iLocation);
            elevations[i] = player.computeElevationFromGpsCoordinates(&iLocation);
        } else {
            azimuths[i] = player.computeAzimuth(&iLocation);
            elevations[i] = player.computeElevation(&iLocation);
        }
    }
}

bool Mixer3D::isPowerOfTwo(int x) {
    return !(x == 0) && !(x & (x - 1));
}

int Mixer3D::loadHRTF(int* pAzimuth, int* pElevation, unsigned int samplerate, unsigned int diffused, Complex *&leftFilterIn, Complex *&rightFilterIn)
{
    int size = mit_hrtf_get(pAzimuth, pElevation, samplerate, diffused, leftFilter, rightFilter);

    if (size == 0) {
        // TODO: Throw MIT HRTF filter does not exist exception
    }
    for (int i = 0; i < size; i++)
    {
        leftFilterIn[i] = (double)(leftFilter[i]);
        rightFilterIn[i] = (double)(rightFilter[i]);
    }

    return size;
}
void Mixer3D::convolution(Complex *input, Complex *filter, Complex *output, long nSig, long nFil, long nFFT) {

	if (input == NULL || filter == NULL) {
		throw invalid_argument("Input and Filter must be non-NULL.");
	}

	if (!isPowerOfTwo(nFFT) || nFFT < nSig || nFFT < nFil) {
        throw invalid_argument("NFFT must be a power of two, bigger than the signal length, and bigger than the filter length.");
	}

	// Perform FFT on both input and filter.
    // TODO: "Streamline" CFFT class?
	CFFT::Forward(input, fInput, (unsigned int)nFFT);
	CFFT::Forward(filter, fFilter, (unsigned int)nFFT);

	for (int i = 0; i < nFFT; i++) {
		output[i] = fInput[i] * fFilter[i];
    }
	CFFT::Inverse(output, (unsigned int)nFFT);
}

void Mixer3D::stereoConvolution(Complex *input, Complex *leftFilter, Complex *rightFilter, Complex *leftOutput, Complex *rightOutput, long nSIG, long nFIL, long nFFT)
{
    // TODO: Modify parameter name, input is a data member
    convolution(input, leftFilter, leftOutput, nSIG, nFIL, nFFT);
    convolution(input, rightFilter, rightOutput, nSIG, nFIL, nFFT);
}

void Mixer3D::computeValidAudioObjects(vector<AudioObj*> &validAudioObjectsOut) {
    AudioObj* iAudioObj;
    float iDistance;
    Location iLocation;
    for (int i=0; i < myWorld->getNumObj(); i++) {
        iAudioObj = myWorld->getAudioObj(i);
        if (iAudioObj->isActive()) {
            iLocation = iAudioObj->getLocation();
            if (iAudioObj->isGpsObject()) {
                if (player.isTrackingGps()) {
                    iDistance = player.computeDistanceFromGpsCoordinates(&iLocation);
                } else {
                    // not tracking gps, so skip
                    continue;
                }
            } else {
                iDistance = player.computeDistanceFrom(&iLocation);
            }
            
            if (iDistance < MAX_GPS_OBJ_DISTANCE) {
                validAudioObjectsOut.push_back(iAudioObj);
            }
        }
    }
}

void Mixer3D::performMix(short *ioDataLeft, short *ioDataRight)
{
    double runtime;
    struct timeval start, finish;
    gettimeofday(&start, NULL);
    //Zero the output arrays.
    for(int i = 0; i < bufferSize; i++)
    {
       ioDataLeft[i] = 0;
       ioDataRight[i] = 0;
    }
    
    //Iterate through all audio objects, obtain input data and calculate resulting audio data for each object.
    AudioObj* iAudioObj;
    Location iLocation;
    float iVolume, iDistance, iAmplitudeFactor;
    vector<AudioObj*> validAudioObjects;
    
    // get valid audio objects
    computeValidAudioObjects(validAudioObjects);
    int nValidAudioObjects = (int)validAudioObjects.size();
    for (int i = 0; i < nValidAudioObjects; i++) {
        iAudioObj = validAudioObjects[i];
        
        // loading in input data for the iteration accordingly
        if (!(iAudioObj->fillAudioData(inputAO, bufferSize))) {
            cout << "Audio Object" << i << " not loading input quickly enough" << endl;
            continue;
        }
        
        iVolume = iAudioObj->getVolume();
        if (iVolume == 0) {
            // skip 'muted' audio objects, but not after loading their input.
            // that way they are still playing, but silently.
            continue;
        }
        
        // handle background objects
        if (iAudioObj->isBackgroundObject()) {
            // copy input directly to output buffers
            for (int j=0; j < bufferSize; j++) {
                // scale input from [-1, 1] to proper scale depending on bit depth
                ioDataLeft[j] += inputAO[j].real()*iVolume*pow(2, bitDepth) / nValidAudioObjects;
                ioDataRight[j] += inputAO[j].real()*iVolume*pow(2, bitDepth) / nValidAudioObjects;
            }
            continue;
        }
        
        iLocation = iAudioObj->getLocation();
        
        // handle gps objects
        if (iAudioObj->isGpsObject()) {
            if (player.isTrackingGps()) {
                iDistance = player.computeDistanceFromGpsCoordinates(&iLocation);
                if (iDistance > MAX_GPS_OBJ_DISTANCE) {
                    // not within range, skip this audio object
                    continue;
                }
            } else {
                // gps tracking hasn't started yet
                continue;
            }
        } else {
            iDistance = player.computeDistanceFrom(&iLocation);
        }

        if (iDistance == 0) {
            iAmplitudeFactor = iVolume;
        } else {
            // ensure that the amplitude factor can never be > 1 to prevent clipping
            if (iDistance > 1) {
                iAmplitudeFactor = iVolume / pow(iDistance, 2);
            } else {
                iAmplitudeFactor = iVolume;
            }
        }
        
        // dynamically calculate the Azimuth and Elevation between every object and the player
        updateAngles();
        
        for(int j = 0; j < bufferSize * 2; j++) {
            if ( j >= bufferSize ) {
                // zero pad
                inputAO[j] = 0;
            } else {
                inputAO[j] *= iAmplitudeFactor;
            }
        }
       
        if (azimuths[i] != prevAzimuths[i] ||
           elevations[i] != prevElevations[i]) {
            // object location relative to player has changed, so fetch a new filter
            filterLength = loadHRTF(&azimuths[i], &elevations[i], sampleRate, 1, complexLeftFilter[i], complexRightFilter[i]);
            
            // zero pad
            for (int j = filterLength; j < 2 * bufferSize; j++) {
                complexLeftFilter[i][j] = 0;
                complexRightFilter[i][j] = 0;
            }
            
            if (azimuths[i] < 0) {
                   azimuths[i] = -azimuths[i];
            }
            
            
            //Since the filter changed, we perform a convolution on the old input with the new filter and pull out its tail.
            stereoConvolution(overlapInput, complexLeftFilter[i], complexRightFilter[i], outputLeft[i], outputRight[i], bufferSize, filterLength, 2 * bufferSize);
        
            // update the overlap part for the next iteration
            for (int j = 0; j < bufferSize; j++) {
                    overlapLeft[i][j] = outputLeft[i][j + bufferSize];
                    overlapRight[i][j] = outputRight[i][j + bufferSize];
            }
        }
        
        //Perform the convolution of the current input and current filter.
        stereoConvolution(inputAO, complexLeftFilter[i], complexRightFilter[i], outputLeft[i], outputRight[i], bufferSize, filterLength, 2 * bufferSize);
  
        //Output the data to the output arrays in short integer format. In addition to pushing the output of the main
        //convolution, we also need to add the overlapped tail of the last output and divide by 2. Finally, we need
        //to divide by the number of audio objects to ensure no clipping.
        for (int j = 0; j < bufferSize; j++)
        {
            ioDataLeft[j]  += (short)( (outputLeft[i][j].real() + overlapLeft[i][j].real()) / (2*nValidAudioObjects));
            ioDataRight[j] += (short)( (outputRight[i][j].real() + overlapRight[i][j].real()) / (2*nValidAudioObjects));
        }
        
        //Updating the overlapInput for the next iteration for the correpsonding object
        for (int j = 0; j < bufferSize * 2; j++) {
            if (j >= bufferSize) {
                overlapInput[j] = 0;
            } else {
                overlapInput[j] = inputAO[j];
            }
        }

        // TODO: If the filter has been changed, didn't we already do this?
        //Updating the default overlap information for the next iteration if the filter won't be changed
        for (int j = 0 ; j < bufferSize; j++) {
            overlapLeft[i][j] = outputLeft[i][j + bufferSize];
            overlapRight[i][j] = outputRight[i][j + bufferSize];
        }
    
        //storing the Azimuth value in this iteration for the comparison for the next iteration so that
        //we can know that whether the filter needs to be changed in the next iteration.
        prevAzimuths[i] = azimuths[i];
        prevElevations[i] = elevations[i];
    }
    gettimeofday(&finish, NULL);
    runtime = finish.tv_sec - start.tv_sec;
    runtime += (finish.tv_usec - start.tv_usec) / 1000000.0;
    
    cout << "performMix: " << runtime << endl;
}