Attribute VB_Name = "Math"
Option Explicit

Public Declare Function GetAsyncKeyState Lib "user32" (ByVal vKey As Long) As Integer

Public Type Vector2
    X As Single
    Y As Single
End Type

Public Type Vector3
    X As Single
    Y As Single
    Z As Single
End Type

Public Type Vector4
    X As Single
    Y As Single
    Z As Single
    W As Single
End Type

Public Type Box
    Min As Vector3
    Max As Vector3
End Type

Public Type Matrix3
    XAxis As Vector3
    YAxis As Vector3
    ZAxis As Vector3
End Type

Public Type Matrix4
    XAxis As Vector4
    YAxis As Vector4
    ZAxis As Vector4
    WAxis As Vector4
End Type

Public Const PI As Single = 3.14159265358979
Public Const DegreesPerRadian As Single = 57.2957795130823 '180 / PI
Public Const RadiansPerDegree As Single = 1.74532925199433E-02 'PI / 180

Public Function KeyDown_Truth(ByVal vKey As Long) As Boolean
    KeyDown_Truth = (GetAsyncKeyState(vKey) < 0)
End Function

Public Function KeyDown_ZeroToOne(ByVal vKey As Long) As Single
    If GetAsyncKeyState(vKey) < 0 Then
        KeyDown_ZeroToOne = 1
    Else
        KeyDown_ZeroToOne = 0
    End If
End Function

Public Function KeyDown_OneToZero(ByVal vKey As Long) As Single
    If GetAsyncKeyState(vKey) < 0 Then
        KeyDown_OneToZero = 0
    Else
        KeyDown_OneToZero = 1
    End If
End Function

Public Function SafeSingleToByte(ByVal X As Single) As Byte
    Static Rounded As Long
    Rounded = X * 255
    If Rounded < 0 Then
        Rounded = 0
    ElseIf Rounded > 255 Then
        Rounded = 255
    End If
    SafeSingleToByte = Rounded
End Function

Public Function Min(ByVal A As Single, ByVal B As Single) As Single
    If A < B Then
        Min = A
    Else
        Min = B
    End If
End Function

Public Function Max(ByVal A As Single, ByVal B As Single) As Single
    If A > B Then
        Max = A
    Else
        Max = B
    End If
End Function

Public Function Min_Vector2(A As Vector2, B As Vector2) As Vector2
    Min_Vector2 = MakeVector2(Min(A.X, B.X), Min(A.Y, B.Y))
End Function

Public Function Max_Vector2(A As Vector2, B As Vector2) As Vector2
    Max_Vector2 = MakeVector2(Max(A.X, B.X), Max(A.Y, B.Y))
End Function

Public Function Min_Vector3(A As Vector3, B As Vector3) As Vector3
    Min_Vector3 = MakeVector3(Min(A.X, B.X), Min(A.Y, B.Y), Min(A.Z, B.Z))
End Function

Public Function Max_Vector3(A As Vector3, B As Vector3) As Vector3
    Max_Vector3 = MakeVector3(Max(A.X, B.X), Max(A.Y, B.Y), Max(A.Z, B.Z))
End Function

Public Function Min_Vector4(A As Vector4, B As Vector4) As Vector4
    Min_Vector4 = MakeVector4(Min(A.X, B.X), Min(A.Y, B.Y), Min(A.Z, B.Z), Min(A.W, B.W))
End Function

Public Function Max_Vector4(A As Vector4, B As Vector4) As Vector4
    Max_Vector4 = MakeVector4(Max(A.X, B.X), Max(A.Y, B.Y), Max(A.Z, B.Z), Max(A.W, B.W))
End Function

Public Function ACos(ByVal X As Single) As Single
    If X <> 1 Then
        ACos = Atn(-X / Sqr(-(X ^ 2) + 1)) + (2 * Atn(1))
    Else
        ACos = 0
    End If
End Function

Public Function Cubic(ByVal X As Single) As Single
    Cubic = (3 * (X ^ 2)) - (2 * (X ^ 3))
End Function

Public Function GenerateNormalFromPoints(A As Vector3, B As Vector3, C As Vector3) As Vector3
    Dim DirDot As Single
    DirDot = DotProduct(NormalVector3(ToVector3(A, C)), NormalVector3(ToVector3(A, B)))
    If DistVector3(A, B) = 0 Or DistVector3(B, C) = 0 Or DistVector3(C, B) = 0 Then
        'Returns (0,1,0) for non existing normal
        GenerateNormalFromPoints = MakeVector3(0, 1, 0)
    Else
        GenerateNormalFromPoints = NormalVector3(CrossProduct(ToVector3(A, C), ToVector3(A, B)))
    End If
End Function

Public Function ZDerativesFromPoints(A As Vector3, B As Vector3, C As Vector3) As Vector2
    ZDerativesFromPoints = ZDerivativesFromNormal(GenerateNormalFromPoints(A, B, C))
End Function

Public Function ZDerivativesFromNormal(N As Vector3) As Vector2
    If N.Z = 0 Then
        ZDerivativesFromNormal = MakeVector2(0, 0)
    Else
        ZDerivativesFromNormal = MakeVector2(-N.X / N.Z, -N.Y / N.Z)
    End If
