ArraySensor.cc

Go to the documentation of this file.

/*

    Class representing single sensor for array operations
    Copyright (C) 1999-2002 Jussi Laako

    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.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

*/


#include <stdio.h>
#include <math.h>
#include <float.h>

#include "ArraySensor.hh"


inline void clArraySensor::SinCosC (float fX,
    float *fpSinX, float *fpCosX)
{
    #ifdef _GNU_SOURCE
    sincosf(fX, fpSinX, fpCosX);
    #else
    *fpSinX = sin(fX);
    *fpCosX = cos(fX);
    #endif
    *fpSinX = -(*fpSinX);
}


inline void clArraySensor::SinCosC (double dX,
    double *dpSinX, double *dpCosX)
{
    #ifdef _GNU_SOURCE
    sincos(dX, dpSinX, dpCosX);
    #else
    *dpSinX = sin(dX);
    *dpCosX = cos(dX);
    #endif
    *dpSinX = -(*dpSinX);
}


clArraySensor::clArraySensor ()
{
    bDebug = false;
    fShadeCoeff = 1;
    fPI = acos(-1.0);
}


bool clArraySensor::Initialize (GDT fLDistance, GDT fRDistance, long lWinSize)
{
    GDT *fpNullPtr = NULL;

    fLeftDistance = fLDistance;
    fRightDistance = fRDistance;
    lWindowSize = lWinSize;
    lProcCount = lWindowSize / 2;
    if (bDebug) 
    {
        printf("Left distance %g, right distance %g\n",
            fLeftDistance, fRightDistance);
        printf("Window size %li\n", lWinSize);
    }
    dspvProcData.SetSize(lProcCount);
    if (!Filter.Initialize(lWindowSize, fpNullPtr, 
        (GDT) DSP_FILT_DEF_OVERLAP, (GDT) DSP_FILT_DEF_BETA, 
        FILTER_SMOOTH_KAISER_BESSEL))
        return false;
    return true;
}


void clArraySensor::SetSampleRate (GDT fFs)
{
    fSampleRate = fFs;
    fSampleSpacing = (GDT) 1.0 / fSampleRate;
    fFreqResolution = fSampleRate / (GDT) 2 / 
        (GDT) (lWindowSize / 2 + 1);
    if (bDebug) 
    {
        printf("Samplerate %g, spacing %g ms, resolution %g Hz\n", 
            fSampleRate, fSampleSpacing * 1000.0, fFreqResolution);
    }
}


void clArraySensor::SetSoundSpeed (GDT fSndSpeed)
{
    fSoundSpeed = fSndSpeed;
    fSecsPerMeter = (GDT) 1.0 / fSndSpeed;
    fLeftTime = fLeftDistance * fSecsPerMeter;
    fRightTime = fRightDistance * fSecsPerMeter;
    if (bDebug) 
    {
        printf("Sound speed %g ms/m\n", fSecsPerMeter * 1000.0);
        printf("Left delay time %g ms, right delay time %g ms\n",
            fLeftTime * 1000.0, fRightTime * 1000.0);
    }
}


void clArraySensor::SetArrayFrequency (GDT fFa)
{
    fArrayFrequency = fFa;
    if (bDebug) printf("Array frequency %g Hz\n", fArrayFrequency);
}


void clArraySensor::SetShading (GDT fCoeff)
{
    fShadeCoeff = fCoeff;
    if (bDebug) printf("Shading coefficient %g\n", fShadeCoeff);
}


void clArraySensor::SetDirection (GDT fDir, bool bLowPass)
{
    long lMaxBin;
    long lBinCntr;
    GDT fDirPhase;
    GDT fArrayTime;
    GDT fFreqTime;
    GDT fFreqPhase;
    GDT fHighCoeff;
    clDSPVector<GCDT> dspvFiltCoeffs;

    if (bDebug) 
    {
        /*printf("Direction %g rad (%g deg)\n", 
            fDir, DSP.RadToDeg(fDir));
        if (bLowPass) puts("Lowpass filter enabled");
        else puts("Lowpass filter disabled");*/
    }
    dspvFiltCoeffs.SetSize(lWindowSize / 2 + 1);
    fDirPhase = sin(fDir) * fPI;
    fArrayTime = (fDir < (GDT) 0) ? fRightTime : fLeftTime;
    lMaxBin = (long) (fArrayFrequency / fFreqResolution + (GDT) 0.5);
    if (lMaxBin > dspvFiltCoeffs.Size())
        lMaxBin = dspvFiltCoeffs.Size();
    for (lBinCntr = 0; lBinCntr < lMaxBin; lBinCntr++)
    {
        fFreqTime = (GDT) 1 / 
            ((GDT) lBinCntr * fFreqResolution * (GDT) 2);
        fFreqPhase = fDirPhase * (fArrayTime / fFreqTime);
        SinCosC(fFreqPhase, &dspvFiltCoeffs[lBinCntr].I,
            &dspvFiltCoeffs[lBinCntr].R);
    }
    fHighCoeff = (bLowPass) ? 0 : 1;
    for (lBinCntr = lMaxBin; lBinCntr < dspvFiltCoeffs.Size(); lBinCntr++)
    {
        dspvFiltCoeffs[lBinCntr].R = fHighCoeff;
        dspvFiltCoeffs[lBinCntr].I = (GDT) 0;
    }
    Filter.SetCoeffs(dspvFiltCoeffs.Ptr(), true);
}


void clArraySensor::Put (const GDT *fpSrc, long lSrcCount)
{
    dspvInData.Put(fpSrc, lSrcCount);
    while (dspvInData.Get(dspvProcData.Ptr(), dspvProcData.Size()))
    {
        dspvProcData *= fShadeCoeff;
        Filter.Put(dspvProcData.Ptr(), dspvProcData.Size());
        while (Filter.Get(dspvProcData.Ptr(), dspvProcData.Size()))
        {
            dspvOutData.Put(dspvProcData.Ptr(), dspvProcData.Size());
        }
    }
}


bool clArraySensor::Get (GDT *fpDest, long lDestCount)
{
    return dspvOutData.Get(fpDest, lDestCount);
}

---