Fixed rotation (finally).

src/touch.py

@@ -36,13 +36,19 @@ 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
+def rotation(p0, p1):
+    """
+    Calculate the relative rotation from p0 to p1 around (0, 0) in radians.
+    """
+    rot = atan2(*p1) - atan2(*p0)
 
-    return angle
+    # Assert that the smallest possible rotation value is returned
+    if rot >= pi:
+        rot = 2 * pi - rot
+    elif rot <= -pi:
+        rot = -2 * pi - rot
+
+    return rot
 
 
 def distance(a, b):
@@ -114,21 +120,11 @@ 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
+    def rotation_around(self, cx, cy):
+        prev = self.px - cx, self.py - cy
+        current = self.x - cx, self.y - cy
 
-        return delta
+        return rotation(prev, current)
 
 
 class MultiTouchListener(Logger):
@@ -146,11 +142,12 @@ class MultiTouchListener(Logger):
 
         # Put centroid outside screen to prevent misinterpretation
         self.centroid = -1., -1.
+        self.rot_centroid = -1., -1.
 
         self.server = TuioServer2D(self, verbose=tuio_verbose)
 
         self.angle = self.pinch = None
-        self.prev_pinch = None
+        self.prev_dist = None
 
     def point_down(self, sid, x, y):
         """
@@ -222,51 +219,43 @@ class MultiTouchListener(Logger):
     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.
+        centroid. Pinch is the percentual change in average distance change
+        from the points to the centroid.
         """
         l = len(self.points)
 
         if l < 2 or ('pinch' not in self.handlers \
                      and 'rotate' not in self.handlers):
-            self.prev_pinch = None
+            self.prev_dist = None
             return
 
-        # 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
+        rotation = sum([p.rotation_around(cx, cy) for p in self.points]) / l
+        dist = sum([p.distance(cx, cy) for p in self.points]) / l
 
         if rotation:
             self.trigger(Rotate(cx, cy, rotation, l))
 
-        if self.prev_pinch == None:
-            self.prev_pinch = pinch
-        else:
-            pinch -= self.prev_pinch
+        if self.prev_dist != None:
+            pinch = dist / self.prev_dist
 
             if pinch:
                 self.trigger(Pinch(cx, cy, pinch, l))
 
+        self.prev_dist = dist
+
     def update_centroid(self):
         """
         Calculate the centroid of all current touch points.
         """
         self.old_centroid = self.centroid
-        l = len(self.points)
-
-        # If there are no touch points, move the centroid to outside the screen
-        if not l:
-            self.centroid = -1., -1.
-            return
 
+        if self.points:
             self.centroid = average([p.xy for p in 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
+        else:
+            # There are no touch points, move the centroid out of the screen
+            self.centroid = -1., -1.
 
 
     def centroid_movement(self):