End Function

Public Function MakeVector2(ByVal X As Single, ByVal Y As Single) As Vector2
    MakeVector2.X = X
    MakeVector2.Y = Y
End Function

Public Function MakeVector3(ByVal X As Single, ByVal Y As Single, ByVal Z As Single) As Vector3
    MakeVector3.X = X
    MakeVector3.Y = Y
    MakeVector3.Z = Z
End Function

Public Function MakeVector4(ByVal X As Single, ByVal Y As Single, ByVal Z As Single, ByVal W As Single) As Vector4
    MakeVector4.X = X
    MakeVector4.Y = Y
    MakeVector4.Z = Z
    MakeVector4.W = W
End Function

Public Function Vector4ToVector3(A As Vector4) As Vector3
    Vector4ToVector3.X = A.X
    Vector4ToVector3.Y = A.Y
    Vector4ToVector3.Z = A.Z
End Function

Public Function Vector3ToVector4(A As Vector3, ByVal W As Single) As Vector4
    Vector3ToVector4.X = A.X
    Vector3ToVector4.Y = A.Y
    Vector3ToVector4.Z = A.Z
    Vector3ToVector4.W = W
End Function

Public Function Vector4ToVector2(A As Vector4) As Vector2
    Vector4ToVector2.X = A.X
    Vector4ToVector2.Y = A.Y
End Function

Public Function Vector3ToVector2(A As Vector3) As Vector2
    Vector3ToVector2.X = A.X
    Vector3ToVector2.Y = A.Y
End Function

Public Function RotateVector2(V As Vector2, ByVal Degrees As Single) As Vector2
    Static Radians As Single
    Radians = Degrees * RadiansPerDegree
    RotateVector2.X = (V.X * Cos(Radians)) + (V.Y * -Sin(Radians))
    RotateVector2.Y = (V.X * Sin(Radians)) + (V.Y * Cos(Radians))
End Function

Public Function RotateAroundX(V As Vector3, ByVal Degrees As Single) As Vector3
    Static Radians As Single
    Radians = Degrees * RadiansPerDegree
    RotateAroundX.X = V.X
    RotateAroundX.Y = (V.Y * Cos(Radians)) + (V.Z * -Sin(Radians))
    RotateAroundX.Z = (V.Y * Sin(Radians)) + (V.Z * Cos(Radians))
End Function

Public Function RotateAroundY(V As Vector3, ByVal Degrees As Single) As Vector3
    Static Radians As Single
    Radians = Degrees * RadiansPerDegree
    RotateAroundY.X = (V.X * Cos(Radians)) + (V.Z * Sin(Radians))
    RotateAroundY.Y = V.Y
    RotateAroundY.Z = (V.X * -Sin(Radians)) + (V.Z * Cos(Radians))
End Function

Public Function RotateAroundZ(V As Vector3, ByVal Degrees As Single) As Vector3
    Static Radians As Single
    Radians = Degrees * RadiansPerDegree
    RotateAroundZ.X = (V.X * Cos(Radians)) + (V.Y * Sin(Radians))
    RotateAroundZ.Y = (V.X * -Sin(Radians)) + (V.Y * Cos(Radians))
    RotateAroundZ.Z = V.Z
End Function

Public Function Matrix4ToMatrix3(A As Matrix4) As Matrix3
    Matrix4ToMatrix3.XAxis = Vector4ToVector3(A.XAxis)
    Matrix4ToMatrix3.YAxis = Vector4ToVector3(A.YAxis)
    Matrix4ToMatrix3.ZAxis = Vector4ToVector3(A.ZAxis)
End Function

Public Function PositionFromMatrix4(A As Matrix4) As Vector3
    PositionFromMatrix4 = Vector4ToVector3(A.WAxis)
End Function

Public Function ClosestPointOn3DLine(Position As Vector3, LineStart As Vector3, LineEnd As Vector3) As Vector3
    Static Length As Single
    Static Direction As Vector3
    Static PositionRelativeToStart As Vector3
    Static DepthFromStartAlongDirection As Single
    Static StartEndWeight As Single
    Length = DistVector3(LineStart, LineEnd)
    Direction = NormalVector3(SubVector3(LineEnd, LineStart))
    PositionRelativeToStart = SubVector3(Position, LineStart)
    DepthFromStartAlongDirection = DotProduct(Direction, PositionRelativeToStart)
    StartEndWeight = Saturate(DepthFromStartAlongDirection / Length)
    ClosestPointOn3DLine = LerpVector3(LineStart, LineEnd, StartEndWeight)
End Function

Public Function Lerp(ByVal A As Single, ByVal B As Single, ByVal Ratio As Single) As Single
    Lerp = (A * (1 - Ratio)) + (B * Ratio)
End Function

Public Function LerpVector2(A As Vector2, B As Vector2, ByVal Ratio As Single) As Vector2
    LerpVector2.X = Lerp(A.X, B.X, Ratio)
    LerpVector2.Y = Lerp(A.Y, B.Y, Ratio)
End Function

