bbiwarg-original/bbiwarg/Graphics/Output.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
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 IVideoHandle videoHandle;

        private uint imageBufferId, pointBufferId, quadBufferId;

        public Output(IVideoHandle videoHandle)
        {
            this.videoHandle = videoHandle;
        }

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

            initBuffers();

            GL.Enable(EnableCap.Blend);
            GL.BlendFunc(BlendingFactorSrc.SrcAlpha, BlendingFactorDest.OneMinusSrcAlpha);
        }

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

            videoHandle.nextFrame();

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

            GL.EnableClientState(ArrayCap.VertexArray);
            GL.EnableClientState(ArrayCap.ColorArray);

            GL.BindBuffer(BufferTarget.ArrayBuffer, imageBufferId);
            videoHandle.createVertexArray(GL.MapBuffer(BufferTarget.ArrayBuffer, BufferAccess.WriteOnly));
            GL.UnmapBuffer(BufferTarget.ArrayBuffer);

            GL.VertexPointer(3, VertexPointerType.Float, 3 * sizeof(float) + 4 * sizeof(byte), IntPtr.Zero);
            GL.ColorPointer(4, ColorPointerType.UnsignedByte, 3 * sizeof(float) + 4 * sizeof(byte), 3 * sizeof(float));

            GL.PointSize(2.0f);
            GL.DrawArrays(PrimitiveType.Points, 0, videoHandle.getWidth() * videoHandle.getHeight());

            // draw points
            float[] pointData;
            
            DetectionStatus[] fingerStatus = videoHandle.getFingerStatus(1);
            int numFingersDetected = 0;
            for (int i = 0; i < fingerStatus.Length; ++i)
            {
                if (fingerStatus[i] == DetectionStatus.Detected || fingerStatus[i] == DetectionStatus.Tracked)
                    ++numFingersDetected;
            }

            pointData = new float[(4 + 3) * (3 + numFingersDetected)];
            Color y = Color.Yellow;
            Vector palmPosition = videoHandle.getPalmPosition3D(1);
            Vector foreFingerPosition = new DenseVector(new float[] { 0, 0, 0 }); //videoHandle.getForeFingerPosition3D(1);
            Vector foreArmPosition = videoHandle.getForearmPosition3D(1);

            pointData[0] = palmPosition[0];
            pointData[1] = palmPosition[1];
            pointData[2] = -palmPosition[2];

            pointData[3] = y.R / 255.0f;
            pointData[4] = y.G / 255.0f;
            pointData[5] = y.B / 255.0f;
            pointData[6] = y.A / 255.0f;

            pointData[7] = foreArmPosition[0];
            pointData[8] = foreArmPosition[1];
            pointData[9] = -foreArmPosition[2];

            pointData[10] = y.R / 255.0f;
            pointData[11] = y.G / 255.0f;
            pointData[12] = y.B / 255.0f;
            pointData[13] = y.A / 255.0f;


            int index = 14;
            Vector[] fingerPositions = videoHandle.getFingerTipPositions3D(1);
            for (int i = 0; i < fingerStatus.Length; ++i)
            {
                if (fingerStatus[i] == DetectionStatus.Detected || fingerStatus[i] == DetectionStatus.Tracked)
                {
                    pointData[index + 0] = fingerPositions[i][0];
                    pointData[index + 1] = fingerPositions[i][1];
                    pointData[index + 2] = -fingerPositions[i][2];

                    pointData[index + 3] = y.R / 255.0f;
                    pointData[index + 4] = y.G / 255.0f;
                    pointData[index + 5] = y.B / 255.0f;
                    pointData[index + 6] = y.A / 255.0f;
                    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);
            GL.BufferData(BufferTarget.ArrayBuffer, (IntPtr)(pointData.Length * sizeof(float)), pointData, BufferUsageHint.DynamicDraw);
            GL.VertexPointer(3, VertexPointerType.Float, (4 + 3) * sizeof(float), IntPtr.Zero);
            GL.ColorPointer(4, ColorPointerType.Float, (4 + 3) * sizeof(float), 3 * sizeof(float));

            GL.PointSize(10f);
            GL.DrawArrays(PrimitiveType.Points, 0, pointData.Length / (4 + 3));

            // draw quads
            float[] quadData = new float[4 * (4 + 3) * 1];

            Palm palm = videoHandle.getPalm(1);
            Vector ul = palm.getUpperLeft();
            Vector ur = palm.getUpperRight();
            Vector lr = palm.getLowerRight();
            Vector ll = palm.getLowerLeft();

            quadData[0] = ul.x();
            quadData[1] = ul.y();
            quadData[2] = -ul.z();

            quadData[3] = y.R / 255.0f;
            quadData[4] = y.G / 255.0f;
            quadData[5] = y.B / 255.0f;
            quadData[6] = 0.5f;

            quadData[7] = ur.x();
            quadData[8] = ur.y();
            quadData[9] = -ur.z();

            quadData[10] = y.R / 255.0f;
            quadData[11] = y.G / 255.0f;
            quadData[12] = y.B / 255.0f;
            quadData[13] = 0.5f;

            quadData[14] = lr.x();
            quadData[15] = lr.y();
            quadData[16] = -lr.z();

            quadData[17] = y.R / 255.0f;
            quadData[18] = y.G / 255.0f;
            quadData[19] = y.B / 255.0f;
            quadData[20] = 0.5f;

            quadData[21] = ll.x();
            quadData[22] = ll.y();
            quadData[23] = -ll.z();

            quadData[24] = y.R / 255.0f;
            quadData[25] = y.G / 255.0f;
            quadData[26] = y.B / 255.0f;
            quadData[27] = 0.5f;

            GL.BindBuffer(BufferTarget.ArrayBuffer, 0);
            GL.BindBuffer(BufferTarget.ArrayBuffer, quadBufferId);
            GL.BufferData(BufferTarget.ArrayBuffer, (IntPtr)(quadData.Length * sizeof(float)), quadData, BufferUsageHint.DynamicDraw);
            GL.VertexPointer(3, VertexPointerType.Float, (4 + 3) * sizeof(float), IntPtr.Zero);
            GL.ColorPointer(4, ColorPointerType.Float, (4 + 3) * sizeof(float), 3 * sizeof(float));

            GL.DrawArrays(PrimitiveType.Quads, 0, quadData.Length / (4 + 3));

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

        private void initBuffers()
        {
            GL.GenBuffers(1, out imageBufferId);
            GL.GenBuffers(1, out pointBufferId);
            GL.GenBuffers(1, out quadBufferId);

            GL.BindBuffer(BufferTarget.ArrayBuffer, imageBufferId);
            GL.BufferData(BufferTarget.ArrayBuffer, 
                          (IntPtr)((3 * sizeof(float) + 4  * sizeof(byte)) * videoHandle.getWidth() * videoHandle.getHeight()), 
                          IntPtr.Zero, BufferUsageHint.StreamDraw);
        } 
    }
}