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 }