Public Function LerpVector3(A As Vector3, B As Vector3, ByVal Ratio As Single) As Vector3
    LerpVector3.X = Lerp(A.X, B.X, Ratio)
    LerpVector3.Y = Lerp(A.Y, B.Y, Ratio)
    LerpVector3.Z = Lerp(A.Z, B.Z, Ratio)
End Function

Public Function LerpVector4(A As Vector4, B As Vector4, ByVal Ratio As Single) As Vector4
    LerpVector4.X = Lerp(A.X, B.X, Ratio)
    LerpVector4.Y = Lerp(A.Y, B.Y, Ratio)
    LerpVector4.Z = Lerp(A.Z, B.Z, Ratio)
    LerpVector4.W = Lerp(A.W, B.W, Ratio)
End Function

'Precondition: A <> B
Public Function InverseLerp(ByVal A As Single, ByVal B As Single, ByVal ValueInBetween As Single) As Single
    InverseLerp = (ValueInBetween - A) / (B - A)
End Function

'Precondition: A.x <> B.x and A.y <> B.y
Public Function InverseLerpVector2(A As Vector2, B As Vector2, ValueInBetween As Vector2) As Vector2
    InverseLerpVector2.X = InverseLerp(A.X, B.X, ValueInBetween.X)
    InverseLerpVector2.Y = InverseLerp(A.Y, B.Y, ValueInBetween.Y)
End Function

'Precondition: A.x <> B.x, A.y <> B.y and A.z <> B.z
Public Function InverseLerpVector3(A As Vector3, B As Vector3, ValueInBetween As Vector3) As Vector3
    InverseLerpVector3.X = InverseLerp(A.X, B.X, ValueInBetween.X)
    InverseLerpVector3.Y = InverseLerp(A.Y, B.Y, ValueInBetween.Y)
    InverseLerpVector3.Z = InverseLerp(A.Z, B.Z, ValueInBetween.Z)
End Function

'Precondition: A.x <> B.x, A.y <> B.y, A.z <> B.z and A.w <> B.w
Public Function InverseLerpVector4(A As Vector4, B As Vector4, ValueInBetween As Vector4) As Vector4
    InverseLerpVector4.X = InverseLerp(A.X, B.X, ValueInBetween.X)
    InverseLerpVector4.Y = InverseLerp(A.Y, B.Y, ValueInBetween.Y)
    InverseLerpVector4.Z = InverseLerp(A.Z, B.Z, ValueInBetween.Z)
    InverseLerpVector4.W = InverseLerp(A.W, B.W, ValueInBetween.W)
End Function

Public Function Clamp(ByVal X As Single, ByVal Min As Single, ByVal Max As Single) As Single
    If X < Min Then
        Clamp = Min
    ElseIf X > Max Then
        Clamp = Max
    Else
        Clamp = X
    End If
End Function

Public Function Clamp_Long(ByVal X As Long, ByVal Min As Long, ByVal Max As Long) As Long
    If X < Min Then
        Clamp_Long = Min
    ElseIf X > Max Then
        Clamp_Long = Max
    Else
        Clamp_Long = X
    End If
End Function

Public Function ClampVector2(V As Vector2, ByVal Min As Single, ByVal Max As Single) As Vector2
    ClampVector2.X = Clamp(V.X, Min, Max)
    ClampVector2.Y = Clamp(V.Y, Min, Max)
End Function

Public Function ClampVector3(V As Vector3, ByVal Min As Single, ByVal Max As Single) As Vector3
    ClampVector3.X = Clamp(V.X, Min, Max)
    ClampVector3.Y = Clamp(V.Y, Min, Max)
    ClampVector3.Z = Clamp(V.Z, Min, Max)
End Function

Public Function ClampVector4(V As Vector4, ByVal Min As Single, ByVal Max As Single) As Vector4
    ClampVector4.X = Clamp(V.X, Min, Max)
    ClampVector4.Y = Clamp(V.Y, Min, Max)
    ClampVector4.Z = Clamp(V.Z, Min, Max)
    ClampVector4.W = Clamp(V.W, Min, Max)
End Function

Public Function ClampSphere3(V As Vector3, MaxLength As Single) As Vector3
    Dim Length As Single
    Length = AbsVector3(V)
    If Length > MaxLength Then
        ClampSphere3 = MulVector3(V, MaxLength / Length)
    Else
        ClampSphere3 = V
    End If
End Function

Public Function Saturate(ByVal X As Single) As Single
    If X < 0 Then
        Saturate = 0
    ElseIf X > 1 Then
        Saturate = 1
    Else
        Saturate = X
    End If
End Function

Public Function SaturateVector2(V As Vector2) As Vector2
    SaturateVector2.X = Saturate(V.X)
    SaturateVector2.Y = Saturate(V.Y)
End Function

Public Function SaturateVector3(V As Vector3) As Vector3
    SaturateVector3.X = Saturate(V.X)
    SaturateVector3.Y = Saturate(V.Y)
    SaturateVector3.Z = Saturate(V.Z)
End Function

Public Function SaturateVector4(V As Vector4) As Vector4
    SaturateVector4.X = Saturate(V.X)
    SaturateVector4.Y = Saturate(V.Y)
    SaturateVector4.Z = Saturate(V.Z)
    SaturateVector4.W = Saturate(V.W)
End Function

