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