| // Copyright (c) 2012 The Chromium OS Authors. All rights reserved. |
| // Use of this source code is governed by a BSD-style license that can be |
| // found in the LICENSE file. |
| |
| #include "gestures/include/iir_filter_interpreter.h" |
| |
| #include <utility> |
| |
| namespace gestures { |
| |
| void IirFilterInterpreter::IoHistory::Increment() { |
| out_head = NextOutHead(); |
| in_head = NextInHead(); |
| } |
| |
| // Uses exact comparisions, rather than approximate float comparisions, |
| // for operator== |
| bool IirFilterInterpreter::IoHistory::operator==( |
| const IirFilterInterpreter::IoHistory& that) const { |
| for (size_t i = 0; i < kInSize; i++) |
| if (in[i] != that.in[i]) |
| return false; |
| for (size_t i = 0; i < kOutSize; i++) |
| if (out[i] != that.out[i]) |
| return false; |
| return true; |
| } |
| |
| // The default filter is a low-pass 2nd order Butterworth IIR filter with a |
| // normalized cutoff frequency of 0.2. |
| IirFilterInterpreter::IirFilterInterpreter(PropRegistry* prop_reg, |
| Interpreter* next, |
| Tracer* tracer) |
| : FilterInterpreter(NULL, next, tracer, false), |
| b0_(prop_reg, "IIR b0", 0.0674552738890719, this), |
| b1_(prop_reg, "IIR b1", 0.134910547778144, this), |
| b2_(prop_reg, "IIR b2", 0.0674552738890719, this), |
| b3_(prop_reg, "IIR b3", 0.0, this), |
| a1_(prop_reg, "IIR a1", -1.1429805025399, this), |
| a2_(prop_reg, "IIR a2", 0.412801598096189, this), |
| iir_dist_thresh_(prop_reg, "IIR Distance Threshold", 10, this), |
| adjust_iir_on_warp_(prop_reg, "Adjust IIR History On Warp", 0), |
| using_iir_(true) { |
| InitName(); |
| } |
| |
| void IirFilterInterpreter::SyncInterpretImpl(HardwareState* hwstate, |
| stime_t* timeout) { |
| // Delete old entries from map |
| short dead_ids[histories_.size()]; |
| size_t dead_ids_len = 0; |
| for (map<short, IoHistory, kMaxFingers>::iterator it = histories_.begin(), |
| e = histories_.end(); it != e; ++it) |
| if (!hwstate->GetFingerState((*it).first)) |
| dead_ids[dead_ids_len++] = (*it).first; |
| for (size_t i = 0; i < dead_ids_len; ++i) |
| histories_.erase(dead_ids[i]); |
| |
| // Modify current hwstate |
| for (size_t i = 0; i < hwstate->finger_cnt; i++) { |
| FingerState* fs = &hwstate->fingers[i]; |
| map<short, IoHistory, kMaxFingers>::iterator history = |
| histories_.find(fs->tracking_id); |
| if (history == histories_.end()) { |
| // new finger |
| IoHistory hist(*fs); |
| histories_[fs->tracking_id] = hist; |
| continue; |
| } |
| // existing finger, apply filter |
| IoHistory* hist = &(*history).second; |
| |
| // Finger WARP detected, adjust the IO history |
| if (adjust_iir_on_warp_.val_) { |
| float dx = 0.0, dy = 0.0; |
| |
| if (fs->flags & GESTURES_FINGER_WARP_X_MOVE) |
| dx = fs->position_x - hist->PrevIn(0)->position_x; |
| if (fs->flags & GESTURES_FINGER_WARP_Y_MOVE) |
| dy = fs->position_y - hist->PrevIn(0)->position_y; |
| |
| hist->WarpBy(dx, dy); |
| } |
| |
| float dx = fs->position_x - hist->PrevOut(0)->position_x; |
| float dy = fs->position_y - hist->PrevOut(0)->position_y; |
| |
| // IIR filter is too smooth for a quick finger movement. We do a simple |
| // rolling average if the position change between current and previous |
| // frames is larger than iir_dist_thresh_. |
| if (dx * dx + dy * dy > iir_dist_thresh_.val_ * iir_dist_thresh_.val_) |
| using_iir_ = false; |
| else |
| using_iir_ = true; |
| |
| // TODO(adlr): consider applying filter to other fields |
| float FingerState::*fields[] = { &FingerState::position_x, |
| &FingerState::position_y, |
| &FingerState::pressure }; |
| for (size_t f_idx = 0; f_idx < arraysize(fields); f_idx++) { |
| float FingerState::*field = fields[f_idx]; |
| // Keep the current pressure reading, so we could make sure the pressure |
| // values will be same if there is two fingers on a SemiMT device. |
| if (hwprops_ && hwprops_->support_semi_mt && |
| (field == &FingerState::pressure)) { |
| hist->NextOut()->pressure = fs->pressure; |
| continue; |
| } |
| |
| if (adjust_iir_on_warp_.val_) { |
| if (field == &FingerState::position_x && |
| (fs->flags & GESTURES_FINGER_WARP_X_MOVE)) { |
| hist->NextOut()->position_x = fs->position_x; |
| continue; |
| } |
| |
| if (field == &FingerState::position_y && |
| (fs->flags & GESTURES_FINGER_WARP_Y_MOVE)) { |
| hist->NextOut()->position_y = fs->position_y; |
| continue; |
| } |
| } |
| |
| if (using_iir_) { |
| hist->NextOut()->*field = |
| b3_.val_ * hist->PrevIn(2)->*field + |
| b2_.val_ * hist->PrevIn(1)->*field + |
| b1_.val_ * hist->PrevIn(0)->*field + |
| b0_.val_ * fs->*field - |
| a2_.val_ * hist->PrevOut(1)->*field - |
| a1_.val_ * hist->PrevOut(0)->*field; |
| } else { |
| hist->NextOut()->*field = 0.5 * (fs->*field + hist->PrevOut(0)->*field); |
| } |
| } |
| float FingerState::*pass_fields[] = { &FingerState::touch_major, |
| &FingerState::touch_minor, |
| &FingerState::width_major, |
| &FingerState::width_minor, |
| &FingerState::orientation }; |
| for (size_t f_idx = 0; f_idx < arraysize(pass_fields); f_idx++) |
| hist->NextOut()->*pass_fields[f_idx] = fs->*pass_fields[f_idx]; |
| hist->NextOut()->flags = fs->flags; |
| *hist->NextIn() = *fs; |
| *fs = *hist->NextOut(); |
| hist->Increment(); |
| } |
| next_->SyncInterpret(hwstate, timeout); |
| } |
| |
| void IirFilterInterpreter::DoubleWasWritten(DoubleProperty* prop) { |
| histories_.clear(); |
| } |
| |
| void IirFilterInterpreter::IoHistory::WarpBy(float dx, float dy) { |
| for (size_t i = 0; i < kInSize; i++) { |
| PrevIn(i)->position_x += dx; |
| PrevIn(i)->position_y += dy; |
| } |
| |
| for (size_t i = 0; i < kOutSize; i++) { |
| PrevOut(i)->position_x += dx; |
| PrevOut(i)->position_y += dy; |
| } |
| } |
| |
| } // namespace gestures |