Public Function DistVector2(A As Vector2, B As Vector2) As Single
    DistVector2 = AbsVector2(SubVector2(A, B))
End Function

Public Function DistVector3(A As Vector3, B As Vector3) As Single
    DistVector3 = AbsVector3(SubVector3(A, B))
End Function

Public Function DistVector4(A As Vector4, B As Vector4) As Single
    DistVector4 = AbsVector4(SubVector4(A, B))
End Function

Public Function MakeBox_FromVectors(Min As Vector3, Max As Vector3) As Box
    MakeBox_FromVectors.Min.X = Min.X
    MakeBox_FromVectors.Min.Y = Min.Y
    MakeBox_FromVectors.Min.Z = Min.Z
    MakeBox_FromVectors.Max.X = Max.X
    MakeBox_FromVectors.Max.Y = Max.Y
    MakeBox_FromVectors.Max.Z = Max.Z
End Function

Public Function MakeBox_FromArgs(ByVal MinX As Single, ByVal MinY As Single, ByVal MinZ As Single, ByVal MaxX As Single, ByVal MaxY As Single, ByVal MaxZ As Single) As Box
    MakeBox_FromArgs.Min.X = MinX
    MakeBox_FromArgs.Min.Y = MinY
    MakeBox_FromArgs.Min.Z = MinZ
    MakeBox_FromArgs.Max.X = MaxX
    MakeBox_FromArgs.Max.Y = MaxY
    MakeBox_FromArgs.Max.Z = MaxZ
End Function

Public Function MakeMatrix3(XAxis As Vector3, YAxis As Vector3, ZAxis As Vector3) As Matrix3
    MakeMatrix3.XAxis = XAxis
    MakeMatrix3.YAxis = YAxis
    MakeMatrix3.ZAxis = ZAxis
End Function

Public Function MakeMatrix4(XAxis As Vector4, YAxis As Vector4, ZAxis As Vector4, WAxis As Vector4) As Matrix4
    MakeMatrix4.XAxis = XAxis
    MakeMatrix4.YAxis = YAxis
    MakeMatrix4.ZAxis = ZAxis
    MakeMatrix4.WAxis = WAxis
End Function

Public Function MakeUnitMatrix3() As Matrix3
    MakeUnitMatrix3.XAxis = MakeVector3(1, 0, 0)
    MakeUnitMatrix3.YAxis = MakeVector3(0, 1, 0)
    MakeUnitMatrix3.ZAxis = MakeVector3(0, 0, 1)
End Function

Public Function MakeUnitMatrix4() As Matrix4
    MakeUnitMatrix4.XAxis = MakeVector4(1, 0, 0, 0)
    MakeUnitMatrix4.YAxis = MakeVector4(0, 1, 0, 0)
    MakeUnitMatrix4.ZAxis = MakeVector4(0, 0, 1, 0)
    MakeUnitMatrix4.WAxis = MakeVector4(0, 0, 0, 1)
End Function

Public Function DotProduct(A As Vector3, B As Vector3) As Single
    DotProduct = (A.X * B.X) + (A.Y * B.Y) + (A.Z * B.Z)
End Function

Public Function CrossProduct(A As Vector3, B As Vector3) As Vector3
    CrossProduct.X = A.Y * B.Z - A.Z * B.Y
    CrossProduct.Y = A.Z * B.X - A.X * B.Z
    CrossProduct.Z = A.X * B.Y - A.Y * B.X
End Function

Public Function MulMatMat3(A As Matrix3, B As Matrix3) As Matrix3
    MulMatMat3.XAxis = MulVecMat3(A.XAxis, B)
    MulMatMat3.YAxis = MulVecMat3(A.YAxis, B)
    MulMatMat3.ZAxis = MulVecMat3(A.ZAxis, B)
End Function

Public Function MulVecMat3(V As Vector3, M As Matrix3) As Vector3
    MulVecMat3.X = (V.X * M.XAxis.X) + (V.Y * M.YAxis.X) + (V.Z * M.ZAxis.X)
    MulVecMat3.Y = (V.X * M.XAxis.Y) + (V.Y * M.YAxis.Y) + (V.Z * M.ZAxis.Y)
    MulVecMat3.Z = (V.X * M.XAxis.Z) + (V.Y * M.YAxis.Z) + (V.Z * M.ZAxis.Z)
End Function

Public Function MulVecTransposedMat3(V As Vector3, M As Matrix3) As Vector3
    MulVecTransposedMat3.X = (V.X * M.XAxis.X) + (V.Y * M.XAxis.Y) + (V.Z * M.XAxis.Z)
    MulVecTransposedMat3.Y = (V.X * M.YAxis.X) + (V.Y * M.YAxis.Y) + (V.Z * M.YAxis.Z)
    MulVecTransposedMat3.Z = (V.X * M.ZAxis.X) + (V.Y * M.ZAxis.Y) + (V.Z * M.ZAxis.Z)
End Function

Public Function MulMatMat4(A As Matrix4, B As Matrix4) As Matrix4
    MulMatMat4.XAxis = MulVecMat4(A.XAxis, B)
    MulMatMat4.YAxis = MulVecMat4(A.YAxis, B)
    MulMatMat4.ZAxis = MulVecMat4(A.ZAxis, B)
    MulMatMat4.WAxis = MulVecMat4(A.WAxis, B)
