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