#include once "3dmath.bi"


Sub Matrix_Mul_Matrix ( Matrix_1() As Single, Matrix_2() As Single )
    
    Dim Matrix_R(15) As Single
    Dim As Integer i
    
    Matrix_R(0) = (Matrix_1(0) * Matrix_2(0))+(Matrix_1(1) * Matrix_2(4))+(Matrix_1(2) * Matrix_2(8))+(Matrix_1(3) * Matrix_2(12))
    Matrix_R(1) = (Matrix_1(0) * Matrix_2(1))+(Matrix_1(1) * Matrix_2(5))+(Matrix_1(2) * Matrix_2(9))+(Matrix_1(3) * Matrix_2(13))
    Matrix_R(2) = (Matrix_1(0) * Matrix_2(2))+(Matrix_1(1) * Matrix_2(6))+(Matrix_1(2) * Matrix_2(10))+(Matrix_1(3) * Matrix_2(14))
    Matrix_R(3) = (Matrix_1(0) * Matrix_2(3))+(Matrix_1(1) * Matrix_2(7))+(Matrix_1(2) * Matrix_2(11))+(Matrix_1(3) * Matrix_2(15))
    Matrix_R(4) = (Matrix_1(4) * Matrix_2(0))+(Matrix_1(5) * Matrix_2(4))+(Matrix_1(6) * Matrix_2(8))+(Matrix_1(7) * Matrix_2(12))
    Matrix_R(5) = (Matrix_1(4) * Matrix_2(1))+(Matrix_1(5) * Matrix_2(5))+(Matrix_1(6) * Matrix_2(9))+(Matrix_1(7) * Matrix_2(13))
    Matrix_R(6) = (Matrix_1(4) * Matrix_2(2))+(Matrix_1(5) * Matrix_2(6))+(Matrix_1(6) * Matrix_2(10))+(Matrix_1(7) * Matrix_2(14))
    Matrix_R(7) = (Matrix_1(4) * Matrix_2(3))+(Matrix_1(5) * Matrix_2(7))+(Matrix_1(6) * Matrix_2(11))+(Matrix_1(7) * Matrix_2(15))
    Matrix_R(8) = (Matrix_1(8) * Matrix_2(0))+(Matrix_1(9) * Matrix_2(4))+(Matrix_1(10) * Matrix_2(8))+(Matrix_1(11) * Matrix_2(12))
    Matrix_R(9) = (Matrix_1(8) * Matrix_2(1))+(Matrix_1(9) * Matrix_2(5))+(Matrix_1(10) * Matrix_2(9))+(Matrix_1(11) * Matrix_2(13))
    Matrix_R(10) = (Matrix_1(8) * Matrix_2(2))+(Matrix_1(9) * Matrix_2(6))+(Matrix_1(10) * Matrix_2(10))+(Matrix_1(11) * Matrix_2(14))
    Matrix_R(11) = (Matrix_1(8) * Matrix_2(3))+(Matrix_1(9) * Matrix_2(7))+(Matrix_1(10) * Matrix_2(11))+(Matrix_1(11) * Matrix_2(15))
    Matrix_R(12) = (Matrix_1(12) * Matrix_2(0))+(Matrix_1(13) * Matrix_2(4))+(Matrix_1(14) * Matrix_2(8))+(Matrix_1(15) * Matrix_2(12))
    Matrix_R(13) = (Matrix_1(12) * Matrix_2(1))+(Matrix_1(13) * Matrix_2(5))+(Matrix_1(14) * Matrix_2(9))+(Matrix_1(15) * Matrix_2(13))
    Matrix_R(14) = (Matrix_1(12) * Matrix_2(2))+(Matrix_1(13) * Matrix_2(6))+(Matrix_1(14) * Matrix_2(10))+(Matrix_1(15) * Matrix_2(14))
    Matrix_R(15) = (Matrix_1(12) * Matrix_2(3))+(Matrix_1(13) * Matrix_2(7))+(Matrix_1(14) * Matrix_2(11))+(Matrix_1(15) * Matrix_2(15))
    
    For i = 0 To 15
        Matrix_1(i) = Matrix_R(i)
    Next
End Sub


Function Matrix_Mul_Vector( Byval M As Single Ptr, Byval vIn As Vector3D ) As Vector3D
    Dim As Vector3D vOut
    
    vOut.X = vIn.X*M[0] + vIn.Y*M[4] + vIn.Z*M[8]  + M[12]
    vOut.Y = vIn.X*M[1] + vIn.Y*M[5] + vIn.Z*M[9]  + M[13]
    vOut.Z = vIn.X*M[2] + vIn.Y*M[6] + vIn.Z*M[10] + M[14]
    
    Return vOut
End Function


Function Vector_Dot (Byval v1 As Vector3D, Byval v2 As Vector3D) As Single
    Vector_Dot = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z
End Function

Function Vector_Cross ( Byval v1 As Vector3D, Byval v2 As Vector3D ) As Vector3D
    Dim As Vector3D v
    v.x = (v1.y * v2.z) - (v2.y * v1.z)
    v.y = (v1.z * v2.x) - (v2.z * v1.x)
    v.z = (v1.x * v2.y) - (v2.x * v1.y)
    Return v
End Function

Function Magnitude( Byval V As Vector3D) As Single
    Dim Mag As Single
    Mag = Sqr(v.x ^2 + v.y ^2 + v.z ^2)
    If Mag = 0 Then Mag = 1
    Magnitude = Mag
End Function

Sub Vector_Normalize (Byref v As Vector3D)
    Dim Mag As Single
    Mag = Magnitude(V)   
    V/=Mag
End Sub

Sub Poly_Normal(V() As Vector3D, Byref tNorm As Vector3D)
    Dim As Vector3D V1 = V(2) - V(1)
    Dim As Vector3D V2 = V(3) - V(1)
    
    tNorm = Vector_Cross( V1, V2 )
    Vector_Normalize tNorm
End Sub

Function Distance( Byval P1 As Vector3D, Byval P2 As Vector3D) As Single
    Distance = Sqr((P2.x - P1.x)^2 + (P2.y - P1.y)^2 + (P2.z - P1.z)^2)
End Function

Function Angle_Between_Vectors( Byval Vector1 As Vector3D, Byval Vector2 As Vector3D) As Single
    Dim tDot As Single, VecsMag As Single, Angle As Single
    
    tDot = Vector_Dot(Vector1, Vector2)
    VecsMag = Magnitude(Vector1) * Magnitude(Vector2)
    Angle = Acos(tDot/VecsMag)
    Angle_Between_Vectors = Angle
End Function

Function Plane_Distance( Byval tNormal As Vector3D, Byval tPoint As Vector3D) As Single  
    Plane_Distance = -((tNormal.x * tPoint.x) + (tNormal.y * tPoint.y) + (tNormal.z * tPoint.z))
End Function