End Function

Public Function MulVecMat4(V As Vector4, M As Matrix4) As Vector4
    MulVecMat4.X = (V.X * M.XAxis.X) + (V.Y * M.YAxis.X) + (V.Z * M.ZAxis.X) + (V.W * M.WAxis.X)
    MulVecMat4.Y = (V.X * M.XAxis.Y) + (V.Y * M.YAxis.Y) + (V.Z * M.ZAxis.Y) + (V.W * M.WAxis.Y)
    MulVecMat4.Z = (V.X * M.XAxis.Z) + (V.Y * M.YAxis.Z) + (V.Z * M.ZAxis.Z) + (V.W * M.WAxis.Z)
    MulVecMat4.W = (V.X * M.XAxis.W) + (V.Y * M.YAxis.W) + (V.Z * M.ZAxis.W) + (V.W * M.WAxis.W)
End Function

'Precondition: M.XAxis.W = 0, M.YAxis.W = 0, M.ZAxis.W = 0, M.WAxis.W = 1
'Postcondition: The vector V multiplied by the 3x3 part of M plus the position from M's W axis
Public Function MulVec3Mat4(V As Vector3, M As Matrix4) As Vector3
    Debug.Assert Abs(M.XAxis.W) < 0.0001
    Debug.Assert Abs(M.YAxis.W) < 0.0001
    Debug.Assert Abs(M.ZAxis.W) < 0.0001
    Debug.Assert Abs(M.WAxis.W - 1) < 0.0001
    MulVec3Mat4 = AddVector3(MulVecMat3(V, Matrix4ToMatrix3(M)), Vector4ToVector3(M.WAxis))
End Function

Public Function DeterminantMat4(M As Matrix4) As Single
    DeterminantMat4 = _
        M.XAxis.X * M.YAxis.Y * M.ZAxis.Z * M.WAxis.W - M.XAxis.X * M.YAxis.Y * M.ZAxis.W * M.WAxis.Z - M.XAxis.X * M.ZAxis.Y * M.YAxis.Z * M.WAxis.W + M.XAxis.X * M.ZAxis.Y * M.YAxis.W * M.WAxis.Z _
      + M.XAxis.X * M.WAxis.Y * M.YAxis.Z * M.ZAxis.W - M.XAxis.X * M.WAxis.Y * M.YAxis.W * M.ZAxis.Z - M.YAxis.X * M.XAxis.Y * M.ZAxis.Z * M.WAxis.W + M.YAxis.X * M.XAxis.Y * M.ZAxis.W * M.WAxis.Z _
      + M.YAxis.X * M.ZAxis.Y * M.XAxis.Z * M.WAxis.W - M.YAxis.X * M.ZAxis.Y * M.XAxis.W * M.WAxis.Z - M.YAxis.X * M.WAxis.Y * M.XAxis.Z * M.ZAxis.W + M.YAxis.X * M.WAxis.Y * M.XAxis.W * M.ZAxis.Z _
      + M.ZAxis.X * M.XAxis.Y * M.YAxis.Z * M.WAxis.W - M.ZAxis.X * M.XAxis.Y * M.YAxis.W * M.WAxis.Z - M.ZAxis.X * M.YAxis.Y * M.XAxis.Z * M.WAxis.W + M.ZAxis.X * M.YAxis.Y * M.XAxis.W * M.WAxis.Z _
      + M.ZAxis.X * M.WAxis.Y * M.XAxis.Z * M.YAxis.W - M.ZAxis.X * M.WAxis.Y * M.XAxis.W * M.YAxis.Z - M.WAxis.X * M.XAxis.Y * M.YAxis.Z * M.ZAxis.W + M.WAxis.X * M.XAxis.Y * M.YAxis.W * M.ZAxis.Z _
      + M.WAxis.X * M.YAxis.Y * M.XAxis.Z * M.ZAxis.W - M.WAxis.X * M.YAxis.Y * M.XAxis.W * M.ZAxis.Z - M.WAxis.X * M.ZAxis.Y * M.XAxis.Z * M.YAxis.W + M.WAxis.X * M.ZAxis.Y * M.XAxis.W * M.YAxis.Z
End Function

