bbiwarg-original/bbiwarg/Graphics/Output.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using OpenTK;
using OpenTK.Graphics.OpenGL;
using MathNet.Numerics.LinearAlgebra.Single;
using bbiwarg.DataSource;


namespace bbiwarg.Graphics
{
    class Output : GameWindow
    {
        private IVideoDataSource source;
        private Point[] depthPixels;
        private ImageData currentImage;

        private List<IGraphicElement> graphicElements = new List<IGraphicElement>();
        private int VBOid = new int();
        private int IBOid = new int();
        private float[] vertices = new float[0];
        private uint[] triangles = new uint[0];

        private Point palmPoint;
        private Point[] fingerPoints;


        public Output(IVideoDataSource source)
        {
            this.source = source;
            currentImage = source.getImageData();
        }

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

            initializeDepthPixels();
            //initBuffers();
        }

        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);

            source.releaseFrame();
            source.updateFrame();
            currentImage = source.getImageData();

            updateDepthPixels();
            
            foreach (IGraphicElement graphicElement in graphicElements) {
                graphicElement.draw();
            }
            
            //updateBuffer();
            //drawBuffer();

            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);
        }

        private void initializeDepthPixels()
        {
            VertexArray vertexArray = source.getVertexArray();
            int numVertices = vertexArray.getNumVertices();
            depthPixels = new Point[numVertices];

            float size = 0.002f;
            for (int i = 0; i < numVertices; i++) {
                Vertex vertex = vertexArray.getVertex(i);
                Color color = vertexArray.getColor(i);
                Point pixel = new Point(vertex, color, size);
                depthPixels[i] = pixel;
                graphicElements.Add(pixel);
            }

            Vector palmPosition = source.getPalmPosition3D(1);
            Vertex palmVertex = new Vertex(palmPosition[0], palmPosition[2], palmPosition[1]);
            palmPoint = new Point(palmVertex, Color.Yellow, 0.005f);
            graphicElements.Add(palmPoint);

            fingerPoints = new Point[5];
            for (int i = 0; i < 5; i++)
            {
                fingerPoints[i] = new Point(new Vertex(0f, 0f, 0f), Color.Yellow, 0.005f);
            }
        }

        private void updateDepthPixels()
        {
            VertexArray vertexArray = source.getVertexArray();
            int numVertices = vertexArray.getNumVertices();

            for (int i = 0; i < numVertices; i++)
            {
                depthPixels[i].position = vertexArray.getVertex(i);
                depthPixels[i].color = vertexArray.getColor(i);
            }
            Vector palmPosition = source.getPalmPosition3D(1);
            palmPoint.position.x = palmPosition[0];
            palmPoint.position.y = palmPosition[2];
            palmPoint.position.z = palmPosition[1];

            
            Vector[] fingerPositions = source.getFingerTipPositions3D(1);
            DetectionStatus[] fingerStatus = source.getFingerStatus(1);
            for(int i=0; i<5; i++) {
                if(fingerStatus[i] == DetectionStatus.Tracked) {
                   fingerPoints[i].position.x = fingerPositions[i][0];
                   fingerPoints[i].position.y = fingerPositions[i][2];
                   fingerPoints[i].position.z = fingerPositions[i][1];
                } 
                else if(fingerStatus[i] == DetectionStatus.Detected) {
                    graphicElements.Add(fingerPoints[i]);
                    fingerPoints[i].position.x = fingerPositions[i][0];
                    fingerPoints[i].position.y = fingerPositions[i][2];
                    fingerPoints[i].position.z = fingerPositions[i][1];
                }
                else if(fingerStatus[i] == DetectionStatus.Inactive && graphicElements.IndexOf(fingerPoints[i]) > -1) {
                    graphicElements.Remove(fingerPoints[i]);
                }
            }

            
        }

        private void initBuffers()
        {
            GL.EnableClientState(ArrayCap.VertexArray);

            GL.GenBuffers(1, out VBOid);
            GL.GenBuffers(1, out IBOid);

            GL.BindBuffer(BufferTarget.ArrayBuffer, VBOid);
            GL.BufferData(BufferTarget.ArrayBuffer, (IntPtr)(vertices.Length * sizeof(float)), vertices, BufferUsageHint.StreamDraw);
            GL.BindBuffer(BufferTarget.ArrayBuffer, 0);


            GL.BindBuffer(BufferTarget.ElementArrayBuffer, IBOid);
            GL.BufferData(BufferTarget.ElementArrayBuffer, (IntPtr)(triangles.Length * sizeof(int)), triangles, BufferUsageHint.StaticDraw);
            GL.BindBuffer(BufferTarget.ElementArrayBuffer, 0);
        }

        public void updateBuffer()
        {
            List<float> verticesData = new List<float>();
            List<uint> triangleData = new List<uint>();

            foreach (IGraphicElement graphicElement in graphicElements)
            {
                List<Vector> elementVertices = graphicElement.getVertices();
                uint[] elementTriangles = graphicElement.getTriangleIndices();
                Color elementColor = graphicElement.getColor();

                uint c = (uint)verticesData.Count / 6;
                foreach (Vector elementVertex in elementVertices)
                {
                    verticesData.AddRange(convertColor(elementColor));
                    verticesData.AddRange(elementVertex);
                }
                for (int i = 0; i < elementTriangles.Length; i++)
                {
                    triangleData.Add(elementTriangles[i] + c);
                }
            }

            vertices = verticesData.ToArray();
            triangles = triangleData.ToArray();


            GL.BindBuffer(BufferTarget.ArrayBuffer, VBOid);
            GL.BufferData(BufferTarget.ArrayBuffer, (IntPtr)(vertices.Length * sizeof(float)), vertices, BufferUsageHint.StreamDraw);
            GL.BindBuffer(BufferTarget.ArrayBuffer, 0);

            GL.BindBuffer(BufferTarget.ElementArrayBuffer, IBOid);
            GL.BufferData(BufferTarget.ElementArrayBuffer, (IntPtr)(triangles.Length * sizeof(int)), triangles, BufferUsageHint.StaticDraw);
            GL.BindBuffer(BufferTarget.ElementArrayBuffer, 0);

        }

        private void drawBuffer()
        {
            GL.BindBuffer(BufferTarget.ArrayBuffer, VBOid);
            GL.BindBuffer(BufferTarget.ElementArrayBuffer, IBOid);

            GL.InterleavedArrays(InterleavedArrayFormat.C3fV3f, 0, IntPtr.Zero);
            GL.DrawElements(BeginMode.Triangles, triangles.Length, DrawElementsType.UnsignedInt, 0);

            GL.BindBuffer(BufferTarget.ArrayBuffer, 0);
            GL.BindBuffer(BufferTarget.ElementArrayBuffer, 0);
        }

        private List<float> convertColor(Color color)
        {
            List<float> result = new List<float>();
            result.Add(color.R / 255f);
            result.Add(color.G / 255f);
            result.Add(color.B / 255f);
            return result;
        }

        
    }
}