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

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  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();

}