'If you happend to know the inverse determinant of M from the data that generated M, this is much faster than using InverseMat4_Simple
'Precondition:
'   M.XAxis.w = 0
'   M.YAxis.w = 0
'   M.ZAxis.w = 0
'   M.WAxis.w = 1
'   InvDet = 1 / DeterminantMat4(M)
'Post condition:
'   Returns the inverse matrix of M
'       so that M * Inverse(M) = UnitMatrix and (X * M) * Inverse(M) = X
Public Function InverseMat4_Fast(M As Matrix4, ByVal InvDet As Single) As Matrix4
    Debug.Assert Abs(M.XAxis.W) < 0.0001
    Debug.Assert Abs(M.YAxis.W) < 0.0001
    Debug.Assert Abs(M.ZAxis.W) < 0.0001
    Debug.Assert Abs(M.WAxis.W - 1) < 0.0001
    InverseMat4_Fast.XAxis.X = InvDet * (M.YAxis.Y * M.ZAxis.Z - M.YAxis.Z * M.ZAxis.Y)
    InverseMat4_Fast.XAxis.Y = -InvDet * (M.XAxis.Y * M.ZAxis.Z - M.XAxis.Z * M.ZAxis.Y)
    InverseMat4_Fast.XAxis.Z = InvDet * (M.XAxis.Y * M.YAxis.Z - M.XAxis.Z * M.YAxis.Y)
    InverseMat4_Fast.XAxis.W = 0
    InverseMat4_Fast.YAxis.X = -InvDet * (M.YAxis.X * M.ZAxis.Z - M.YAxis.Z * M.ZAxis.X)
    InverseMat4_Fast.YAxis.Y = InvDet * (M.XAxis.X * M.ZAxis.Z - M.XAxis.Z * M.ZAxis.X)
    InverseMat4_Fast.YAxis.Z = -InvDet * (M.XAxis.X * M.YAxis.Z - M.XAxis.Z * M.YAxis.X)
    InverseMat4_Fast.YAxis.W = 0
    InverseMat4_Fast.ZAxis.X = InvDet * (M.YAxis.X * M.ZAxis.Y - M.YAxis.Y * M.ZAxis.X)
    InverseMat4_Fast.ZAxis.Y = -InvDet * (M.XAxis.X * M.ZAxis.Y - M.XAxis.Y * M.ZAxis.X)
    InverseMat4_Fast.ZAxis.Z = InvDet * (M.XAxis.X * M.YAxis.Y - M.XAxis.Y * M.YAxis.X)
    InverseMat4_Fast.ZAxis.W = 0
    InverseMat4_Fast.WAxis.X = -(M.WAxis.X * InverseMat4_Fast.XAxis.X + M.WAxis.Y * InverseMat4_Fast.YAxis.X + M.WAxis.Z * InverseMat4_Fast.ZAxis.X)
    InverseMat4_Fast.WAxis.Y = -(M.WAxis.X * InverseMat4_Fast.XAxis.Y + M.WAxis.Y * InverseMat4_Fast.YAxis.Y + M.WAxis.Z * InverseMat4_Fast.ZAxis.Y)
    InverseMat4_Fast.WAxis.Z = -(M.WAxis.X * InverseMat4_Fast.XAxis.Z + M.WAxis.Y * InverseMat4_Fast.YAxis.Z + M.WAxis.Z * InverseMat4_Fast.ZAxis.Z)
    InverseMat4_Fast.WAxis.W = 1
End Function

'A simpler version of InverseMat4_Fast
'Precondition:
'   M.XAxis.w = 0
'   M.YAxis.w = 0
'   M.ZAxis.w = 0
'   M.WAxis.w = 1
'Post condition:
'   Returns the inverse matrix of M
'       so that M * Inverse(M) = UnitMatrix and (X * M) * Inverse(M) = X
Public Function InverseMat4_Simple(M As Matrix4) As Matrix4
    InverseMat4_Simple = InverseMat4_Fast(M, 1 / DeterminantMat4(M))
End Function

Public Function NormalVector2(V As Vector2) As Vector2
    Static Dist As Single
    Dist = AbsVector2(V)
    If Dist = 0 Then
        NormalVector2 = MakeVector2(0, 1)
    Else
        NormalVector2 = DivVector2(V, Dist)
    End If
End Function

Public Function NormalVector3(V As Vector3) As Vector3
    Static Dist As Single
    Dist = AbsVector3(V)
    If Dist = 0 Then
        NormalVector3 = MakeVector3(0, 0, 1)
    Else
        NormalVector3 = DivVector3(V, Dist)
    End If
End Function

Public Function AbsVector2(V As Vector2) As Single
    AbsVector2 = Sqr((V.X ^ 2) + (V.Y ^ 2))
End Function

Public Function AbsVector3(V As Vector3) As Single
    AbsVector3 = Sqr((V.X ^ 2) + (V.Y ^ 2) + (V.Z ^ 2))
End Function

Public Function AbsVector4(V As Vector4) As Single
    AbsVector4 = Sqr((V.X ^ 2) + (V.Y ^ 2) + (V.Z ^ 2) + (V.W ^ 2))
End Function

Public Function AddMatrix3(A As Matrix3, B As Matrix3) As Matrix3
    AddMatrix3.XAxis = AddVector3(A.XAxis, B.XAxis)
    AddMatrix3.YAxis = AddVector3(A.YAxis, B.YAxis)
    AddMatrix3.ZAxis = AddVector3(A.ZAxis, B.ZAxis)
End Function

Public Function SubMatrix3(A As Matrix3, B As Matrix3) As Matrix3
    SubMatrix3.XAxis = SubVector3(A.XAxis, B.XAxis)
    SubMatrix3.YAxis = SubVector3(A.YAxis, B.YAxis)
    SubMatrix3.ZAxis = SubVector3(A.ZAxis, B.ZAxis)
End Function

Public Function NegVector2(A As Vector2) As Vector2
    NegVector2.X = -A.X
    NegVector2.Y = -A.Y
End Function

