multitouch/src/touch.py

#!/usr/bin/env python
import time
from threading import Thread
from math import atan2, pi

from tuio_server import TuiListener
from logger import Logger
from events import TapEvent, FlickEvent, RotateEvent, PinchEvent, PanEvent


def distance(a, b):
    """
    Calculate the distance between points a and b.
    """
    xa, ya = a
    xb, yb = b

    return ((xb - xa) ** 2 + (yb - ya) ** 2) ** .5


def add(a, b):
    return a + b


class TouchPoint(object):
    def __init__(self, sid, x, y):
        self.sid = sid
        self.x = x
        self.y = y

    def update(self, x, y):
        self.px = self.x
        self.py = self.y
        self.x = x
        self.y = y

    def distance_to(self, other_x, other_y):
        return distance(self.x, self.y, other_x, other_y)

    def init_gesture_data(self, cx, cy):
        self.pinch = self.old_pinch = self.distance_to(cx, cy)
        self.angle = self.old_angle = atan2(self.y - cy, self.x - cx)

    def set_angle(self, angle):
        self.old_angle = self.angle
        self.angle = angle

    def set_pinch(self, pinch):
        self.old_pinch = self.pinch
        self.pinch = pinch

    def angle_diff(self):
        return self.angle - self.old_angle

    def dx(self):
        return int(self.x - self.px)

    def dy(self):
        return int(self.y - self.py)


# Heuristic constants
# TODO: Encapsulate DPI resolution in distance heuristics
SUPPORTED_GESTURES = ('tap', 'pan', 'flick', 'rotate', 'pinch')
TUIO_ADDRESS = ('localhost', 3333)
DOUBLE_TAP_DISTANCE_THRESHOLD = 30
FLICK_VELOCITY_TRESHOLD = 20
TAP_INTERVAL = .200
TAP_TIMEOUT = .200
MAX_MULTI_DRAG_DISTANCE = 100


