Add "Vector pixel2VertexPosition(Vector)" to IVideoHandle/VideoHandle and add the foreFingerPosition to the output-data.

bbiwarg/DataSource/ForeFingerDetection.cs

@@ -23,6 +23,7 @@ namespace bbiwarg.DataSource
         public const int TOLERANCE_2D = 5;
 
         private IVideoHandle source;
+
         private HashSet<Vector> seenPoints;
         private Vector palmPosition;
         private Vector foreFingerPosition;
@@ -37,14 +38,15 @@ namespace bbiwarg.DataSource
 
         public Vector getForeFingerPosition3D(uint handIndex)
         {
-            seenPoints = new HashSet<Vector>();
             Vector palmPosition2D = source.getPalmPosition2D(handIndex);
             palmPosition = new DenseVector(3);
             palmPosition[0] = palmPosition2D[0];
             palmPosition[1] = palmPosition2D[1];
             palmPosition[2] = source.getDepth((int)palmPosition[0], (int)palmPosition[1]);
 
+            seenPoints = new HashSet<Vector>();
             foreFingerPosition = palmPosition;
+            foreFingerDistance = 0;
             checkPoint(palmPosition);
             return foreFingerPosition;
         }

bbiwarg/DataSource/IVideoHandle.cs

@@ -32,6 +32,7 @@ namespace bbiwarg.DataSource
 
         Vector getPalmPosition2D(uint handIndex);
         Vector getPalmPosition3D(uint handIndex);
+        Vector pixel2VertexPosition(Vector pixelPosition);
         int getWidth();
         int getHeight();
     }

bbiwarg/DataSource/VideoHandle.cs

@@ -109,22 +109,34 @@ namespace bbiwarg.DataSource
             }
         }
 
-        private void create3DVertexFrom2D(float pixelX, float pixelY, int depth, Color c, int index, IntPtr vertexBuffer)
+        public Vector pixel2VertexPosition(Vector pixelPosition)
         {
             int width = depthImage.getWidth();
             int height = depthImage.getHeight();
 
-            float convertedDepth = depth / 1000f; // mm into m
+            float convertedDepth = pixelPosition[2] / 1000f; // mm into m
 
-            float u = (pixelX / (float)width - 0.5f) * 2f;
-            float v = ((1 - pixelY / (float)height) - 0.5f) * 2f;
+            float u = (pixelPosition[0] / (float)width - 0.5f) * 2f;
+            float v = ((1 - pixelPosition[1] / (float)height) - 0.5f) * 2f;
 
             float relX = (u * maxU);
             float relY = (v * maxV);
 
-            float z = convertedDepth / (float)Math.Sqrt(1 + relX * relX + relY * relY);
-            float x = relX * z;
-            float y = relY * z;
+            Vector result = new DenseVector(3);
+            result[2] = -convertedDepth / (float)Math.Sqrt(1 + relX * relX + relY * relY);
+            result[0] = -relX * result[2];
+            result[1] = -relY * result[2];
+
+            return result;
+        }
+        private void create3DVertexFrom2D(float pixelX, float pixelY, int depth, Color c, int index, IntPtr vertexBuffer)
+        {
+            Vector pixelPosition = new DenseVector(3);
+            pixelPosition[0] = pixelX;
+            pixelPosition[1] = pixelY;
+            pixelPosition[2] = depth;
+            Vector vertexPosition = pixel2VertexPosition(pixelPosition);
+
 
             int i4 = (3 * sizeof(float) + 4 * sizeof(byte)) / sizeof(float) * index;
             int i16 = (3 * sizeof(float) + 4 * sizeof(byte)) * index;
@@ -133,9 +145,9 @@ namespace bbiwarg.DataSource
                 byte* vertexArrayB = (byte*)vertexBuffer.ToPointer();
                 float* vertexArrayF = (float*)vertexBuffer.ToPointer();
 
-                vertexArrayF[i4 + 0] = x;
-                vertexArrayF[i4 + 1] = y;
-                vertexArrayF[i4 + 2] = -z;
+                vertexArrayF[i4 + 0] = vertexPosition[0];
+                vertexArrayF[i4 + 1] = vertexPosition[1];
+                vertexArrayF[i4 + 2] = vertexPosition[2];
 
                 vertexArrayB[i16 + 12] = c.R;
                 vertexArrayB[i16 + 13] = c.G;

bbiwarg/Graphics/Output.cs

@@ -17,6 +17,7 @@ namespace bbiwarg.Graphics
     {
         private IInputProvider inputProvider;
         private IVideoHandle videoHandle;
+        private ForeFingerDetection foreFingerDetection;
         
         private uint imageBufferId, pointBufferId;
 
@@ -24,6 +25,7 @@ namespace bbiwarg.Graphics
         {
             this.inputProvider = inputProvider;
             this.videoHandle = videoHandle;
+            this.foreFingerDetection = new ForeFingerDetection(videoHandle);
         }
 
         protected override void OnLoad(EventArgs e)
@@ -72,9 +74,10 @@ namespace bbiwarg.Graphics
                     ++numFingersDetected;
             }
 
-            pointData = new float[(4 + 3) * (1 + numFingersDetected)];
+            pointData = new float[(4 + 3) * (2 + numFingersDetected)];
             Color y = Color.Yellow;
             Vector palmPosition = videoHandle.getPalmPosition3D(1);
+            Vector foreFingerPosition = videoHandle.pixel2VertexPosition(foreFingerDetection.getForeFingerPosition3D(1));
 
             pointData[0] = palmPosition[0];
             pointData[1] = palmPosition[1];
@@ -102,6 +105,14 @@ namespace bbiwarg.Graphics
                     index += 7;
                 }
             }
+            pointData[index + 0] = foreFingerPosition[0];
+            pointData[index + 1] = foreFingerPosition[1];
+            pointData[index + 2] = foreFingerPosition[2];
+            pointData[index + 3] = Color.Red.R / 255.0f;
+            pointData[index + 4] = Color.Red.G / 255.0f;
+            pointData[index + 5] = Color.Red.B / 255.0f;
+            pointData[index + 6] = Color.Red.A / 255.0f;
+            index += 7;
 
             GL.BindBuffer(BufferTarget.ArrayBuffer, 0);
             GL.BindBuffer(BufferTarget.ArrayBuffer, pointBufferId);