Public Function NegVector3(A As Vector3) As Vector3
    NegVector3.X = -A.X
    NegVector3.Y = -A.Y
    NegVector3.Z = -A.Z
End Function

Public Function NegVector4(A As Vector4) As Vector4
    NegVector4.X = -A.X
    NegVector4.Y = -A.Y
    NegVector4.Z = -A.Z
    NegVector4.W = -A.W
End Function

Public Function AddVector2(A As Vector2, B As Vector2) As Vector2
    AddVector2.X = A.X + B.X
    AddVector2.Y = A.Y + B.Y
End Function

Public Function AddVector3(A As Vector3, B As Vector3) As Vector3
    AddVector3.X = A.X + B.X
    AddVector3.Y = A.Y + B.Y
    AddVector3.Z = A.Z + B.Z
End Function

Public Function AddVector4(A As Vector4, B As Vector4) As Vector4
    AddVector4.X = A.X + B.X
    AddVector4.Y = A.Y + B.Y
    AddVector4.Z = A.Z + B.Z
    AddVector4.W = A.W + B.W
End Function

Public Function SubVector2(A As Vector2, B As Vector2) As Vector2
    SubVector2.X = A.X - B.X
    SubVector2.Y = A.Y - B.Y
End Function

Public Function SubVector3(A As Vector3, B As Vector3) As Vector3
    SubVector3.X = A.X - B.X
    SubVector3.Y = A.Y - B.Y
    SubVector3.Z = A.Z - B.Z
End Function

Public Function SubVector4(A As Vector4, B As Vector4) As Vector4
    SubVector4.X = A.X - B.X
    SubVector4.Y = A.Y - B.Y
    SubVector4.Z = A.Z - B.Z
    SubVector4.W = A.W - B.W
End Function

Public Function ElemMulVector2(A As Vector2, B As Vector2) As Vector2
    ElemMulVector2.X = A.X * B.X
    ElemMulVector2.Y = A.Y * B.Y
End Function

Public Function ElemMulVector3(A As Vector3, B As Vector3) As Vector3
    ElemMulVector3.X = A.X * B.X
    ElemMulVector3.Y = A.Y * B.Y
    ElemMulVector3.Z = A.Z * B.Z
End Function

Public Function ElemMulVector4(A As Vector4, B As Vector4) As Vector4
    ElemMulVector4.X = A.X * B.X
    ElemMulVector4.Y = A.Y * B.Y
    ElemMulVector4.Z = A.Z * B.Z
    ElemMulVector4.W = A.W * B.W
End Function

Public Function MulVector2(A As Vector2, ByVal S As Single) As Vector2
    MulVector2.X = A.X * S
    MulVector2.Y = A.Y * S
End Function

Public Function MulVector3(A As Vector3, ByVal S As Single) As Vector3
    MulVector3.X = A.X * S
    MulVector3.Y = A.Y * S
    MulVector3.Z = A.Z * S
End Function

Public Function MulVector4(A As Vector4, ByVal S As Single) As Vector4
    MulVector4.X = A.X * S
    MulVector4.Y = A.Y * S
    MulVector4.Z = A.Z * S
    MulVector4.W = A.W * S
End Function

Public Function DivVector2(A As Vector2, ByVal S As Single) As Vector2
    DivVector2.X = A.X / S
    DivVector2.Y = A.Y / S
End Function

Public Function DivVector3(A As Vector3, ByVal S As Single) As Vector3
    DivVector3.X = A.X / S
    DivVector3.Y = A.Y / S
    DivVector3.Z = A.Z / S
End Function

Public Function DivVector4(A As Vector4, ByVal S As Single) As Vector4
    DivVector4.X = A.X / S
    DivVector4.Y = A.Y / S
    DivVector4.Z = A.Z / S
    DivVector4.W = A.W / S
End Function

Public Function ToVector2(A As Vector2, B As Vector2) As Vector2
    ToVector2.X = B.X - A.X
    ToVector2.Y = B.Y - A.Y
End Function

Public Function ToVector3(A As Vector3, B As Vector3) As Vector3
    ToVector3.X = B.X - A.X
    ToVector3.Y = B.Y - A.Y
    ToVector3.Z = B.Z - A.Z
End Function

Public Function ToVector4(A As Vector4, B As Vector4) As Vector4
    ToVector4.X = B.X - A.X
    ToVector4.Y = B.Y - A.Y
    ToVector4.Z = B.Z - A.Z
    ToVector4.W = B.W - A.W
End Function

Public Function MiddleVector2(A As Vector2, B As Vector2) As Vector2
    MiddleVector2.X = (A.X + B.X) / 2
    MiddleVector2.Y = (A.Y + B.Y) / 2
End Function

Public Function MiddleVector3(A As Vector3, B As Vector3) As Vector3
    MiddleVector3.X = (A.X + B.X) / 2
    MiddleVector3.Y = (A.Y + B.Y) / 2
    MiddleVector3.Z = (A.Z + B.Z) / 2
End Function

Public Function MiddleVector4(A As Vector4, B As Vector4) As Vector4
    MiddleVector4.X = (A.X + B.X) / 2
    MiddleVector4.Y = (A.Y + B.Y) / 2
    MiddleVector4.Z = (A.Z + B.Z) / 2
    MiddleVector4.W = (A.W + B.W) / 2
