1 // Copyright 2012 The ChromiumOS Authors
2 // Use of this source code is governed by a BSD-style license that can be
3 // found in the LICENSE file.
4
5 #include "include/iir_filter_interpreter.h"
6
7 #include <utility>
8
9 namespace gestures {
10
Increment()11 void IirFilterInterpreter::IoHistory::Increment() {
12 out_head = NextOutHead();
13 in_head = NextInHead();
14 }
15
16 // Uses exact comparisions, rather than approximate float comparisions,
17 // for operator==
operator ==(const IirFilterInterpreter::IoHistory & that) const18 bool IirFilterInterpreter::IoHistory::operator==(
19 const IirFilterInterpreter::IoHistory& that) const {
20 for (size_t i = 0; i < kInSize; i++)
21 if (in[i] != that.in[i])
22 return false;
23 for (size_t i = 0; i < kOutSize; i++)
24 if (out[i] != that.out[i])
25 return false;
26 return true;
27 }
28
29 // The default filter is a low-pass 2nd order Butterworth IIR filter with a
30 // normalized cutoff frequency of 0.2.
IirFilterInterpreter(PropRegistry * prop_reg,Interpreter * next,Tracer * tracer)31 IirFilterInterpreter::IirFilterInterpreter(PropRegistry* prop_reg,
32 Interpreter* next,
33 Tracer* tracer)
34 : FilterInterpreter(NULL, next, tracer, false),
35 using_iir_(true),
36 b0_(prop_reg, "IIR b0", 0.0674552738890719),
37 b1_(prop_reg, "IIR b1", 0.134910547778144),
38 b2_(prop_reg, "IIR b2", 0.0674552738890719),
39 b3_(prop_reg, "IIR b3", 0.0),
40 a1_(prop_reg, "IIR a1", -1.1429805025399),
41 a2_(prop_reg, "IIR a2", 0.412801598096189),
42 iir_dist_thresh_(prop_reg, "IIR Distance Threshold", 10),
43 adjust_iir_on_warp_(prop_reg, "Adjust IIR History On Warp", false) {
44 InitName();
45 b0_.SetDelegate(this);
46 b1_.SetDelegate(this);
47 b2_.SetDelegate(this);
48 b3_.SetDelegate(this);
49 a1_.SetDelegate(this);
50 a2_.SetDelegate(this);
51 iir_dist_thresh_.SetDelegate(this);
52 }
53
SyncInterpretImpl(HardwareState * hwstate,stime_t * timeout)54 void IirFilterInterpreter::SyncInterpretImpl(HardwareState* hwstate,
55 stime_t* timeout) {
56 // Delete old entries from map
57 short dead_ids[histories_.size() + 1];
58 size_t dead_ids_len = 0;
59 for (std::map<short, IoHistory>::iterator it = histories_.begin(),
60 e = histories_.end(); it != e; ++it)
61 if (!hwstate->GetFingerState((*it).first))
62 dead_ids[dead_ids_len++] = (*it).first;
63 for (size_t i = 0; i < dead_ids_len; ++i)
64 histories_.erase(dead_ids[i]);
65
66 // Modify current hwstate
67 for (size_t i = 0; i < hwstate->finger_cnt; i++) {
68 FingerState* fs = &hwstate->fingers[i];
69 std::map<short, IoHistory>::iterator history =
70 histories_.find(fs->tracking_id);
71 if (history == histories_.end()) {
72 // new finger
73 IoHistory hist(*fs);
74 histories_[fs->tracking_id] = hist;
75 continue;
76 }
77 // existing finger, apply filter
78 IoHistory* hist = &(*history).second;
79
80 // Finger WARP detected, adjust the IO history
81 if (adjust_iir_on_warp_.val_) {
82 float dx = 0.0, dy = 0.0;
83
84 if (fs->flags & GESTURES_FINGER_WARP_X_MOVE)
85 dx = fs->position_x - hist->PrevIn(0)->position_x;
86 if (fs->flags & GESTURES_FINGER_WARP_Y_MOVE)
87 dy = fs->position_y - hist->PrevIn(0)->position_y;
88
89 hist->WarpBy(dx, dy);
90 }
91
92 float dx = fs->position_x - hist->PrevOut(0)->position_x;
93 float dy = fs->position_y - hist->PrevOut(0)->position_y;
94
95 // IIR filter is too smooth for a quick finger movement. We do a simple
96 // rolling average if the position change between current and previous
97 // frames is larger than iir_dist_thresh_.
98 if (dx * dx + dy * dy > iir_dist_thresh_.val_ * iir_dist_thresh_.val_)
99 using_iir_ = false;
100 else
101 using_iir_ = true;
102
103 // TODO(adlr): consider applying filter to other fields
104 float FingerState::*fields[] = { &FingerState::position_x,
105 &FingerState::position_y,
106 &FingerState::pressure };
107 for (size_t f_idx = 0; f_idx < arraysize(fields); f_idx++) {
108 float FingerState::*field = fields[f_idx];
109 // Keep the current pressure reading, so we could make sure the pressure
110 // values will be same if there is two fingers on a SemiMT device.
111 if (hwprops_ && hwprops_->support_semi_mt &&
112 (field == &FingerState::pressure)) {
113 hist->NextOut()->pressure = fs->pressure;
114 continue;
115 }
116
117 if (adjust_iir_on_warp_.val_) {
118 if (field == &FingerState::position_x &&
119 (fs->flags & GESTURES_FINGER_WARP_X_MOVE)) {
120 hist->NextOut()->position_x = fs->position_x;
121 continue;
122 }
123
124 if (field == &FingerState::position_y &&
125 (fs->flags & GESTURES_FINGER_WARP_Y_MOVE)) {
126 hist->NextOut()->position_y = fs->position_y;
127 continue;
128 }
129 }
130
131 if (using_iir_) {
132 hist->NextOut()->*field =
133 b3_.val_ * hist->PrevIn(2)->*field +
134 b2_.val_ * hist->PrevIn(1)->*field +
135 b1_.val_ * hist->PrevIn(0)->*field +
136 b0_.val_ * fs->*field -
137 a2_.val_ * hist->PrevOut(1)->*field -
138 a1_.val_ * hist->PrevOut(0)->*field;
139 } else {
140 hist->NextOut()->*field = 0.5 * (fs->*field + hist->PrevOut(0)->*field);
141 }
142 }
143 float FingerState::*pass_fields[] = { &FingerState::touch_major,
144 &FingerState::touch_minor,
145 &FingerState::width_major,
146 &FingerState::width_minor,
147 &FingerState::orientation };
148 for (size_t f_idx = 0; f_idx < arraysize(pass_fields); f_idx++)
149 hist->NextOut()->*pass_fields[f_idx] = fs->*pass_fields[f_idx];
150 hist->NextOut()->flags = fs->flags;
151 *hist->NextIn() = *fs;
152 *fs = *hist->NextOut();
153 hist->Increment();
154 }
155 next_->SyncInterpret(hwstate, timeout);
156 }
157
DoubleWasWritten(DoubleProperty * prop)158 void IirFilterInterpreter::DoubleWasWritten(DoubleProperty* prop) {
159 histories_.clear();
160 }
161
WarpBy(float dx,float dy)162 void IirFilterInterpreter::IoHistory::WarpBy(float dx, float dy) {
163 for (size_t i = 0; i < kInSize; i++) {
164 PrevIn(i)->position_x += dx;
165 PrevIn(i)->position_y += dy;
166 }
167
168 for (size_t i = 0; i < kOutSize; i++) {
169 PrevOut(i)->position_x += dx;
170 PrevOut(i)->position_y += dy;
171 }
172 }
173
174 } // namespace gestures
175