source: flair-src/trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp@ 7

//  created:    2011/05/01

//  filename:   LowPassFilter_impl.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    objet permettant le calcul d'un filtre passe bas

//

//

/*********************************************************************/



#include "LowPassFilter_impl.h"

#include "LowPassFilter.h"

#include <cvmatrix.h>

#include <Layout.h>

#include <GroupBox.h>

#include <SpinBox.h>

#include <DoubleSpinBox.h>



#define PI ((float)3.14159265358979323846)



using std::string;

using namespace flair::core;

using namespace flair::gui;

using namespace flair::filter;



LowPassFilter_impl::LowPassFilter_impl(const LowPassFilter* self,const LayoutPosition* position,string name,const cvmatrix* init_value)

{

    first_update=true;



    if(init_value!=NULL)

    {

        prev_value=(cvmatrix*)init_value;

    }

    else

    {

        //if NULL, assume dimension 1, and init=0

        cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);

        desc->SetElementName(0,0,"output");

        prev_value=new cvmatrix(self,desc,floatType,name);

        prev_value->SetValue(0,0,0);

    }



    //init UI

    GroupBox* reglages_groupbox=new GroupBox(position,name);

        T=new DoubleSpinBox(reglages_groupbox->NewRow(),"period, 0 for auto"," s",0,10,0.01);

        freq=new DoubleSpinBox(reglages_groupbox->NewRow(),"cutoff frequency"," Hz",0,10000,0.1,2,1);





    //init output matrix of same size as init

    cvmatrix_descriptor* desc=new cvmatrix_descriptor(prev_value->Rows(),prev_value->Cols());



    for(int i=0;i<prev_value->Rows();i++)

    {

        for(int j=0;j<prev_value->Cols();j++)

        {

            desc->SetElementName(i,j,prev_value->Name(i,j));

        }

    }



    output=new cvmatrix(self,desc,prev_value->GetDataType().GetElementDataType(),name);



    output->SetValue(0,0,0);

}



LowPassFilter_impl::~LowPassFilter_impl()

{

}



void LowPassFilter_impl::UpdateFrom(const io_data *data)

{

    float delta_t;

    float result;

    cvmatrix *input=(cvmatrix*)data;



    //on prend une fois pour toute les mutex et on fait des accès directs

    output->GetMutex();

    input->GetMutex();



    if(first_update==true)

    {

        for(int i=0;i<input->Rows();i++)

        {

            for(int j=0;j<input->Cols();j++)

            {

                output->SetValueNoMutex(i,j,prev_value->ValueNoMutex(i,j));

                prev_value->SetValueNoMutex(i,j,input->ValueNoMutex(i,j));

            }

        }

        first_update=false;

    }

    else

    {

        if(T->Value()==0)

        {

            delta_t=(float)(data->DataTime()-previous_time)/1000000000.;

        }

        else

        {

            delta_t=T->Value();

        }

        for(int i=0;i<input->Rows();i++)

        {

            for(int j=0;j<input->Cols();j++)

            {



                if(freq->Value()!=0)

                {

                    output->SetValueNoMutex(i,j,(1-2*PI*freq->Value()*delta_t)*prev_value->ValueNoMutex(i,j)+2*PI*freq->Value()*delta_t*input->ValueNoMutex(i,j));

                }

                else

                {

                    output->SetValueNoMutex(i,j,input->ValueNoMutex(i,j));

                }

                prev_value->SetValueNoMutex(i,j,output->ValueNoMutex(i,j));

            }

        }

    }

    input->ReleaseMutex();

    output->ReleaseMutex();



    output->SetDataTime(data->DataTime());

    previous_time=data->DataTime();

}