Utilisation de la kinect sur une plage de distance plus réduite.

[minwii.git] / src / kinect / pygamedisplay.py

diff --git a/src/kinect/pygamedisplay.py b/src/kinect/pygamedisplay.py
index 17ab173..321578d 100755 (executable)
--- a/src/kinect/pygamedisplay.py
+++ b/src/kinect/pygamedisplay.py
@@ -14,9 +14,13 @@ import pygame
 SCREEN_SIZE = 640, 480
 SCREEN_TITLE = "Kinect debug"
 FPS = 30
+MOUSE_SCREEN_COORDINATES = 0
+MOUSE_REAL_COORDINATES = 1
+DEFAULT_HAND_DISTANCE_RANGE = 150 # distance de débatement, autour du centre, en mm
 
 class RGB :
-    def __init__(self) :
+    def __init__(self, mouseMode=MOUSE_REAL_COORDINATES) :
+        self.mouseMode = mouseMode
         self.context = Context()
         self.context.init()
         self.imgGene = ImageGenerator()
@@ -48,16 +52,28 @@ class RGB :
         screen = pygame.display.get_surface()
         self.xratio = float(screen.get_size()[0]) / SCREEN_SIZE[0]
         self.yratio = float(screen.get_size()[1]) / SCREEN_SIZE[1]
+        self.handCenteredPosition = None
+        self.pygameScreenWidth, self.pygameScreenHeight = screen.get_size()
     
     def getProjPos(self, realPos) :
         return self.depthGene.to_projective([realPos])[0]
     
     def setMousePosition(self, realPos) :
-        x, y, z = self.getProjPos(realPos)
-        # mirror
-        x = SCREEN_SIZE[0] - x
-        # scale
-        x, y = x * self.xratio, y * self.yratio
+        if self.mouseMode == MOUSE_SCREEN_COORDINATES :
+            x, y, z = self.getProjPos(realPos)
+            # mirror
+            x = SCREEN_SIZE[0] - x
+            # scale
+            x, y = x * self.xratio, y * self.yratio
+        elif self.mouseMode == MOUSE_REAL_COORDINATES :
+            x, y = realPos[:2]
+            x, y = x - self.handCenteredPosition[0], y - self.handCenteredPosition[1]
+            x = -x # miroir
+            x, y = x + DEFAULT_HAND_DISTANCE_RANGE, DEFAULT_HAND_DISTANCE_RANGE - y
+            x, y = map(lambda i : numpy.clip(i, 0, DEFAULT_HAND_DISTANCE_RANGE * 2), [x, y])
+            x = x * (self.pygameScreenWidth / (2. * DEFAULT_HAND_DISTANCE_RANGE))
+            y = y * (self.pygameScreenHeight / (2. * DEFAULT_HAND_DISTANCE_RANGE))
+            print x, y
         pygame.mouse.set_pos(int(x), int(y))
 
     def handCreateCB(self, src, id, pos, time):
@@ -76,6 +92,7 @@ class RGB :
         print 'gestureProgress', src, gesture, point, progress
         self.handsGene.start_generating()
         self.handsGene.start_tracking(point)
+        self.handCenteredPosition = point
 
     def newUserCB(self, *args, **kw) :
         print 'newUserCB', args, kw