class MultiTouchListener(Logger):
    def __init__(self, verbose=0, update_rate=60, **kwargs):
        super(MultiTouchListener, self).__init__(**kwargs)
        self.verbose = verbose
        self.last_tap = 0
        self.update_rate = update_rate
        self.points_changed = False
        self.handlers = {}

        # Session id's pointing to point coordinates
        self.points = {}

        self.taps_down = []
        self.taps = []

    def update_centroid(self):
        self.old_centroid = self.centroid
        cx, cy = zip(*[(p.x, p.y) for p in self.points])
        l = len(self.points)
        self.centroid = (reduce(add, cx, 0) / l, reduce(add, cy, 0) / l)

    def analyze(self):
        self.detect_taps()

        if self.points_changed:
            self.update_centroid()

            # Do not try to rotate or pinch while panning
            # This gets rid of a lot of jittery events
            if not self.detect_pan():
                self.detect_rotation_and_pinch()

            self.points_changed = False

    def detect_taps(self):
        if len(self.taps) == 2:
            if distance(*self.taps) > DOUBLE_TAP_DISTANCE_THRESHOLD:
                # Taps are too far away too be a double tap, add 2 separate
                # events
                self.trigger(TapEvent(*self.taps[0]))
                self.trigger(TapEvent(*self.taps[1]))
            else:
                # Distance is within treshold, trigger a 'double tap' event
                self.trigger(TapEvent(*self.taps[0], double=True))

            self.taps = []
        elif len(self.taps) == 1:
            # FIXME: Ignore successive single- and double taps?
            if time.time() - self.last_tap > TAP_TIMEOUT:
                self.trigger(TapEvent(*self.taps[0]))
                self.taps = []

    def detect_rotation_and_pinch(self):
        """
        Rotation is the average angle change between each point and the
        centroid. Pinch is the average distance change from the points to the
        centroid.
        """
        l = len(self.points)

        if 'pinch' not in self.handlers or l < 2:
            return

        rotation = pinch = 0
        cx, cy = self.centroid

        for p in self.points:
            p.set_angle(atan2(p.y - cy, p.x - cx))
            da = p.angle_diff()

            # Assert that angle is in [-pi, pi]
            if da > pi:
                da -= 2 * pi
            elif da < pi:
                da += 2 * pi

            rotation += da

            p.set_pinch(distance(p.x, p.y, cx, cy))
            pinch += p.pinch_diff()

        if rotation:
            self.trigger(RotateEvent(cx, cy, rotation / l, l))

        if pinch:
            self.trigger(PinchEvent(cx, cy, pinch / l, l))

    def detect_pan(self):
        """
        Look for multi-finger drag events. Multi-drag is defined as all the
        fingers moving close-ish together in the same direction.
        """
        l = len(self.points)
        m = MAX_MULTI_DRAG_DISTANCE
        clustered = l == 1 or all([p.distance_to(*self.centroid) <= m \
                                   for p in self.points])
        directions = [(cmp(p.dx(), 0), cmp(p.dy(), 0)) for p in self.points]

        if any(map(all, zip(*directions))) and clustered:
            if l == 1:
                p = self.points[0]
                cx, cy, dx, dy = p.x, p.y, p.dx(), p.dy()
            else:
                cx, cy = self.centroid
                old_cx, old_cy = self.old_centroid
                dx, dy = cx - old_cx, cy - old_cy

            self.trigger(PanEvent(cx, cy, dx, dy, l))

    def point_down(self, sid, x, y):
        if sid in self.points:
            raise KeyError('Point with session id "%s" already exists.' % sid)

        self.points[sid] = p = TouchPoint(sid, x, y)
        self.update_centroid()

        # Detect multi-point gestures
        if len(self.points) > 1:
            p.init_gesture_data(*self.centroid)

            if len(self.points) == 2:
                first_p = self.points[0]
                first_p.init_gesture_data(*self.centroid)

        self.taps_down.append(p)
        self.last_tap = time.time()
        self.points_changed = True

    def point_up(self, sid):
        if sid not in self.points:
            raise KeyError('No point with session id "%s".' % sid)

        p = self.points[sid]
        del self.points[sid]

        # Tap/flick detection
        if p in self.taps_down:
            # Detect if Flick based on movement
            if distance(p.x, p.y, p.px, p.py) > FLICK_VELOCITY_TRESHOLD:
                self.trigger(FlickEvent(p.px, p.py, (p.dx(), p.dy())))
            else:
                if time.time() - self.last_tap < TAP_INTERVAL:
                    # Move from taps_down to taps for us in tap detection
                    self.taps_down.remove(p)
                    self.taps.append((p.x, p.y))

        self.points_changed = True

    def point_move(self, sid, x, y):
        self.points[sid].update(x, y)
        self.points_changed = True

    def _tuio_handler(self, addr, tags, data, source):
        # TODO: Call self.point_{down,up,move}
        pass

    def _tuio_server(self):
        server = OSCServer(TUIO_ADDRESS)
        server.addDefaultHandlers()
        server.addMsgHandler('/tuio', self._tuio_handler)
        server.serve_forever()

    def start(self):
        """
        Start event loop.
        """
        self.log('Starting event loop')

        try:
            # Run TUIO message listener in a different thread
            #thread = Thread(target=self.__class__._tuio_server, args=(self))
            thread = Thread(target=self._tuio_server)
            thread.daemon = True
            thread.start()

            interval = 1. / self.update_rate

            while True:
                self.analyze()
                time.sleep(interval)
        except KeyboardInterrupt:
            self.log('Stopping event loop')

    def bind(self, gesture, handler):
        if gesture not in SUPPORTED_GESTURES:
            raise ValueError('Unsupported gesture "%s".' % gesture)

        if gesture not in self.handlers:
            self.handlers[gesture] = []

        self.handlers[gesture].append(handler)

    def trigger(self, event):
        if event.gesture in self.handlers:
            h = self.handlers[event.gesture]
            self.log('Event triggered: "%s" (%d handlers)' % (event, len(h)))

            for handler in h:
                handler(event)


if __name__ == '__main__':
    def tap(event):
        print 'tap:', event

    loop = MultiTouchListener(verbose=2)
    loop.bind('tap', tap)
    loop.start()