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_THRESHOLD 0.4                   /* in units/ms */
  55 #define DEFAULT_ACCELERATION 2.0                /* unitless factor */
  56
  57 /*
  58  * Pointer acceleration filter constants
  59  */
  60
  61 #define MAX_VELOCITY_DIFF       1.0 /* units/ms */
  62 #define MOTION_TIMEOUT          300 /* (ms) */
  63 #define NUM_POINTER_TRACKERS    16
  64
  65 struct pointer_tracker {
  66         double dx;      /* delta to most recent event, in device units */
  67         double dy;      /* delta to most recent event, in device units */
  68         uint64_t time;  /* ms */
  69         int dir;
  70 };
  71
  72 struct pointer_accelerator;
  73 struct pointer_accelerator {
  74         struct motion_filter base;
  75
  76         accel_profile_func_t profile;
  77
  78         double velocity;        /* units/ms */
  79         double last_velocity;   /* units/ms */
  80         int last_dx;            /* device units */
  81         int last_dy;            /* device units */
  82
  83         struct pointer_tracker *trackers;
  84         int cur_tracker;
  85 };
  86
  87 enum directions {
  88         N  = 1 << 0,
  89         NE = 1 << 1,
  90         E  = 1 << 2,
  91         SE = 1 << 3,
  92         S  = 1 << 4,
  93         SW = 1 << 5,
  94         W  = 1 << 6,
  95         NW = 1 << 7,
  96         UNDEFINED_DIRECTION = 0xff
  97 };
  98
  99 static int
 100 get_direction(int dx, int dy)
 101 {
 102         int dir = UNDEFINED_DIRECTION;
 103         int d1, d2;
 104         double r;
 105
 106         if (abs(dx) < 2 && abs(dy) < 2) {
 107                 if (dx > 0 && dy > 0)
 108                         dir = S | SE | E;
 109                 else if (dx > 0 && dy < 0)
 110                         dir = N | NE | E;
 111                 else if (dx < 0 && dy > 0)
 112                         dir = S | SW | W;
 113                 else if (dx < 0 && dy < 0)
 114                         dir = N | NW | W;
 115                 else if (dx > 0)
 116                         dir = NE | E | SE;
 117                 else if (dx < 0)
 118                         dir = NW | W | SW;
 119                 else if (dy > 0)
 120                         dir = SE | S | SW;
 121                 else if (dy < 0)
 122                         dir = NE | N | NW;
 123         } else {
 124                 /* Calculate r within the interval  [0 to 8)
 125                  *
 126                  * r = [0 .. 2π] where 0 is North
 127                  * d_f = r / 2π  ([0 .. 1))
 128                  * d_8 = 8 * d_f
 129                  */
 130                 r = atan2(dy, dx);
 131                 r = fmod(r + 2.5*M_PI, 2*M_PI);
 132                 r *= 4*M_1_PI;
 133
 134                 /* Mark one or two close enough octants */
 135                 d1 = (int)(r + 0.9) % 8;
 136                 d2 = (int)(r + 0.1) % 8;
 137
 138                 dir = (1 << d1) | (1 << d2);
 139         }
 140
 141         return dir;
 142 }
 143
 144 static void
 145 feed_trackers(struct pointer_accelerator *accel,
 146               double dx, double dy,
 147               uint64_t time)
 148 {
 149         int i, current;
 150         struct pointer_tracker *trackers = accel->trackers;
 151
 152         for (i = 0; i < NUM_POINTER_TRACKERS; i++) {
 153                 trackers[i].dx += dx;
 154                 trackers[i].dy += dy;
 155         }
 156
 157         current = (accel->cur_tracker + 1) % NUM_POINTER_TRACKERS;
 158         accel->cur_tracker = current;
 159
 160         trackers[current].dx = 0.0;
 161         trackers[current].dy = 0.0;
 162         trackers[current].time = time;
 163         trackers[current].dir = get_direction(dx, dy);
 164 }
 165
 166 static struct pointer_tracker *
 167 tracker_by_offset(struct pointer_accelerator *accel, unsigned int offset)
 168 {
 169         unsigned int index =
 170                 (accel->cur_tracker + NUM_POINTER_TRACKERS - offset)
 171                 % NUM_POINTER_TRACKERS;
 172         return &accel->trackers[index];
 173 }
 174
 175 static double
 176 calculate_tracker_velocity(struct pointer_tracker *tracker, uint64_t time)
 177 {
 178         int dx;
 179         int dy;
 180         double distance;
 181
 182         dx = tracker->dx;
 183         dy = tracker->dy;
 184         distance = sqrt(dx*dx + dy*dy);
 185         return distance / (double)(time - tracker->time); /* units/ms */
 186 }
 187
 188 static double
 189 calculate_velocity(struct pointer_accelerator *accel, uint64_t time)
 190 {
 191         struct pointer_tracker *tracker;
 192         double velocity;
 193         double result = 0.0;
 194         double initial_velocity = 0.0;
 195         double velocity_diff;
 196         unsigned int offset;
 197
 198         unsigned int dir = tracker_by_offset(accel, 0)->dir;
 199
 200         /* Find least recent vector within a timelimit, maximum velocity diff
 201          * and direction threshold. */
 202         for (offset = 1; offset < NUM_POINTER_TRACKERS; offset++) {
 203                 tracker = tracker_by_offset(accel, offset);
 204
 205                 /* Stop if too far away in time */
 206                 if (time - tracker->time > MOTION_TIMEOUT ||
 207                     tracker->time > time)
 208                         break;
 209
 210                 /* Stop if direction changed */
 211                 dir &= tracker->dir;
 212                 if (dir == 0)
 213                         break;
 214
 215                 velocity = calculate_tracker_velocity(tracker, time);
 216
 217                 if (initial_velocity == 0.0) {
 218                         result = initial_velocity = velocity;
 219                 } else {
 220                         /* Stop if velocity differs too much from initial */
 221                         velocity_diff = fabs(initial_velocity - velocity);
 222                         if (velocity_diff > MAX_VELOCITY_DIFF)
 223                                 break;
 224
 225                         result = velocity;
 226                 }
 227         }
 228
 229         return result; /* units/ms */
 230 }
 231
 232 static double
 233 acceleration_profile(struct pointer_accelerator *accel,
 234                      void *data, double velocity, uint64_t time)
 235 {
 236         return accel->profile(&accel->base, data, velocity, time);
 237 }
 238
 239 static double
 240 calculate_acceleration(struct pointer_accelerator *accel,
 241                        void *data, double velocity, uint64_t time)
 242 {
 243         double factor;
 244
 245         /* Use Simpson's rule to calculate the avarage acceleration between
 246          * the previous motion and the most recent. */
 247         factor = acceleration_profile(accel, data, velocity, time);
 248         factor += acceleration_profile(accel, data, accel->last_velocity, time);
 249         factor += 4.0 *
 250                 acceleration_profile(accel, data,
 251                                      (accel->last_velocity + velocity) / 2,
 252                                      time);
 253
 254         factor = factor / 6.0;
 255
 256         return factor; /* unitless factor */
 257 }
 258
 259 static void
 260 accelerator_filter(struct motion_filter *filter,
 261                    struct motion_params *motion,
 262                    void *data, uint64_t time)
 263 {
 264         struct pointer_accelerator *accel =
 265                 (struct pointer_accelerator *) filter;
 266         double velocity; /* units/ms */
 267         double accel_value; /* unitless factor */
 268
 269         feed_trackers(accel, motion->dx, motion->dy, time);
 270         velocity = calculate_velocity(accel, time);
 271         accel_value = calculate_acceleration(accel, data, velocity, time);
 272
 273         motion->dx = accel_value * motion->dx;
 274         motion->dy = accel_value * motion->dy;
 275
 276         accel->last_dx = motion->dx;
 277         accel->last_dy = motion->dy;
 278
 279         accel->last_velocity = velocity;
 280 }
 281
 282 static void
 283 accelerator_destroy(struct motion_filter *filter)
 284 {
 285         struct pointer_accelerator *accel =
 286                 (struct pointer_accelerator *) filter;
 287
 288         free(accel->trackers);
 289         free(accel);
 290 }
 291
 292 struct motion_filter_interface accelerator_interface = {
 293         accelerator_filter,
 294         accelerator_destroy
 295 };
 296
 297 struct motion_filter *
 298 create_pointer_accelator_filter(accel_profile_func_t profile)
 299 {
 300         struct pointer_accelerator *filter;
 301
 302         filter = malloc(sizeof *filter);
 303         if (filter == NULL)
 304                 return NULL;
 305
 306         filter->base.interface = &accelerator_interface;
 307
 308         filter->profile = profile;
 309         filter->last_velocity = 0.0;
 310         filter->last_dx = 0;
 311         filter->last_dy = 0;
 312
 313         filter->trackers =
 314                 calloc(NUM_POINTER_TRACKERS, sizeof *filter->trackers);
 315         filter->cur_tracker = 0;
 316
 317         return &filter->base;
 318 }
 319
 320 static inline double
 321 calc_penumbral_gradient(double x)
 322 {
 323         x *= 2.0;
 324         x -= 1.0;
 325         return 0.5 + (x * sqrt(1.0 - x * x) + asin(x)) / M_PI;
 326 }
 327
 328 double
 329 pointer_accel_profile_smooth_simple(struct motion_filter *filter,
 330                                     void *data,
 331                                     double velocity, /* units/ms */
 332                                     uint64_t time)
 333 {
 334         double threshold = DEFAULT_THRESHOLD; /* units/ms */
 335         double accel = DEFAULT_ACCELERATION; /* unitless factor */
 336         double smooth_accel_coefficient; /* unitless factor */
 337         double factor; /* unitless factor */
 338
 339         if (threshold < 0.1)
 340                 threshold = 0.1;
 341         if (accel < 1.0)
 342                 accel = 1.0;
 343
 344         /* We use units/ms as velocity but it has no real meaning unless all
 345            devices have the same resolution. For touchpads, we normalize to
 346            400dpi (15.75 units/mm), but the resolution on USB mice is all
 347            over the place. Though most mice these days have either 400
 348            dpi (15.75 units/mm), 800 dpi or 1000dpi, excluding gaming mice
 349            that can usually adjust it on the fly anyway and currently go up
 350            to 8200dpi.
 351           */
 352         if (velocity < (threshold / 2.0))
 353                 return calc_penumbral_gradient(0.5 + velocity / threshold) * 2.0 - 1.0;
 354
 355         if (velocity <= threshold)
 356                 return 1.0;
 357
 358         factor = velocity/threshold;
 359         if (factor >= accel)
 360                 return accel;
 361
 362         /* factor is between 1.0 and accel, scale this to 0.0 - 1.0 */
 363         factor = (factor - 1.0) / (accel - 1.0);
 364         smooth_accel_coefficient = calc_penumbral_gradient(factor);
 365         return 1.0 + (smooth_accel_coefficient * (accel - 1.0));
 366 }