Brought rotation back to point instead of global state.

src/touch.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+from __future__ import division
 import time
 import pygame.display
 from math import atan2, pi
@@ -35,6 +36,15 @@ GESTURE_UPDATE_RATE = 60
 DIST_THRESHOLD = 1. / max(screen_size)
 
 
+def angle_mod(angle):
+    if angle > pi:
+        angle -= 2 * pi
+    elif angle < -pi:
+        angle += 2 * pi
+
+    return angle
+
+
 def distance(a, b):
     """
     Calculate the Pythagorian distance between points a and b (which are (x, y)
@@ -46,11 +56,11 @@ def distance(a, b):
     return ((xb - xa) ** 2 + (yb - ya) ** 2) ** .5
 
 
-def add(a, b):
-    """
-    Add a an b, used for some functional programming calls.
-    """
-    return a + b
+def average(points):
+    l = len(points)
+    x, y = zip(*points)
+
+    return sum(x) / l, sum(y) / l
 
 
 # Maximum distance between touch- and release location of a tap event
@@ -66,6 +76,7 @@ class TouchPoint(object):
         self.sid = sid
         self.start_x = self.px = self.x = x
         self.start_y = self.py = self.y = y
+        self.prev_angle = None
 
     @property
     def xy(self):
@@ -75,7 +86,7 @@ class TouchPoint(object):
         return self.x - self.px
 
     def dy(self):
-        return self.y- self.py
+        return self.y - self.py
 
     def update(self, x, y):
         self.update_time = time.time()
@@ -103,6 +114,22 @@ class TouchPoint(object):
     def is_stationary(self):
         return self.distance_to_prev() < DIST_THRESHOLD
 
+    def angle_diff(self, cx, cy):
+        angle = atan2(cy - self.y, self.x - cx)
+
+        if self.prev_angle == None:
+            delta = 0
+        elif self.prev_angle > 0 and angle < 0:
+            delta = -2 * pi - angle + self.prev_angle
+        elif self.prev_angle < 0 and angle > 0:
+            delta = 2 * pi - angle + self.prev_angle
+        else:
+            delta = angle - self.prev_angle
+
+        self.prev_angle = angle
+
+        return delta
+
 
 class MultiTouchListener(Logger):
     def __init__(self, verbose=0, tuio_verbose=0, **kwargs):
@@ -123,6 +150,7 @@ class MultiTouchListener(Logger):
         self.server = TuioServer2D(self, verbose=tuio_verbose)
 
         self.angle = self.pinch = None
+        self.prev_pinch = None
 
     def point_down(self, sid, x, y):
         """
@@ -201,50 +229,26 @@ class MultiTouchListener(Logger):
 
         if l < 2 or ('pinch' not in self.handlers \
                      and 'rotate' not in self.handlers):
-            self.set_state(None, None)
+            self.prev_pinch = None
             return
 
-        self.calc_state()
-        rotation, pinch = self.state_diff()
+        # Get the sum of all angle differences
+        l = len(self.points)
         cx, cy = self.centroid
+        #rcx, rcy = self.rot_centroid
+        rotation = sum([p.angle_diff(cx, cy) for p in self.points]) / l
+        pinch = sum([p.distance(cx, cy) for p in self.points]) / l
 
         if rotation:
             self.trigger(Rotate(cx, cy, rotation, l))
 
-        if pinch:
-            self.trigger(Pinch(cx, cy, pinch, l))
-
-    def calc_state(self):
-        l = len(self.points)
-        cx, cy = self.centroid
-        angle = reduce(add, [p.angle(cx, cy) for p in self.points]) / l
-        distance = reduce(add, [p.distance(cx, cy) for p in self.points]) / l
-
-        if self.angle == None:
-            self.set_state(angle, distance)
-            self.set_prev_state()
+        if self.prev_pinch == None:
+            self.prev_pinch = pinch
         else:
-            self.set_prev_state()
-            self.set_state(angle, distance)
-
-    def set_state(self, angle, distance):
-        self.angle = angle
-        self.distance = distance
-
-    def set_prev_state(self):
-        self.prev_angle = self.angle
-        self.prev_distance = self.distance
-
-    def state_diff(self):
-        da = self.angle - self.prev_angle
-
-        # Assert that angle is in [-pi, pi]
-        #if da > pi:
-        #    da %= 2 * pi
-        #elif da < -pi:
-        #    da %= -2 * pi
+            pinch -= self.prev_pinch
 
-        return da, self.distance / self.prev_distance
+            if pinch:
+                self.trigger(Pinch(cx, cy, pinch, l))
 
     def update_centroid(self):
         """
@@ -258,14 +262,12 @@ class MultiTouchListener(Logger):
             self.centroid = -1., -1.
             return
 
-        # Only use stationary points, if any
-        use = filter(TouchPoint.is_stationary, self.points)
+        self.centroid = average([p.xy for p in self.points])
 
-        if not len(use):
-            use = self.points
+        # Only use stationary points for rotational centroid, if any
+        stat = [p.xy for p in filter(TouchPoint.is_stationary, self.points)]
+        self.rot_centroid = average(stat) if stat else self.centroid
 
-        cx, cy = zip(*[(p.x, p.y) for p in use])
-        self.centroid = reduce(add, cx, 0) / l, reduce(add, cy, 0) / l
 
     def centroid_movement(self):
         cx, cy = self.centroid