Add velocity estimate

The per-touch velocity is useful in many contexts. This patch
adds a simple estimator of the velocity, which also smooths out
position jumps caused by course-grained event updates.

Signed-off-by: Henrik Rydberg <rydberg@euromail.se>

5 files changed, 37 insertions, 6 deletions

diff --git a/include/utouch/frame.h b/include/utouch/frame.h
index 03a44aa..60195ef 100644
--- a/include/utouch/frame.h
+++ b/include/utouch/frame.h
@@ -115,6 +115,8 @@ struct utouch_surface {
  * @orientation: direction of ellipse (left: -Pi/2, up: 0, right: Pi/2)
  * @pressure: pressure of contact (pressure units)
  * @distance: distance of contact (surface units)
+ * @vx: horizontal velocity coordinate (units / millisecond)
+ * @vy: vertical velocity coordinate (units / millisecond)
  *
  * Surface contact details. Later versions of this struct may grow in
  * size, but will remain binary compatible with older versions.
@@ -138,6 +140,8 @@ struct utouch_contact {
         float orientation;
         float pressure;
         float distance;
+        float vx;
+        float vy;
 };
 
 /* time in milliseconds */
diff --git a/src/frame-mtdev.c b/src/frame-mtdev.c
index caf90ae..1d9976d 100644
--- a/src/frame-mtdev.c
+++ b/src/frame-mtdev.c
@@ -154,9 +154,11 @@ static int handle_abs_event(utouch_frame_handle fh,
                 return 1;
         case ABS_MT_POSITION_X:
                 t->x = ev->value;
+                t->vx = 0;
                 return 1;
         case ABS_MT_POSITION_Y:
                 t->y = ev->value;
+                t->vy = 0;
                 return 1;
         case ABS_MT_TOUCH_MAJOR:
                 t->touch_major = ev->value;
diff --git a/src/frame-xi2.c b/src/frame-xi2.c
index a78d9bf..bead751 100644
--- a/src/frame-xi2.c
+++ b/src/frame-xi2.c
@@ -217,9 +217,11 @@ static int handle_event(utouch_frame_handle fh,
         switch (fh->evmap[ix]) {
         case ABS_MT_POSITION_X:
                 slot->x = value;
+                slot->vx = 0;
                 return 1;
         case ABS_MT_POSITION_Y:
                 slot->y = value;
+                slot->vy = 0;
                 return 1;
         case ABS_MT_TOUCH_MAJOR:
                 slot->touch_major = value;
diff --git a/src/frame.c b/src/frame.c
index 32bd2cd..f1c8aa4 100644
--- a/src/frame.c
+++ b/src/frame.c
@@ -245,11 +245,14 @@ static void transform_slot(struct utouch_contact *slot,
         slot->distance *= f;
 }
 
-static void copy_contact(utouch_frame_handle fh,
-                         struct utouch_contact *a,
-                         const struct utouch_contact *b)
+static void set_contact(utouch_frame_handle fh,
+                        struct utouch_contact *a,
+                        struct utouch_contact *b,
+                        utouch_frame_time_t dt)
 {
+        static const float D = 0.333;
         struct utouch_surface *s = fh->surface;
+        const struct utouch_contact *ap = a->prev;
 
         a->active = b->id != -1;
         a->id = b->id;
@@ -267,6 +270,21 @@ static void copy_contact(utouch_frame_handle fh,
         a->distance = b->distance;
 
         transform_slot(a, s);
+
+        if (a->active && ap->active && a->id == ap->id) {
+                a->x += b->vx * dt;
+                a->y += b->vy * dt;
+                if (dt > 0) {
+                        a->vx = (1 - D) * ap->vx + D * (a->x - ap->x) / dt;
+                        a->vy = (1 - D) * ap->vy + D * (a->y - ap->y) / dt;
+                }
+        } else {
+                a->vx = 0;
+                a->vy = 0;
+        }
+
+        b->vx = a->vx;
+        b->vy = a->vy;
 }
 
 static int detect_addrem(const struct utouch_contact *a,
@@ -303,14 +321,18 @@ const struct utouch_frame *utouch_frame_sync(utouch_frame_handle fh,
         const struct utouch_frame *prev = frame->prev;
         struct utouch_frame *next = fh->next;
         int naddrem = 0, nmod = 0;
+        utouch_frame_time_t dt;
         int i;
 
+        frame->time = time ? time : get_time_ms();
         frame->num_active = 0;
+        dt = frame->time - prev->time;
+
         for (i = 0; i < fh->num_slots; i++) {
                 struct utouch_contact *p = frame->slots[i];
-                const struct utouch_contact *q = next->slots[i];
+                struct utouch_contact *q = next->slots[i];
 
-                copy_contact(fh, p, q);
+                set_contact(fh, p, q, dt);
                 if (p->active)
                         frame->active[frame->num_active++] = p;
 
@@ -320,7 +342,6 @@ const struct utouch_frame *utouch_frame_sync(utouch_frame_handle fh,
         if (naddrem + nmod == 0)
                 return 0;
 
-        frame->time = time ? time : get_time_ms();
         if (naddrem == 0 && frame->time < prev->time + fh->hold_ms)
                 return 0;
 
diff --git a/tools/common-defs.h b/tools/common-defs.h
index 0e08bc5..e435f72 100644
--- a/tools/common-defs.h
+++ b/tools/common-defs.h
@@ -72,6 +72,8 @@ static void report_frame(const struct utouch_frame *frame)
                 fprintf(stderr, "  angle:        %f\n", t->orientation);
                 fprintf(stderr, "  pressure:     %f\n", t->pressure);
                 fprintf(stderr, "  distance:     %f\n", t->distance);
+                fprintf(stderr, "  vx:           %f\n", t->vx);
+                fprintf(stderr, "  vy:           %f\n", t->vy);
         }
 
         fprintf(stderr, "sync %d %ld %d %d %d\n",