End Function

Public Sub IncreaseVector2(A As Vector2, B As Vector2, ByVal C As Single)
    A.X = A.X + (B.X * C)
    A.Y = A.Y + (B.Y * C)
End Sub

Public Sub IncreaseVector3(A As Vector3, B As Vector3, ByVal C As Single)
    A.X = A.X + (B.X * C)
    A.Y = A.Y + (B.Y * C)
    A.Z = A.Z + (B.Z * C)
End Sub

Public Sub IncreaseVector4(A As Vector4, B As Vector4, ByVal C As Single)
    A.X = A.X + (B.X * C)
    A.Y = A.Y + (B.Y * C)
    A.Z = A.Z + (B.Z * C)
    A.W = A.W + (B.W * C)
End Sub

Public Function MakeBoxFromTwoPoints(A As Vector3, B As Vector3) As Box
    MakeBoxFromTwoPoints.Min.X = Min(A.X, B.X)
    MakeBoxFromTwoPoints.Max.X = Max(A.X, B.X)
    MakeBoxFromTwoPoints.Min.Y = Min(A.Y, B.Y)
    MakeBoxFromTwoPoints.Max.Y = Max(A.Y, B.Y)
    MakeBoxFromTwoPoints.Min.Z = Min(A.Z, B.Z)
    MakeBoxFromTwoPoints.Max.Z = Max(A.Z, B.Z)
End Function

Public Sub ExtendBoxUsingPoint(P As Vector3, ByRef B As Box)
    'The box B should be expanded as little as it can to include itself and the point P
    If P.X < B.Min.X Then B.Min.X = P.X
    If P.Y < B.Min.Y Then B.Min.Y = P.Y
    If P.Z < B.Min.Z Then B.Min.Z = P.Z
    If P.X > B.Max.X Then B.Max.X = P.X
    If P.Y > B.Max.Y Then B.Max.Y = P.Y
    If P.Z > B.Max.Z Then B.Max.Z = P.Z
    'P should be in the new B
    Debug.Assert PointTouchesBox(P, B)
End Sub

Public Function BoxesOverlaps(A As Box, B As Box) As Boolean
    BoxesOverlaps = ( _
      A.Max.X > B.Min.X And _
      A.Min.X < B.Max.X And _
      A.Max.Y > B.Min.Y And _
      A.Min.Y < B.Max.Y And _
      A.Max.Z > B.Min.Z And _
      A.Min.Z < B.Max.Z)
End Function

Public Function BoxesTouches(A As Box, B As Box) As Boolean
    BoxesTouches = ( _
      A.Max.X >= B.Min.X And _
      A.Min.X <= B.Max.X And _
      A.Max.Y >= B.Min.Y And _
      A.Min.Y <= B.Max.Y And _
      A.Max.Z >= B.Min.Z And _
      A.Min.Z <= B.Max.Z)
End Function

Public Function PointIsInBox(P As Vector3, B As Box) As Boolean
    PointIsInBox = ( _
      P.X > B.Min.X And _
      P.X < B.Max.X And _
      P.Y > B.Min.Y And _
      P.Y < B.Max.Y And _
      P.Z > B.Min.Z And _
      P.Z < B.Max.Z)
End Function

Public Function PointTouchesBox(P As Vector3, B As Box) As Boolean
    PointTouchesBox = ( _
      P.X >= B.Min.X And _
      P.X <= B.Max.X And _
      P.Y >= B.Min.Y And _
      P.Y <= B.Max.Y And _
      P.Z >= B.Min.Z And _
      P.Z <= B.Max.Z)
End Function

Public Function MoveBox(P As Vector3, B As Box) As Box
    MoveBox = MakeBox_FromArgs( _
      P.X + B.Min.X, _
      P.Y + B.Min.Y, _
      P.Z + B.Min.Z, _
      P.X + B.Max.X, _
      P.Y + B.Max.Y, _
      P.Z + B.Max.Z)
End Function

Public Function GetBoxCenter(B As Box) As Vector3
    GetBoxCenter = MakeVector3( _
      ((B.Min.X + B.Max.X) / 2), _
      ((B.Min.Y + B.Max.Y) / 2), _
      ((B.Min.Z + B.Max.Z) / 2))
End Function

'P is moved to the closest point in B
Public Function ClosestPointAtBox(P As Vector3, B As Box) As Vector3
    ClosestPointAtBox = P
    If ClosestPointAtBox.X < B.Min.X Then ClosestPointAtBox.X = B.Min.X
    If ClosestPointAtBox.X > B.Max.X Then ClosestPointAtBox.X = B.Max.X
    If ClosestPointAtBox.Y < B.Min.Y Then ClosestPointAtBox.Y = B.Min.Y
    If ClosestPointAtBox.Y > B.Max.Y Then ClosestPointAtBox.Y = B.Max.Y
    If ClosestPointAtBox.Z < B.Min.Z Then ClosestPointAtBox.Z = B.Min.Z
    If ClosestPointAtBox.Z > B.Max.Z Then ClosestPointAtBox.Z = B.Max.Z
End Function
