filter: use a separate variable for the final accel factor
[platform/upstream/libinput.git] / src / filter.c
1 /*
2  * Copyright © 2012 Jonas Ådahl
3  *
4  * Permission to use, copy, modify, distribute, and sell this software and
5  * its documentation for any purpose is hereby granted without fee, provided
6  * that the above copyright notice appear in all copies and that both that
7  * copyright notice and this permission notice appear in supporting
8  * documentation, and that the name of the copyright holders not be used in
9  * advertising or publicity pertaining to distribution of the software
10  * without specific, written prior permission.  The copyright holders make
11  * no representations about the suitability of this software for any
12  * purpose.  It is provided "as is" without express or implied warranty.
13  *
14  * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS
15  * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
16  * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
17  * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
18  * RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
19  * CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
20  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
21  */
22
23 #include "config.h"
24
25 #include <stdio.h>
26 #include <stdlib.h>
27 #include <stdint.h>
28 #include <limits.h>
29 #include <math.h>
30
31 #include "filter.h"
32
33 void
34 filter_dispatch(struct motion_filter *filter,
35                 struct motion_params *motion,
36                 void *data, uint64_t time)
37 {
38         filter->interface->filter(filter, motion, data, time);
39 }
40
41 void
42 filter_destroy(struct motion_filter *filter)
43 {
44         if (!filter)
45                 return;
46
47         filter->interface->destroy(filter);
48 }
49
50 /*
51  * Default parameters for pointer acceleration profiles.
52  */
53
54 #define DEFAULT_CONSTANT_ACCELERATION 10.0      /* unitless factor */
55 #define DEFAULT_THRESHOLD 4.0                   /* in units/ms */
56 #define DEFAULT_ACCELERATION 2.0                /* unitless factor */
57
58 /*
59  * Pointer acceleration filter constants
60  */
61
62 #define MAX_VELOCITY_DIFF       1.0 /* units/ms */
63 #define MOTION_TIMEOUT          300 /* (ms) */
64 #define NUM_POINTER_TRACKERS    16
65
66 struct pointer_tracker {
67         double dx;      /* delta to most recent event, in device units */
68         double dy;      /* delta to most recent event, in device units */
69         uint64_t time;  /* ms */
70         int dir;
71 };
72
73 struct pointer_accelerator;
74 struct pointer_accelerator {
75         struct motion_filter base;
76
77         accel_profile_func_t profile;
78
79         double velocity;        /* units/ms */
80         double last_velocity;   /* units/ms */
81         int last_dx;            /* device units */
82         int last_dy;            /* device units */
83
84         struct pointer_tracker *trackers;
85         int cur_tracker;
86 };
87
88 enum directions {
89         N  = 1 << 0,
90         NE = 1 << 1,
91         E  = 1 << 2,
92         SE = 1 << 3,
93         S  = 1 << 4,
94         SW = 1 << 5,
95         W  = 1 << 6,
96         NW = 1 << 7,
97         UNDEFINED_DIRECTION = 0xff
98 };
99
100 static int
101 get_direction(int dx, int dy)
102 {
103         int dir = UNDEFINED_DIRECTION;
104         int d1, d2;
105         double r;
106
107         if (abs(dx) < 2 && abs(dy) < 2) {
108                 if (dx > 0 && dy > 0)
109                         dir = S | SE | E;
110                 else if (dx > 0 && dy < 0)
111                         dir = N | NE | E;
112                 else if (dx < 0 && dy > 0)
113                         dir = S | SW | W;
114                 else if (dx < 0 && dy < 0)
115                         dir = N | NW | W;
116                 else if (dx > 0)
117                         dir = NE | E | SE;
118                 else if (dx < 0)
119                         dir = NW | W | SW;
120                 else if (dy > 0)
121                         dir = SE | S | SW;
122                 else if (dy < 0)
123                         dir = NE | N | NW;
124         } else {
125                 /* Calculate r within the interval  [0 to 8)
126                  *
127                  * r = [0 .. 2π] where 0 is North
128                  * d_f = r / 2π  ([0 .. 1))
129                  * d_8 = 8 * d_f
130                  */
131                 r = atan2(dy, dx);
132                 r = fmod(r + 2.5*M_PI, 2*M_PI);
133                 r *= 4*M_1_PI;
134
135                 /* Mark one or two close enough octants */
136                 d1 = (int)(r + 0.9) % 8;
137                 d2 = (int)(r + 0.1) % 8;
138
139                 dir = (1 << d1) | (1 << d2);
140         }
141
142         return dir;
143 }
144
145 static void
146 feed_trackers(struct pointer_accelerator *accel,
147               double dx, double dy,
148               uint64_t time)
149 {
150         int i, current;
151         struct pointer_tracker *trackers = accel->trackers;
152
153         for (i = 0; i < NUM_POINTER_TRACKERS; i++) {
154                 trackers[i].dx += dx;
155                 trackers[i].dy += dy;
156         }
157
158         current = (accel->cur_tracker + 1) % NUM_POINTER_TRACKERS;
159         accel->cur_tracker = current;
160
161         trackers[current].dx = 0.0;
162         trackers[current].dy = 0.0;
163         trackers[current].time = time;
164         trackers[current].dir = get_direction(dx, dy);
165 }
166
167 static struct pointer_tracker *
168 tracker_by_offset(struct pointer_accelerator *accel, unsigned int offset)
169 {
170         unsigned int index =
171                 (accel->cur_tracker + NUM_POINTER_TRACKERS - offset)
172                 % NUM_POINTER_TRACKERS;
173         return &accel->trackers[index];
174 }
175
176 static double
177 calculate_tracker_velocity(struct pointer_tracker *tracker, uint64_t time)
178 {
179         int dx;
180         int dy;
181         double distance;
182
183         dx = tracker->dx;
184         dy = tracker->dy;
185         distance = sqrt(dx*dx + dy*dy);
186         return distance / (double)(time - tracker->time); /* units/ms */
187 }
188
189 static double
190 calculate_velocity(struct pointer_accelerator *accel, uint64_t time)
191 {
192         struct pointer_tracker *tracker;
193         double velocity;
194         double result = 0.0;
195         double initial_velocity = 0.0;
196         double velocity_diff;
197         unsigned int offset;
198
199         unsigned int dir = tracker_by_offset(accel, 0)->dir;
200
201         /* Find least recent vector within a timelimit, maximum velocity diff
202          * and direction threshold. */
203         for (offset = 1; offset < NUM_POINTER_TRACKERS; offset++) {
204                 tracker = tracker_by_offset(accel, offset);
205
206                 /* Stop if too far away in time */
207                 if (time - tracker->time > MOTION_TIMEOUT ||
208                     tracker->time > time)
209                         break;
210
211                 /* Stop if direction changed */
212                 dir &= tracker->dir;
213                 if (dir == 0)
214                         break;
215
216                 velocity = calculate_tracker_velocity(tracker, time);
217
218                 if (initial_velocity == 0.0) {
219                         result = initial_velocity = velocity;
220                 } else {
221                         /* Stop if velocity differs too much from initial */
222                         velocity_diff = fabs(initial_velocity - velocity);
223                         if (velocity_diff > MAX_VELOCITY_DIFF)
224                                 break;
225
226                         result = velocity;
227                 }
228         }
229
230         return result; /* units/ms */
231 }
232
233 static double
234 acceleration_profile(struct pointer_accelerator *accel,
235                      void *data, double velocity, uint64_t time)
236 {
237         return accel->profile(&accel->base, data, velocity, time);
238 }
239
240 static double
241 calculate_acceleration(struct pointer_accelerator *accel,
242                        void *data, double velocity, uint64_t time)
243 {
244         double factor;
245
246         /* Use Simpson's rule to calculate the avarage acceleration between
247          * the previous motion and the most recent. */
248         factor = acceleration_profile(accel, data, velocity, time);
249         factor += acceleration_profile(accel, data, accel->last_velocity, time);
250         factor += 4.0 *
251                 acceleration_profile(accel, data,
252                                      (accel->last_velocity + velocity) / 2,
253                                      time);
254
255         factor = factor / 6.0;
256
257         return factor; /* unitless factor */
258 }
259
260 static void
261 accelerator_filter(struct motion_filter *filter,
262                    struct motion_params *motion,
263                    void *data, uint64_t time)
264 {
265         struct pointer_accelerator *accel =
266                 (struct pointer_accelerator *) filter;
267         double velocity; /* units/ms */
268         double accel_value; /* unitless factor */
269
270         feed_trackers(accel, motion->dx, motion->dy, time);
271         velocity = calculate_velocity(accel, time);
272         accel_value = calculate_acceleration(accel, data, velocity, time);
273
274         motion->dx = accel_value * motion->dx;
275         motion->dy = accel_value * motion->dy;
276
277         accel->last_dx = motion->dx;
278         accel->last_dy = motion->dy;
279
280         accel->last_velocity = velocity;
281 }
282
283 static void
284 accelerator_destroy(struct motion_filter *filter)
285 {
286         struct pointer_accelerator *accel =
287                 (struct pointer_accelerator *) filter;
288
289         free(accel->trackers);
290         free(accel);
291 }
292
293 struct motion_filter_interface accelerator_interface = {
294         accelerator_filter,
295         accelerator_destroy
296 };
297
298 struct motion_filter *
299 create_pointer_accelator_filter(accel_profile_func_t profile)
300 {
301         struct pointer_accelerator *filter;
302
303         filter = malloc(sizeof *filter);
304         if (filter == NULL)
305                 return NULL;
306
307         filter->base.interface = &accelerator_interface;
308
309         filter->profile = profile;
310         filter->last_velocity = 0.0;
311         filter->last_dx = 0;
312         filter->last_dy = 0;
313
314         filter->trackers =
315                 calloc(NUM_POINTER_TRACKERS, sizeof *filter->trackers);
316         filter->cur_tracker = 0;
317
318         return &filter->base;
319 }
320
321 static inline double
322 calc_penumbral_gradient(double x)
323 {
324         x *= 2.0;
325         x -= 1.0;
326         return 0.5 + (x * sqrt(1.0 - x * x) + asin(x)) / M_PI;
327 }
328
329 double
330 pointer_accel_profile_smooth_simple(struct motion_filter *filter,
331                                     void *data,
332                                     double velocity, /* units/ms */
333                                     uint64_t time)
334 {
335         double threshold = DEFAULT_THRESHOLD; /* units/ms */
336         double accel = DEFAULT_ACCELERATION; /* unitless factor */
337         double smooth_accel_coefficient; /* unitless factor */
338         double factor; /* unitless factor */
339
340         if (threshold < 1.0)
341                 threshold = 1.0;
342         if (accel < 1.0)
343                 accel = 1.0;
344
345         velocity *= DEFAULT_CONSTANT_ACCELERATION;
346
347         if (velocity < (threshold / 2.0))
348                 return calc_penumbral_gradient(0.5 + velocity / threshold) * 2.0 - 1.0;
349
350         if (velocity <= threshold)
351                 return 1.0;
352
353         factor = velocity/threshold;
354         if (factor >= accel)
355                 return accel;
356
357         /* factor is between 1.0 and accel, scale this to 0.0 - 1.0 */
358         factor = (factor - 1.0) / (accel - 1.0);
359         smooth_accel_coefficient = calc_penumbral_gradient(factor);
360         return 1.0 + (smooth_accel_coefficient * (accel - 1.0));
361 }