Modular Motion Processor: modules/DirectionFilter.cpp Source File

00001 #include "DirectionFilter.h"
00002 #include <string>
00003 #include <cmath>
00004 #include "../framedata/VectorFrameData.h"
00005 #include "../visualizer/DirectionFilterGUI.h"
00006 #include "../util/Error.h"
00007 
00008 DirectionFilter::DirectionFilter(double threshold, AbstractModule *successor) :
00009     ProcessModule(successor),  
00010     m_threshold(threshold),
00011     m_lastDirection(0)
00012 {
00013     setGUI(new DirectionFilterGUI(this));
00014 }
00015 
00016 DirectionFilter::DirectionFilter(ConfigSection *config, AbstractModule *successor) :
00017     ProcessModule(successor),  
00018     m_lastDirection(0)
00019 {
00020     if (config == NULL) {
00021         throw Error("DirectionFilter::DirectionFilter(): Pointer ConfigSection is NULL");
00022     }
00023     try {
00024         m_threshold = config->get<double>("threshold");
00025     } catch (Error e) { // TODO: right exeption type
00026         config->setItem("threshold", "0.2");
00027         m_threshold = .2;
00028     }
00029 
00030     setGUI(new DirectionFilterGUI(this));
00031 }
00032 
00033 void DirectionFilter::processFrameData(IFrameData *data)
00034 {
00035     if (isSignal(data)) {
00036         passSignal(data);
00037         return;
00038     }
00039 
00040     assertFramedataType(data, "Vector3D", "DirectionFilter::processFrameData");
00041 
00042     Vector3d tmp = static_cast<VectorFrameData*>(data)->getData();
00043 
00044     // normalize tmp
00045     tmp /= sqrt(tmp.get(0)*tmp.get(0) + tmp.get(1)*tmp.get(1) + tmp.get(2)*tmp.get(2));
00046 
00047     // calc (squared) distance between tmp and saved vector
00048     tmp -= m_lastDirection;
00049     double dist = tmp.get(0)*tmp.get(0) + tmp.get(1)*tmp.get(1) + tmp.get(2)*tmp.get(2);
00050 
00051     // direction is equivalent -> do not proceed!
00052     if (dist < m_threshold) {
00053         delete data;
00054         return;
00055     }
00056 
00057     // direction differs -> set last direction to this frame
00058     m_lastDirection = static_cast<VectorFrameData*>(data)->getData();
00059     // normalize it
00060     m_lastDirection /= sqrt(
00061                 m_lastDirection.get(0)*m_lastDirection.get(0) + 
00062                 m_lastDirection.get(1)*m_lastDirection.get(1) + 
00063                 m_lastDirection.get(2)*m_lastDirection.get(2)
00064             );
00065 
00066     // gui output
00067     static_cast<DirectionFilterGUI*>(getGUI())->addData(static_cast<VectorFrameData*>(data)->getData());
00068 
00069     ProcessModule::processFrameData(data);
00070 }
00071