bbiwarg-original/bbiwarg/Graphics/OutputDepthImage.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using OpenTK;
using OpenTK.Graphics.OpenGL;
using System.Drawing;
using System.Diagnostics;

using MathNet.Numerics.LinearAlgebra.Single;
using bbiwarg.DataSource;

namespace bbiwarg.Graphics
{
    class OutputDepthImage: GameWindow
    {
        private IInputProvider input;
        private uint textureId;

        public OutputDepthImage(IInputProvider input): base(3 * 320, 3 * 240)
        {
            this.input = input;
        }

        protected override void OnLoad(EventArgs e)
        {
            base.OnLoad(e);
            Title = "OutputTest";
            GL.ClearColor(Color.Black);

            // transparency
            GL.Enable(EnableCap.Blend);
            GL.BlendEquation(BlendEquationMode.Max);

            //Depth Test 
            GL.Enable(EnableCap.DepthTest);

            // Textures
            GL.Enable(EnableCap.Texture2D);

            GL.GenTextures(1, out textureId);
            GL.BindTexture(TextureTarget.Texture2D, textureId);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear);
        }

        protected override void OnRenderFrame(FrameEventArgs e)
        {
            base.OnRenderFrame(e);
            GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);
            Matrix4 modelview = Matrix4.LookAt(Vector3.Zero, -Vector3.UnitZ, Vector3.UnitY);
            GL.MatrixMode(MatrixMode.Modelview);
            GL.LoadMatrix(ref modelview);

            input.releaseFrame();
            input.updateFrame();

            Stopwatch sw = new Stopwatch();
            sw.Start();

            DepthImage depthImage = input.getDepthImage();
            depthImage.filterMedian(3);

            Vector palmOrigin = input.getPalmPosition2D(1);
            int palmX = (int) palmOrigin.x();
            int palmY = (int) palmOrigin.y();
            int palmDepth = depthImage.getDepth(palmX, palmY);

            int maxHandSize = 150; // mm Wieviel Pixel sind das?
            List<Vector> points = new List<Vector>();
            if (palmX != 0 && palmY != 0)
            {
                depthImage.thresholdDepth(palmDepth - 10, palmDepth + 30);
                depthImage.thresholdPosition(palmX - maxHandSize / 2, palmX + maxHandSize / 2, palmY - maxHandSize / 2, palmY + maxHandSize / 2);
                points = depthImage.getRectPoints();
            }

            short[] textureData = new short[3 * depthImage.getWidth() * depthImage.getHeight()];
            int index = 0;
            for (int y = 0; y < depthImage.getHeight(); ++y) {
                for (int x = 0; x < depthImage.getWidth(); ++x) {
                    // 0 --> 0 / 2000 -> Int16.MaxValue
                    int d = depthImage.getDepth(x, y);
                    short depth = (short) (d * Int16.MaxValue / 2000);
                    textureData[index] = textureData[index + 1] = textureData[index + 2] = depth;
                    index += 3;
                }
            }

            // draw palm origin
            index = 3 * ((int) palmOrigin.y() * depthImage.getWidth() + (int) palmOrigin.x());
            textureData[index + 0] = Int16.MaxValue;
            textureData[index + 1] = 0;
            textureData[index + 2] = 0;

            // draw rect points
            foreach (Vector v in points)
            {
                int x = (int)v.x() % depthImage.getWidth();
                int y = (int)v.y() % depthImage.getHeight();

                for (int i = -1; i < 2; ++i)
                {
                    for (int j = -1; j < 2; ++j)
                    {
                        int px = Math.Min(depthImage.getWidth() - 1, Math.Max(0, x + i));
                        int py = Math.Min(depthImage.getHeight() - 1, Math.Max(0, y + j));
                        index = 3 * (py * depthImage.getWidth() + px);
                        textureData[index + 0] = 0;
                        textureData[index + 1] = 0;
                        textureData[index + 2] = Int16.MaxValue;
            
                    }
                }
            }

            GL.BindTexture(TextureTarget.Texture2D, textureId);
            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgb, depthImage.getWidth(), depthImage.getHeight(), 0,
                          PixelFormat.Rgb, PixelType.Short, textureData);

            float size_2 = 0.5f / 2.0f;
            GL.Begin(PrimitiveType.Quads);
            GL.TexCoord2(0.0, 0.0); GL.Vertex3(-size_2,  size_2, -0.5);
            GL.TexCoord2(1.0, 0.0); GL.Vertex3( size_2,  size_2, -0.5);
            GL.TexCoord2(1.0, 1.0); GL.Vertex3( size_2, -size_2, -0.5);
            GL.TexCoord2(0.0, 1.0); GL.Vertex3(-size_2, -size_2, -0.5);
            GL.End();

            sw.Stop();
            Console.WriteLine(sw.ElapsedMilliseconds);

            SwapBuffers();
        }

        protected override void OnResize(EventArgs e)
        {
            base.OnResize(e);
            GL.Viewport(ClientRectangle.X, ClientRectangle.Y, ClientRectangle.Width, ClientRectangle.Height);
            Matrix4 projection = Matrix4.CreatePerspectiveFieldOfView((float)Math.PI / 3, Width / (float)Height, 0.01f, 3.0f);
            GL.MatrixMode(MatrixMode.Projection);
            GL.LoadMatrix(ref projection);
        }
    }
}