/cygdrive/d/src/svn/vrut/trunk/modules/tracking/backup/filter.cpp

Go to the documentation of this file.

#include "filter.h"

TrackingFilter::TrackingFilter(float angle, float shift)
{
       this->angleFilter = angle;
       this->distFilter  = shift;
}

TrackingFilter::TFilterEntry * TrackingFilter::findEntry(Body::BodyType btype, int bid)
{
       std::list<TFilterEntry>::iterator i;
       for (i = table.begin(); i!= table.end(); i++)
       {
              if (((*i).id == bid) && ((*i).type == btype)) return &(*i);
       }
       return NULL;
}
bool TrackingFilter::checkTranslation(MATRIX currMatrix, MATRIX prevMatrix, float shift)
{
       if (absolute(currMatrix._m41 - prevMatrix._m41) >= shift
              || absolute(currMatrix._m42 - prevMatrix._m42) >= shift
              || absolute(currMatrix._m43 - prevMatrix._m43) >= shift)
       {
              //LOG(wxT("TRANSLATION FALSE"));
              return true;
       }
       else
       {
              //LOG(wxT("TRANSLATION FALSE"));
              return false;
       }
}
bool TrackingFilter::checkRotation(MATRIX currMatrix, MATRIX prevMatrix, float angle)
{
       float cosA = cos(angle);
       float cy2 = cosf(asinf(currMatrix._m13));
       float cy1 = cosf(asinf(prevMatrix._m13));
       float cosX = (currMatrix._m33 / cy2)*(prevMatrix._m33 / cy1) + (currMatrix._m23 / (-1 * cy2)) * (prevMatrix._m23 / (-1 * cy1));
       float cosY = (cy2 * cy1) + (currMatrix._m13 * prevMatrix._m13);
       float cosZ = ((currMatrix._m11 / cy2) * (prevMatrix._m11 / cy1)) + ((currMatrix._m12 / (-1 * cy2)) * (prevMatrix._m12 / (-1 * cy1)));
       if (cosA >= cosX || cosA >= cosY || cosA >= cosZ)
       {
       //     LOG(wxT("ROTATION TRUE"));
              return true;
       }
       else
       {
              //LOG(wxT("ROTATION FALSE"));
              return false;
       }
}
bool TrackingFilter::Filter(Body *body)
{
       TrackingFilter::TFilterEntry * entry = findEntry(body->GetType(), body->GetID());
       if (entry == NULL) // if no entry. add new entry and
       {
       //     LOG(wxT("NO ENTRY"));
              TrackingFilter::TFilterEntry  newEntry;
              newEntry.id         = body->GetID();
              newEntry.type       = body->GetType();
              newEntry.prevMatrix = body->Matrix();
              newEntry.angleFilter = this->angleFilter;
              newEntry.distFilter  = this->distFilter;
              newEntry.validMatrix = true;
              table.push_back(newEntry);
       //     LOG(wxT("FIRST ENTRY NO FILTER"));
              return false; // body shouldn't be filtered
       }
       else if (entry->validMatrix)
       {
              //LOG(wxT("VALID ENTRY"));
              
              if (body->GetQuality() != 1)
              {
                     // if device is not visible, set matrix to previous (to not be the dummy)
                     // but not filter out the body because of the buttons.
       //            LOG(wxT("FILTER_TRANSFORMATION_NOT BUTTONS"));
                     body->SetMatrix(entry->prevMatrix);
                     return false;
              }
              else
              {
                     bool filt1 = false;
                     MATRIX filterM;
                     if (!checkRotation(body->Matrix(), entry->prevMatrix, entry->angleFilter))
                     {
                            // if rotation is smaller then treshold
                            //LOG(wxT("FILTERED ROTATION"));
                            filterM = entry->prevMatrix.ExtractRotation();
                            filt1 = true;
                     }
                     else
                     {
                            //LOG(wxT("ROTATION"));
                            filterM = body->Matrix().ExtractRotation();
                     }
                     if (!checkTranslation(body->Matrix(), entry->prevMatrix, entry->distFilter))
                     {
                            //LOG(wxT("FILTERED TRANSLATION"));
                            filterM.Translate(entry->prevMatrix.ExtractTranslation());
                     }
                     else
                     {
                            //LOG(wxT("TRANSLATION"));
                            filterM.Translate(body->Matrix().ExtractTranslation());
                            filt1 = false;
                     }
                     entry->prevMatrix = body->Matrix() = filterM;
                     return filt1;
              }
              return true;
       }
       else
       {
              //LOG(wxT("INVALID ENTRY"));
              entry->prevMatrix = body->Matrix();
              entry->validMatrix = true;
              return false;
       }
}

bool TrackingFilter::UpdateFilter(Body::BodyType type, int id, float angle, float shift)
{
       TrackingFilter::TFilterEntry * entry = this->findEntry(type, id);
       if (entry == NULL)
       {
              TFilterEntry new_entry;
              new_entry.angleFilter = angle;
              new_entry.distFilter = shift;
              new_entry.id = id;
              new_entry.type = type;
              new_entry.validMatrix = false;
              table.push_back(new_entry);
              return false;
       }
       else
       {
              entry->angleFilter = angle;
              entry->distFilter  = shift;
              return true;
       }
}
wxString TrackingFilter::GetAsString(const char *lineSeparator, const char *atributeSeparator)
{
       wxString dump = wxT("");
       size_t  max = this->table.size();
       if (max ==0) return wxT("EMPTY");
       std::list<TFilterEntry>::iterator i, j;
       for (i =table.begin(); i != table.end(); i++)
       {
              if (i != table.begin()) dump += wxString::From8BitData(lineSeparator);
              TFilterEntry * entry = &(*i);
              dump += wxString::Format(wxT("%i%s%i%s%f%s%f"),
                                                        entry->type, atributeSeparator,
                                                        entry->id, atributeSeparator,
                                                        entry->angleFilter, atributeSeparator,
                                                        entry->distFilter);
       }
       return dump;
}

---