Laser Scanner Mathing

'Add Corrections, like level-ness correction

Imports OpenTK
Imports OpenTK.Graphics
Imports OpenTK.Graphics.OpenGL

Public Class Form1

    Dim Enc1SetO As New List(Of Integer) 'Horizontal
    Dim Enc2SetO As New List(Of Integer) 'Vertical
    Dim DistSetO As New List(Of Single) 'Laser
    Dim Enc1Set As New List(Of Integer)
    Dim Enc2Set As New List(Of Integer)
    Dim DistSet As New List(Of Single)

    Dim PointsX As New List(Of Single)
    Dim PointsY As New List(Of Single) 'Up
    Dim PointsZ As New List(Of Single)
    Dim colordata As New List(Of Color)

    'OpenTK Declares
    Dim rotateX As Integer = 0
    Dim rotatey As Integer = 1
    Dim rotatez As Integer = 0
    Dim positionx As Integer = 0 '250
    Dim positiony As Integer = 0 '-250
    Dim Scalez As Decimal = 1
    Dim zpos As Integer = 0 '-100
    Dim mousecontrol As Boolean = False
    Dim mousestartrotate As Point
    Dim mousestartposition As Point
    Dim mousestartZpos As Integer
    Dim mousecurrentpos As Point
    Dim mousecontroltype As Integer = 0
    Dim rotateshift As Boolean = False
    Dim translateshift As Boolean = False

    'Open File
    Private Sub OpenToolStripMenuItem_Click(sender As System.Object, e As System.EventArgs) Handles OpenToolStripMenuItem.Click
        OpenFileDialog1.ShowDialog()
    End Sub
    Private Sub OpenFileDialog1_FileOk(sender As System.Object, e As System.ComponentModel.CancelEventArgs) Handles OpenFileDialog1.FileOk

        Enc1Set.Clear()
        Enc2Set.Clear()
        DistSet.Clear()
        Enc1SetO.Clear()
        Enc2SetO.Clear()
        DistSetO.Clear()

        GC.Collect()
        Dim filelines() As String = IO.File.ReadAllLines(OpenFileDialog1.FileName)
        GC.Collect()

        For i = 0 To filelines.Count - 1
            Dim lines() As String = filelines(i).Split(" ")
            Enc1SetO.Add(lines(0))
            Enc2SetO.Add(lines(1))
            DistSetO.Add(lines(2))
            Enc1Set.Add(lines(0))
            Enc2Set.Add(lines(1))
            DistSet.Add(lines(2))
            colordata.Add(Color.White)
        Next


        PointsImported0ToolStripMenuItem.Text = Enc1Set.Count

        EncoderLevelSet()
        SizeColorSet()

    End Sub

    'Math
    Private Sub PointsAngtoCart()

        PointsX.Clear()
        PointsY.Clear()
        PointsZ.Clear()

        For i = 0 To Enc1Set.Count - 1

            'Y is up
            Dim tempang As Single = ((((Enc2Set(i) / 4096) * -360) - 0) * (Math.PI / 180)) 'Vertical, X and Y
            Dim tempx As Single = (Math.Cos(tempang) * DistSet(i))
            Dim tempy As Single = (Math.Sin(tempang) * DistSet(i))
            tempang = (((((Enc1Set(i) / 4096) * 360) - 0) * (Math.PI / 180))) 'Horizontal, X and Z
            Dim tempz As Single = -1 * (tempx * Math.Sin(tempang))
            tempx = tempx * Math.Cos(tempang)

            PointsX.Add(tempx)
            PointsY.Add(tempy)
            PointsZ.Add(tempz)
            GlControl1.Invalidate()

        Next

    End Sub

    'Save File
    Private Sub SaveAsToolStripMenuItem_Click(sender As System.Object, e As System.EventArgs) Handles SaveAsToolStripMenuItem.Click
        SaveFileDialog1.ShowDialog()
    End Sub
    Private Sub SaveFileDialog1_FileOk(sender As System.Object, e As System.ComponentModel.CancelEventArgs) Handles SaveFileDialog1.FileOk

        Dim pointstring(PointsX.Count - 1) As String
        For i = 0 To pointstring.Count - 1

            pointstring(i) = "v " & PointsX(i).ToString & " " & PointsY(i).ToString & " " & PointsZ(i).ToString

        Next

        GC.Collect()
        IO.File.WriteAllLines(SaveFileDialog1.FileName, pointstring)
        GC.Collect()

    End Sub

    'GL control
    Private Sub GlControl1_Load(ByVal sender As Object, ByVal e As System.EventArgs) Handles GlControl1.Load
        GL.ClearColor(Color.Black)
        GlControl1.Invalidate()
        Timer1.Enabled = True
    End Sub
    Private Sub GlControl1_MouseDown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseDown
        mousecontrol = True
        If e.Button = MouseButtons.Right Then
            mousecontroltype = 2
            mousestartposition = New Point(MousePosition.X - positionx, MousePosition.Y - zpos)
        ElseIf e.Button = MouseButtons.Left Then
            mousestartrotate = New Point(MousePosition.X - rotatey, MousePosition.Y - rotateX)
            mousecontroltype = 1
        ElseIf e.Button = MouseButtons.Middle Then
            mousestartZpos = MousePosition.Y - positiony
            mousecontroltype = 3
        End If

    End Sub
    Private Sub GlControl1_MouseMove(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseMove
        If mousecontrol = True Then

            If mousecontroltype = 1 Then
                rotatey = MousePosition.X - mousestartrotate.X
                rotateX = MousePosition.Y - mousestartrotate.Y
            ElseIf mousecontroltype = 2 Then
                positionx = MousePosition.X - mousestartposition.X
                zpos = MousePosition.Y - mousestartposition.Y
            ElseIf mousecontroltype = 3 Then
                positiony = MousePosition.Y - mousestartZpos
            End If

        End If

        mousecurrentpos = e.Location

    End Sub
    Private Sub GlControl1_MouseUp(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseUp
        mousecontrol = False
        mousecontroltype = 0
        GlControl1.Invalidate()
    End Sub
    Private Sub GlControl1_MouseWheel(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseWheel
        Scalez = Scalez + e.Delta / 240
        If Scalez <= 0 Then
            Scalez = 0.1
        End If
        GlControl1.Invalidate()
    End Sub
    Private Sub Timer1_Tick(sender As System.Object, e As System.EventArgs) Handles Timer1.Tick
        GlControl1.Invalidate()
    End Sub
    Private Sub GlControl1_Paint(ByVal sender As Object, ByVal e As System.Windows.Forms.PaintEventArgs) Handles GlControl1.Paint

        GL.Clear(ClearBufferMask.ColorBufferBit)
        GL.Clear(ClearBufferMask.DepthBufferBit)

        Dim perspective As OpenTK.Matrix4 = OpenTK.Matrix4.CreatePerspectiveFieldOfView(1.04, 4 / 3, 1, 10000)
        Dim lookat As OpenTK.Matrix4 = OpenTK.Matrix4.LookAt(0, 20, 20, 0, 0, 0, 0, 1, 0)

        GL.MatrixMode(MatrixMode.Projection)
        GL.LoadIdentity()
        GL.LoadMatrix(perspective)

        GL.MatrixMode(MatrixMode.Modelview)
        GL.LoadIdentity()
        GL.LoadMatrix(lookat)

        GL.Viewport(0, 0, GlControl1.Width, GlControl1.Height) 'Size of window
        GL.Rotate(-rotateX, 1, 0, 0)
        GL.Rotate(rotatey, 0, 1, 0)

        GL.Translate((-positionx * Scalez), (-positiony * Scalez), (zpos * Scalez))
        GL.Scale(Scalez, Scalez, Scalez)

        GL.Enable(EnableCap.DepthTest) 'Enable correct Z Drawings
        GL.DepthFunc(DepthFunction.Less) 'Enable correct Z Drawings
        GL.DepthMask(True)

        Dim vertz((PointsX.Count) * 3) As Single
        For i = 0 To PointsX.Count - 1
            vertz((i * 3) + 0) = PointsX(i)
            vertz((i * 3) + 1) = PointsY(i)
            vertz((i * 3) + 2) = PointsZ(i)
        Next

        'Point Selection
        Dim colorzP((colordata.Count) * 4) As Single
        For i = 0 To colordata.Count - 1
            colorzP((i * 3) + 0) = colordata(i).R / 255
            colorzP((i * 3) + 1) = colordata(i).G / 255
            colorzP((i * 3) + 2) = colordata(i).B / 255
            colorzP((i * 3) + 3) = 0.5
        Next
        GL.PointSize(6)
        GL.EnableClientState(ArrayCap.VertexArray)
        GL.EnableClientState(ArrayCap.ColorArray)
        GL.ColorPointer(4, VertexPointerType.Float, 0, colorzP)
        GL.VertexPointer(3, VertexPointerType.Float, 0, vertz)
        GL.DrawArrays(BeginMode.Points, 0, PointsX.Count)
        GL.DisableClientState(ArrayCap.VertexArray)
        GL.DisableClientState(ArrayCap.ColorArray)

        Dim pixelz(3) As Byte
        GL.ReadPixels(mousecurrentpos.X, GlControl1.Height - mousecurrentpos.Y, 1, 1, PixelFormat.Rgba, PixelType.UnsignedByte, pixelz)
        Label1.Text = pixelz(0) & " " & pixelz(1) & " " & pixelz(2) & " " & pixelz(3)
        GL.Clear(ClearBufferMask.ColorBufferBit)
        GL.Clear(ClearBufferMask.DepthBufferBit)

        Dim colorz((colordata.Count) * 3) As Single
        For i = 0 To colordata.Count - 1
            colorz((i * 3) + 0) = colordata(i).R / 255
            colorz((i * 3) + 1) = colordata(i).G / 255
            colorz((i * 3) + 2) = colordata(i).B / 255
        Next

        'Box
        GL.Begin(BeginMode.LineLoop)
        GL.Color3(Color.Green)
        GL.Vertex3(50, 0, 0)
        GL.Color3(Color.White)
        GL.Vertex3(50, 0, -50)
        GL.Color3(Color.Red)
        GL.Vertex3(0, 0, -50)
        GL.Color3(Color.White)
        GL.Vertex3(-50, 0, -50)
        GL.Color3(Color.Blue)
        GL.Vertex3(-50, 0, 0)
        GL.Color3(Color.White)
        GL.Vertex3(-50, 0, 50)
        GL.Color3(Color.Yellow)
        GL.Vertex3(0, 0, 50)
        GL.Color3(Color.White)
        GL.Vertex3(50, 0, 50)
        GL.Color3(Color.Green)
        GL.Vertex3(50, 0, 0)
        GL.Color3(Color.White)
        GL.End()

        GL.PointSize(My.Settings.PixelSize)
        GL.EnableClientState(ArrayCap.VertexArray)
        GL.EnableClientState(ArrayCap.ColorArray)
        GL.ColorPointer(3, VertexPointerType.Float, 0, colorz)
        GL.VertexPointer(3, VertexPointerType.Float, 0, vertz)
        GL.DrawArrays(BeginMode.Points, 0, PointsX.Count)
        GL.DisableClientState(ArrayCap.VertexArray)
        GL.DisableClientState(ArrayCap.ColorArray)

        GraphicsContext.CurrentContext.VSync = True
        GlControl1.SwapBuffers()


    End Sub

    'Encoder Level
    Private Sub EncoderLevelsToolStripMenuItem_Click(sender As System.Object, e As System.EventArgs) Handles EncoderLevelsToolStripMenuItem.Click
        EncoderLevels.Show()
    End Sub
    Public Sub EncoderLevelSet()
        For i = 0 To Enc2SetO.Count - 1
            Enc2Set(i) = (Enc2SetO(i) - My.Settings.EncoderLevel) Mod 4096
        Next
        PointsAngtoCart()
    End Sub

    'Size and color
    Private Sub ColorToolStripMenuItem_Click(sender As System.Object, e As System.EventArgs) Handles ColorToolStripMenuItem.Click
        ColorSize.Show()
    End Sub
    Public Sub SizeColorSet()
        Select Case My.Settings.Coloring
            Case 0
                For i = 0 To colordata.Count - 1
                    colordata(i) = Color.White
                Next
            Case 1
                Dim max As Single = DistSet.Max()
                For i = 0 To colordata.Count - 1
                    If DistSet(i) >= (max / 2) Then
                        colordata(i) = Color.FromArgb((((DistSet(i) - (max / 2)) / (max / 2)) * 255), 255 - (((DistSet(i) - (max / 2)) / (max / 2)) * 255), 0)
                    ElseIf DistSet(i) < (max / 2) Then
                        colordata(i) = Color.FromArgb(0, ((DistSet(i) / (max / 2)) * 255), 255 - ((DistSet(i) / (max / 2)) * 255))
                    End If
                Next
        End Select

        GlControl1.Invalidate()
    End Sub
End Class