From d619da633517175ffa6977a80760673628f91583 Mon Sep 17 00:00:00 2001
From: Henrik Rydberg <rydberg@euromail.se>
Date: Wed, 16 Feb 2011 18:20:14 +0100
Subject: 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>
---
 include/utouch/frame.h |  4 ++++
 src/frame-mtdev.c      |  2 ++
 src/frame-xi2.c        |  2 ++
 src/frame.c            | 33 +++++++++++++++++++++++++++------
 tools/common-defs.h    |  2 ++
 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",
-- 
cgit v1.2.3