#Region "System Includes"
Imports WindowsApplication1.Form1
Imports WindowsApplication1.NJCM.Types
Imports System.Runtime.InteropServices
#End Region
#Region "C/C++ types in VB.NET"
Imports qword = System.UInt64
Imports dword = System.UInt32
Imports float = System.Single
Imports word = System.UInt16
Imports bool = System.Boolean
#End Region
Public Module Utility
    <StructLayout(LayoutKind.Explicit)> _
    Public Structure Union
        <FieldOffset(0)> Public sq As Int64
        <FieldOffset(0)> Public uq As UInt64
        <FieldOffset(0)> Public LoDword As UInt32
        <FieldOffset(2)> Public MdDword As UInt32
        <FieldOffset(4)> Public HiDword As UInt32
        <FieldOffset(0)> Public LoInt32 As Int32
        <FieldOffset(4)> Public HiInt32 As Int32
    End Structure
    Public Function GetVersion() As String
        Return "1.1.0.0 14 June 2008"
    End Function
    Public ControlFlag As Boolean = False
    Public Sub WriteLn(ByVal A As String, Optional ByVal indent As Integer = 0)
        Form1.rt1.SelectionStart = Len(Form1.rt1.Text) + 1
        Form1.rt1.SelectedText = New String(" ", 5 * indent) & A & vbCrLf
        Form1.rt1.Select()
        Application.DoEvents()
    End Sub
    Public Sub Report(ByVal A As String, Optional ByVal level As Integer = 0)
        If Form1.Verbosity = True Then
            If Form1.VerbosityLevel >= level Then WriteLn(A)
        End If
    End Sub
    Public Sub Cls()
        Form1.rt1.Text = ""
        Application.DoEvents()
    End Sub
    Public Function BitTest(ByVal A As Integer, ByVal B As Integer) As Boolean
        BitTest = (((A >> B) And &H1) = 1)
    End Function
    Public Function BitSet(ByVal A As Integer, ByVal B As Integer, ByVal C As Boolean) As Integer
        'A is intial 32 bit value to set bit in
        'B is Bit to Set
        'C is Value to set bit to...
        Dim a1 As Integer = A And ((1 << B) Xor &HFFFFFFFF)
        If C Then
            Return (a1 Or (1 << B))
        Else
            Return a1
        End If
    End Function
    Public Function BitWise(ByVal A As Integer, ByVal B As Integer) As Boolean
        BitWise = ((A And B) = B)
    End Function
    Public Function HexOut(ByVal N As Object) As String
        'Supercedes PadHex and PadHex2, much neater and nicer.
        Dim L As Integer = 8
        Dim q As String = ""
        If TypeOf N Is UInt64 Then L = 16
        If TypeOf N Is Int64 Then L = 16
        If TypeOf N Is UInt32 Then L = 8
        If TypeOf N Is Int32 Then L = 8
        If TypeOf N Is Integer Then L = 8
        If TypeOf N Is UInt16 Then L = 4
        If TypeOf N Is Int16 Then L = 4
        If TypeOf N Is Byte Then L = 2

        Dim s As String = Hex(N)

        If (L - s.Length) > 0 Then
            q = New String("0", L - s.Length)
        End If
        HexOut = "0x" & q & s
    End Function
    Public Function BinOut(ByVal N As Object) As String
        Dim L As Integer = 8
        Dim m As Integer = 0
        Dim retv As String = ""
        If TypeOf N Is UInt64 Then L = 16
        If TypeOf N Is Int64 Then L = 16
        If TypeOf N Is UInt32 Then L = 8
        If TypeOf N Is Int32 Then L = 8
        If TypeOf N Is Integer Then L = 8
        If TypeOf N Is UInt16 Then L = 4
        If TypeOf N Is Int16 Then L = 4
        If TypeOf N Is Byte Then L = 2
        'Number of Bits
        L *= 4
        For I As Integer = (L - 1) To 0 Step -1
            If BitTest(N, I) Then
                retv &= "1"
            Else
                retv &= "0"
            End If
            m += 1
            If m = 4 Then retv &= " " : m = 0
        Next
        Return retv
    End Function
    Public Function Format2(ByVal N As Single, Optional ByVal F As String = "") As String
        Dim t As String = Format(N, F)
        If t.Length < F.Length Then t = New String(" ", F.Length - t.Length) & t
        Format2 = t
    End Function
    Public Function StepControl() As Boolean
        Form1.Button1.Enabled = True
        Do While ControlFlag = False
            Application.DoEvents()
        Loop
        Form1.Button1.Enabled = False
        ControlFlag = False
    End Function
    Sub Swap(ByRef A As Object, ByRef B As Object)
        Dim C As Object = A
        A = B
        B = C
    End Sub
    Public Function MakeBytes(ByVal A As String) As Byte()
        Return System.Text.Encoding.ASCII.GetBytes(A)
    End Function
    Public Function MakeString(ByVal A() As Byte) As String
        Dim tmp As String = Strings.Trim(System.Text.Encoding.ASCII.GetString(A))
        Dim q As Integer = Strings.InStr(tmp, Chr(0))
        If q > 0 Then tmp = Strings.Mid(tmp, 1, q - 1)
        Return tmp
    End Function
    Public Function MakeYN(ByVal A As Boolean, Optional ByVal YC As Char = "Y", Optional ByVal NC As Char = "N") As Char
        If A Then
            If YC = "" Then Return "Y" Else Return YC
        Else
            If NC = "" Then Return "N" Else Return NC
        End If
    End Function
    Sub BufferToFile(ByVal buf As Byte(), ByVal Fname As String)
        'Write an Array of Byte to a file.  Do Not Append
        'Easier to Remember than what follows below... :)
        My.Computer.FileSystem.WriteAllBytes(Fname, buf, False)
    End Sub
    Function FileToBuffer(ByVal Fname As String) As Byte()
        'Read a file into a memory buffer
        'Easier to type than what follows below... :)
        Return My.Computer.FileSystem.ReadAllBytes(Fname)
    End Function
    Function bufcpy(ByVal src() As Byte, ByVal Index As Integer, ByVal count As Integer) As Byte()
        If (Index + count) > src.Length Then count = src.Length - Index
        Dim t(count) As Byte
        For I As Integer = 0 To count - 1
            t(I) = src(Index + I)
        Next
        Return t
    End Function
    Function Min(ByVal A As Integer, ByVal B As Integer) As Integer
        If A < B Then Return A Else Return B
    End Function
    Function memcmp(ByVal src1() As Byte, ByVal src2() As Byte, ByVal bytes As Integer) As Boolean
        Dim t As Boolean = True
        Dim i As Integer = 0
        Dim tl As Integer = Min(Min(src1.Length - 1, src2.Length - 1), bytes)
        Do
            t = t And (src1(i) = src2(i))
            i += 1
        Loop Until i = tl
        Return t
    End Function
    Function BooltoInt(ByVal A As Boolean) As Integer
        If A = True Then Return 1 Else Return 0
    End Function
    Function IntToBool(ByVal A As Integer) As Boolean
        If A = 0 Then Return False Else Return True
    End Function
    Public Function StripFileName(ByVal A As String) As String
        Dim retv As String = ""
        Dim q1 As Integer = InStrRev(A, ".")
        Dim q2 As Integer = InStrRev(A, "\")
        retv = Mid(A, q2 + 1, ((q1 - 1) - q2))
        Return retv
    End Function
    Public Function StripFilePath(ByVal A As String) As String
        Dim retv As String = ""
        Dim q1 As Integer = InStrRev(A, "\")
        retv = Strings.Left(A, q1 - 1)
        Return retv
    End Function
    Function evalcatmullrom(ByVal t As Single, ByVal p0 As Single, ByVal p1 As Single, ByVal p2 As Single, ByVal p3 As Single) As Single
        'Generic Evaluator for Catmull-Rom spline.
        Dim q As Single = 0
        q = 0.5 * ((2 * p1) + (-p0 + p2) * t + (2 * p0 - 5 * p1 + 4 * p2 - p3) * t ^ 2 + (-p0 + 3 * p1 - 3 * p2 + p3) * t ^ 3)
        Return q
    End Function
    Function evalbicubic(ByVal t As Single, ByVal p0 As Single, ByVal p1 As Single, ByVal p2 As Single, ByVal p3 As Single) As Single
        Dim q As Single = 0
        Dim tv As Single = 1 - t
        q = (tv ^ 3) * p0
        q += ((3 * t) * (tv ^ 2)) * p1
        q += ((3 * t) ^ 2 * (1 - t)) * p2
        q += (t ^ 3) * p3
        Return q
    End Function
    Function interpolate(ByVal t As Single, ByVal V0 As Single, ByVal V1 As Single, Optional ByVal m As Integer = 0) As Single
        'Parametric interpolation of values. m controls how t is generated.0 = linear, 1 = catmull-rom spline, 2 = bicubic spline
        'function defaults to 0, linear interpolation
        Dim lt As Single = 0
        Select Case m
            Case Is = 0
                lt = t
            Case Is = 1
                lt = evalcatmullrom(t, 0, 0, 1, 0)
            Case Is = 2
                lt = evalbicubic(t, 0, 0.15, 0.85, 1)
            Case Else
                lt = t
        End Select
        Return (1 - lt) * V0 + (lt * V1)
    End Function
    Function FibHash(ByVal v As Single) As Integer
        Dim u As Union
        Dim t As Long = 0
        t = v.GetHashCode
        If Math.Sign(v) = -1 Then
            t = Int((Math.Abs(t) * (2 * Math.E)) - Math.E)
        Else
            t = Int(Math.Abs(t) * (2 * Math.E))
        End If
        u.sq = t
        Return u.LoInt32 And &H7FFFFFFF
    End Function
End Module
Public Module MatrixMath
   Const FACTOR As Double = 4294967296
   Const RAD As Double = 3.1415926535897309 / 180.0
    Public Stack As New Collection
    Public MatStack As New Stack(Of Matrix)

   Public Function Identity() As Matrix
      Identity = New Matrix
   End Function

   Private Function Normalize(ByVal a As Matrix) As Matrix
      Dim tmp As New Matrix
      Dim I As Integer = 0
      Dim J As Integer = 0
      For I = 1 To 4
         For J = 1 To 4
            tmp.m(I, J) = a.m(I, J) / a.m(4, 4)
         Next
      Next
      Return tmp
   End Function
   Public Function multiply(ByVal a As Matrix, ByVal b As Matrix) As Matrix
      Dim tmp As New Matrix
      Dim i, j, k As Integer
      Dim q, r As Single
      Dim t As Single = 0
      For i = 1 To 4
         For j = 1 To 4
            t = 0.0
            For k = 1 To 4
               q = a.m(i, k)
               r = b.m(k, j)
               t += a.m(i, k) * b.m(k, j)
            Next
            tmp.m(i, j) = t
         Next
      Next
      Return tmp
   End Function
   Private Function dsin(ByVal Theta As Single) As Single
      dsin = Math.Sin(Theta * RAD)
   End Function
   Private Function dcos(ByVal Theta As Single) As Single
      dcos = Math.Cos(Theta * RAD)
   End Function
   Public Function Add(ByVal a As Matrix, ByVal b As Matrix) As Matrix
      Dim tmp As New Matrix
      Dim I As Integer = 0
      Dim J As Integer = 0
      For I = 1 To 4
         For J = 1 To 4
            tmp.m(I, J) = a.m(I, J) + b.m(I, J)
         Next
      Next
      Return Normalize(tmp)
   End Function
   Public Function Scale(ByVal a As Matrix, ByVal x As Single, ByVal y As Single, ByVal z As Single) As Matrix
      Dim tmp As New Matrix
      tmp.m(1, 1) = x
      tmp.m(2, 2) = y
      tmp.m(3, 3) = z
      tmp.m(4, 4) = 1
      Return multiply(a, tmp)
   End Function
   Public Function ScaleNew(ByVal a As Matrix, ByVal b As XYZCoord) As Matrix
      Dim tmp As New Matrix
      tmp.m(1, 1) = b.X
      tmp.m(2, 2) = b.Y
      tmp.m(3, 3) = b.Z
      tmp.m(4, 4) = 1
      Return multiply(a, tmp)
   End Function
   Public Function Translate(ByVal a As Matrix, ByVal x As Single, ByVal y As Single, ByVal z As Single) As Matrix
      Dim tmp As New Matrix
      tmp.m(4, 1) = x
      tmp.m(4, 2) = y
      tmp.m(4, 3) = z
      tmp.m(4, 4) = 1
      Return multiply(a, tmp)
   End Function
   Public Function TranslateNew(ByVal a As Matrix, ByVal b As XYZCoord) As Matrix
      Dim tmp As New Matrix
      tmp.m(4, 1) = b.X
      tmp.m(4, 2) = b.Y
      tmp.m(4, 3) = b.Z
      tmp.m(4, 4) = 1.0
      Return multiply(a, tmp)
   End Function
   Public Function RotateX(ByVal a As Matrix, ByVal theta As Single) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      Dim c, s As Single
      c = dcos(theta)
      s = dsin(theta)
      tmp.m(2, 2) = c
      tmp.m(2, 3) = s
      tmp.m(3, 2) = -s
      tmp.m(3, 3) = c
      Return multiply(a, tmp)
   End Function
   Public Function RotateY(ByVal a As Matrix, ByVal theta As Single) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      Dim c, s As Single
      c = dcos(theta)
      s = dsin(theta)
      tmp.m(1, 1) = c
      tmp.m(1, 3) = -s
      tmp.m(3, 1) = s
      tmp.m(3, 3) = c
      Return multiply(a, tmp)
   End Function
   Public Function RotateZ(ByVal a As Matrix, ByVal theta As Single) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      Dim c, s As Single
      c = dcos(theta)
      s = dsin(theta)
      tmp.m(1, 1) = c
      tmp.m(1, 2) = s
      tmp.m(2, 1) = -s
      tmp.m(2, 2) = c
      Return multiply(a, tmp)
   End Function
   Public Function Rotate(ByVal a As Matrix, ByVal thetaX As Single, ByVal thetaY As Single, ByVal thetaZ As Single) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      tmp = RotateX(a, thetaX)
      tmp = RotateY(tmp, thetaY)
      tmp = RotateZ(tmp, thetaZ)
      Return tmp
   End Function
   Public Function RotateNew(ByVal a As Matrix, ByVal b As XYZCoord) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      tmp = RotateX(a, b.X)
      tmp = RotateY(tmp, b.Y)
      tmp = RotateZ(tmp, b.Z)
      Return tmp
   End Function
   Public Function Rotate2(ByVal a As Matrix, ByVal thetaX As Single, ByVal thetaY As Single, ByVal thetaZ As Single) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      tmp = RotateZ(a, thetaZ)
      tmp = RotateY(tmp, thetaY)
      tmp = RotateX(tmp, thetaX)
      Return tmp
   End Function
   Public Function Rotate2New(ByVal a As Matrix, ByVal b As XYZCoord) As Matrix
      Dim tmp As New Matrix
      tmp.Identity()
      tmp = RotateZ(a, b.Z)
      tmp = RotateY(tmp, b.Y)
      tmp = RotateX(tmp, b.X)
      Return tmp
   End Function
   Public Sub Apply(ByRef X As Single, ByRef Y As Single, ByRef Z As Single, ByVal a As Matrix)
      Dim temp(4) As Single
      Dim Res(4) As Single
      Dim I, J As Integer
      Dim t As Single
      temp(1) = X
      temp(2) = Y
      temp(3) = Z
      temp(4) = 1
      For I = 1 To 4
         t = 0.0
         For J = 1 To 4
            t += temp(J) * a.m(J, I)
         Next
         Res(I) = t
      Next
      X = Res(1) / Res(4)
      Y = Res(2) / Res(4)
      Z = Res(3) / Res(4)
   End Sub
   Public Function Apply2(ByVal P As XYZCoord, ByVal A As Matrix) As XYZCoord
      Dim t(4) As Single
      Dim r(4) As Single
      Dim s As New XYZCoord
      Dim I As Integer = 0
      Dim J As Integer = 0
      Dim q As Single = 0.0

      t(1) = P.X
      t(2) = P.Y
      t(3) = P.Z
      t(4) = 1.0
      For I = 1 To 4
         q = 0.0
         For J = 1 To 4
            q = q + t(J) * A.m(J, I)
         Next
         r(I) = q
      Next
      s.X = r(1) / r(4)
      s.Y = r(2) / r(4)
      s.Z = r(3) / r(4)
      Return s
   End Function
End Module
Public Module VectorMath
   'Vector Math Functions
   Public Function Normalize(ByVal P As XYZCoord) As XYZCoord
      Dim d As Single = 0
      Dim t As New XYZCoord
      d = Math.Sqrt(P.X ^ 2 + P.Y ^ 2 + P.Z ^ 2)
      If d = 0 Then d = 1
      t.X = P.X / d
      t.Y = P.Y / d
      t.Z = P.Z / d
      Return t
   End Function
   Public Function Cross(ByVal A As XYZCoord, ByVal B As XYZCoord) As XYZCoord
      Dim t As New XYZCoord
      t.X = A.Y * B.Z - A.Z * B.Y
      t.Y = A.Z * B.X - A.X * B.Z
      t.Z = A.X * B.Y - A.Y * B.X
      Return t
   End Function
   Public Function Dot(ByVal A As XYZCoord, ByVal B As XYZCoord) As Single
      Dot = (A.X * B.X) + (A.Y * B.Y) + (A.Z + B.Z)
   End Function
   Public Function Dist(ByVal A As XYZCoord, ByVal B As XYZCoord) As Single
      Dist = Math.Sqrt((B.X - A.X) ^ 2 + (B.Y - A.Y) ^ 2 + (B.Z - A.Z) ^ 2)
   End Function
   Public Function Flip(ByVal A As XYZCoord) As XYZCoord
      Return New XYZCoord(-A.X, -A.Y, -A.Z)
   End Function
End Module
