; *---------------------------------*
; *   OpenGL Matrix                 *
; *   xorc1zt                       *
; *   2011                          *
; *---------------------------------*


; ** IMPORTANT ** : OpenGL Matrix are column-major ordered
;
;   m[0] m[4] m[8]  m[12]     m[Xx] m[Yx] m[Zx] m[Tx]
;   m[1] m[5] m[9]  m[13]     m[Xy] m[Yy] m[Zy] m[Ty]
;   m[2] m[6] m[10] m[14] ==> m[Xz] m[Yz] m[Zz] m[Tz]
;   m[3] m[7] m[11] m[15]     m[Xw] m[Yw] m[Zw] m[Tw]
;
;

EnableExplicit
#MATRIX_SIZEBYTE = 64 ; 16*float
#TRACEFUNCTION = #False

Macro traceFunction(string)
  CompilerIf #TRACEFUNCTION
    Debug "[ TRACE ]: "+string
  CompilerEndIf
EndMacro

Interface MATRIX
  debugValues()
  getAddress()
  identity()
  multiply(MATRIX)
  rotateX(angledegree.f)
  rotateY(angledegree.f)
  rotateZ(angledegree.f)
  translate(x.f, y.f, z.f)
  perspective(angle.f, near.f, far.f, aspect.f)
  orthogonal(left.f, right.f, bottom.f, top.f, near.f, far.f)
  lookAt(eyeX.f, eyeY.f, eyeZ.f, centerX.f, centerY.f, centerZ.f, upX.f, upY.f, upZ.f)
  push()
  pop()
  destroy()
EndInterface

Structure PARENTSTR
  value.f[16]
  *parent.PARENTSTR
EndStructure

Structure MATRIXSTR
  VTable.l
  value.f[16]
  label.s
  *parent.PARENTSTR
EndStructure

Enumeration
  #Xx
  #Xy
  #Xz
  #Xw
  #Yx
  #Yy
  #Yz
  #Yw
  #Zx
  #Zy
  #Zz
  #Zw
  #Tx
  #Ty
  #Tz
  #Tw
EndEnumeration

Declare   crossProduct( Array *a.f(1), Array *b.f(1), Array *result.f(1))
Declare   normalize(Array *a.f(1))
Declare   debugValues(*Self.MATRIXSTR)
Declare.i getAddress(*Self.MATRIXSTR)
Declare   identity(*Self.MATRIXSTR)
Declare   multiply(*self.MATRIXSTR, *matrix.MATRIXSTR)
Declare   rotateX(*Self.MATRIXSTR, angledegree.f)
Declare   rotateY(*Self.MATRIXSTR, angledegree.f)
Declare   rotateZ(*Self.MATRIXSTR, angledegree.f)
Declare   translate(*Self.MATRIXSTR, x.f, y.f, z.f)
Declare   perspective(*Self.MATRIXSTR, angle.f, near.f, far.f, aspect.f)
Declare   orthogonal(*Self.MATRIXSTR, left.f, right.f, bottom.f, top.f, near.f, far.f)
Declare   lookAt(*Self.MATRIXSTR, eyeX.f, eyeY.f, eyeZ.f, centerX.f, centerY.f, centerZ.f, upX.f, upY.f, upZ.f)
Declare   push(*Self.MATRIXSTR)
Declare   pop(*Self.MATRIXSTR)
Declare   destroy(*Self.MATRIXSTR)
Declare.i createMatrix(label.s)   

Procedure crossProduct( Array *a.f(1), Array *b.f(1), Array *result.f(1))
  traceFunction("crossProduct()")
  
  *result(0) = ( *a(1) * *b(2) ) - ( *b(1) * *a(2) )
  *result(1) = ( *a(2) * *b(0) ) - ( *b(2) * *a(0) )
  *result(2) = ( *a(0) * *b(1) ) - ( *b(0) * *a(1) )
EndProcedure

Procedure normalize(Array *a.f(1))
  traceFunction("normalize()")
  
  Define mag.f = Sqr( *a(0) * *a(0) + *a(1) * *a(1) + *a(2) * *a(2) )
  *a(0) / mag
  *a(1) / mag
  *a(2) / mag
EndProcedure

Procedure debugValues(*Self.MATRIXSTR)
  traceFunction("debugValues()")
  
  Debug "---- MATRIX: "+*self\label+" ----"
  Debug StrF( *self\value[#Xx] )+" "+StrF( *self\value[#Yx] )+" "+StrF( *self\value[#Zx] )+" "+StrF( *self\value[#Tx] )
  Debug StrF( *self\value[#Xy] )+" "+StrF( *self\value[#Yy] )+" "+StrF( *self\value[#Zy] )+" "+StrF( *self\value[#Ty] )
  Debug StrF( *self\value[#Xz] )+" "+StrF( *self\value[#Yz] )+" "+StrF( *self\value[#Zz] )+" "+StrF( *self\value[#Tz] )
  Debug StrF( *self\value[#Xw] )+" "+StrF( *self\value[#Yw] )+" "+StrF( *self\value[#Zw] )+" "+StrF( *self\value[#Tw] )
EndProcedure

Procedure.i getAddress(*Self.MATRIXSTR)
  traceFunction("getAddress()")
  ProcedureReturn @*self\value[0]
EndProcedure

Procedure identity(*Self.MATRIXSTR)
  traceFunction("indentity()")
  
  CompilerIf 1
    With *Self
      \value[#Xx] = 1
      \value[#Xy] = 0
      \value[#Xz] = 0
      \value[#Xw] = 0
      
      \value[#Yx] = 0
      \value[#Yy] = 1
      \value[#Yz] = 0
      \value[#Yw] = 0
      
      \value[#Zx] = 0
      \value[#Zy] = 0
      \value[#Zz] = 1
      \value[#Zw] = 0
      
      \value[#Tx] = 0
      \value[#Ty] = 0
      \value[#Tz] = 0
      \value[#Tw] = 1
    EndWith
  CompilerElse
    CopyMemory(?MatrixIdentity, @*self\value[0], #MATRIX_SIZEBYTE)
  CompilerEndIf
EndProcedure

Procedure multiply(*self.MATRIXSTR, *matrix.MATRIXSTR)
  Define tempmatrix.MATRIXSTR
  
  traceFunction("multiply()")
  
  tempmatrix\value[#Xx] = (*self\value[0]  * *matrix\value[0])
  tempmatrix\value[#Xx] + (*self\value[4]  * *matrix\value[1])
  tempmatrix\value[#Xx] + (*self\value[8]  * *matrix\value[2])
  tempmatrix\value[#Xx] + (*self\value[12] * *matrix\value[3])
  
  tempmatrix\value[#Xy] = (*self\value[1]  * *matrix\value[0])
  tempmatrix\value[#Xy] + (*self\value[5]  * *matrix\value[1])
  tempmatrix\value[#Xy] + (*self\value[9]  * *matrix\value[2])
  tempmatrix\value[#Xy] + (*self\value[13] * *matrix\value[3])
  
  tempmatrix\value[#Xz] = (*self\value[2]  * *matrix\value[0])
  tempmatrix\value[#Xz] + (*self\value[6]  * *matrix\value[1])
  tempmatrix\value[#Xz] + (*self\value[10] * *matrix\value[2])
  tempmatrix\value[#Xz] + (*self\value[14] * *matrix\value[3])
  
  tempmatrix\value[#Xw] = (*self\value[3]  * *matrix\value[0])
  tempmatrix\value[#Xw] + (*self\value[7]  * *matrix\value[1])
  tempmatrix\value[#Xw] + (*self\value[11] * *matrix\value[2])
  tempmatrix\value[#Xw] + (*self\value[15] * *matrix\value[3])  
  
  tempmatrix\value[#Yx] = (*self\value[0]  * *matrix\value[4])
  tempmatrix\value[#Yx] + (*self\value[4]  * *matrix\value[5])
  tempmatrix\value[#Yx] + (*self\value[8]  * *matrix\value[6])
  tempmatrix\value[#Yx] + (*self\value[12] * *matrix\value[7])
  
  tempmatrix\value[#Yy] = (*self\value[1]  * *matrix\value[4])
  tempmatrix\value[#Yy] + (*self\value[5]  * *matrix\value[5])
  tempmatrix\value[#Yy] + (*self\value[9]  * *matrix\value[6])
  tempmatrix\value[#Yy] + (*self\value[13] * *matrix\value[7])
  
  tempmatrix\value[#Yz] = (*self\value[2]  * *matrix\value[4])
  tempmatrix\value[#Yz] + (*self\value[6]  * *matrix\value[5])
  tempmatrix\value[#Yz] + (*self\value[10] * *matrix\value[6])
  tempmatrix\value[#Yz] + (*self\value[14] * *matrix\value[7])
  
  tempmatrix\value[#Yw] = (*self\value[3]  * *matrix\value[4])
  tempmatrix\value[#Yw] + (*self\value[7]  * *matrix\value[5])
  tempmatrix\value[#Yw] + (*self\value[11] * *matrix\value[6])
  tempmatrix\value[#Yw] + (*self\value[15] * *matrix\value[7])  

  tempmatrix\value[#Zx] = (*self\value[0]  * *matrix\value[8])
  tempmatrix\value[#Zx] + (*self\value[4]  * *matrix\value[9])
  tempmatrix\value[#Zx] + (*self\value[8]  * *matrix\value[10])
  tempmatrix\value[#Zx] + (*self\value[12] * *matrix\value[11])
  
  tempmatrix\value[#Zy] = (*self\value[1]  * *matrix\value[8])
  tempmatrix\value[#Zy] + (*self\value[5]  * *matrix\value[9])
  tempmatrix\value[#Zy] + (*self\value[9]  * *matrix\value[10])
  tempmatrix\value[#Zy] + (*self\value[13] * *matrix\value[11])
  
  tempmatrix\value[#Zz] = (*self\value[2]  * *matrix\value[8])
  tempmatrix\value[#Zz] + (*self\value[6]  * *matrix\value[9])
  tempmatrix\value[#Zz] + (*self\value[10] * *matrix\value[10])
  tempmatrix\value[#Zz] + (*self\value[14] * *matrix\value[11])
  
  tempmatrix\value[#Zw] = (*self\value[3]  * *matrix\value[8])
  tempmatrix\value[#Zw] + (*self\value[7]  * *matrix\value[9])
  tempmatrix\value[#Zw] + (*self\value[11] * *matrix\value[10])
  tempmatrix\value[#zw] + (*self\value[15] * *matrix\value[11])
  
  tempmatrix\value[#Tx] = (*self\value[0]  * *matrix\value[12])
  tempmatrix\value[#Tx] + (*self\value[4]  * *matrix\value[13])
  tempmatrix\value[#Tx] + (*self\value[8]  * *matrix\value[14])
  tempmatrix\value[#Tx] + (*self\value[12] * *matrix\value[15])
  
  tempmatrix\value[#Ty] = (*self\value[1]  * *matrix\value[12])
  tempmatrix\value[#Ty] + (*self\value[5]  * *matrix\value[13])
  tempmatrix\value[#Ty] + (*self\value[9]  * *matrix\value[14])
  tempmatrix\value[#Ty] + (*self\value[13] * *matrix\value[15])
  
  tempmatrix\value[#Tz] = (*self\value[2]  * *matrix\value[12])
  tempmatrix\value[#Tz] + (*self\value[6]  * *matrix\value[13])
  tempmatrix\value[#Tz] + (*self\value[10] * *matrix\value[14])
  tempmatrix\value[#Tz] + (*self\value[14] * *matrix\value[15])
  
  tempmatrix\value[#Tw] = (*self\value[3]  * *matrix\value[12])
  tempmatrix\value[#Tw] + (*self\value[7]  * *matrix\value[13])
  tempmatrix\value[#Tw] + (*self\value[11] * *matrix\value[14])
  tempmatrix\value[#Tw] + (*self\value[15] * *matrix\value[15])
  
  CopyMemory(@tempmatrix\value[0], @*self\value[0], #MATRIX_SIZEBYTE)
EndProcedure

Procedure rotateX(*Self.MATRIXSTR, angledegree.f)
  Define tempmatrix.MATRIXSTR
  Define sine.f
  Define cosine.f
  
  traceFunction("rotateX()")
  
  angledegree = Radian(angledegree)
  sine = Sin(angledegree)
  cosine = Cos(angledegree)
  
  identity(tempmatrix)
  
  With tempmatrix
	  \value[#Yy] = cosine
	  \value[#Yz] = -sine
	  \value[#Zy] = sine
	  \value[#Zz] = cosine
	EndWith
	
	multiply(*self, tempmatrix)
EndProcedure

Procedure rotateY(*Self.MATRIXSTR, angledegree.f)
  Define tempmatrix.MATRIXSTR
  Define sine.f
  Define cosine.f
  
  traceFunction("rotateY()")
  
  angledegree = Radian(angledegree)
  sine = Sin(angledegree)
  cosine = Cos(angledegree)
  
  identity(tempmatrix)
  
  With tempmatrix
	  \value[#Xx] = cosine
	  \value[#Zx] = sine
	  \value[#Xz] = -sine
	  \value[#Zz] = cosine
	EndWith
	
	multiply(*self, tempmatrix)
EndProcedure

Procedure rotateZ(*Self.MATRIXSTR, angledegree.f)
  Define tempmatrix.MATRIXSTR
  Define sine.f
  Define cosine.f
  
  traceFunction("rotateZ()")
  
  angledegree = Radian(angledegree)
  sine = Sin(angledegree)
  cosine = Cos(angledegree)
  
  identity(tempmatrix)
  
  With tempmatrix
	  \value[#Xx] = cosine
	  \value[#Xy] = -sine
	  \value[#Yx] = sine
	  \value[#Yy] = cosine
	EndWith
	
	multiply(*self, tempmatrix)
EndProcedure

Procedure translate(*Self.MATRIXSTR, x.f, y.f, z.f)
  Define tempmatrix.MATRIXSTR

  traceFunction("translate()")
  
  identity(tempmatrix)
  
  With tempmatrix
	  \value[#Tx] = x
	  \value[#Ty] = y
	  \value[#Tz] = z
	EndWith

	multiply(*self, tempmatrix)
EndProcedure

Procedure perspective(*Self.MATRIXSTR, angle.f, near.f, far.f, aspect.f)
  Define y_scale.f
  Define x_scale.f
  Define frustum_length.f
  
  traceFunction("perspective()")
  
  y_scale = 1.0/Tan(Radian(angle/2))
  x_scale = y_scale/aspect
  frustum_length = (far-near)
  
  With *self
    \value[#Xx] = x_scale
    \value[#Yy] = y_scale
    \value[#Zz] = -( (near+far)/frustum_length )
    \value[#Zw] = -1
    \value[#Tz] = -( (2*near*far)/frustum_length )
    
    \value[#Xy] = 0
    \value[#Xz] = 0
    \value[#Xw] = 0
    \value[#Yx] = 0
    \value[#Yz] = 0
    \value[#Yw] = 0
    \value[#Zx] = 0
    \value[#Zy] = 0
    \value[#Tx] = 0
    \value[#Ty] = 0
    \value[#Tw] = 0
  EndWith
EndProcedure

Procedure orthogonal(*Self.MATRIXSTR, left.f, right.f, bottom.f, top.f, near.f, far.f)
  traceFunction("orthogonal()")
  
  With *Self
    \value[#Xx] = 2/(right-left)
    \value[#Yy] = 2/(top-bottom)
    \value[#Zz] = -2/(far-near)
    \value[#Tx] = -( (right+left)/(right-left) )
    \value[#Ty] = -( (top+bottom)/(top-bottom) )
    \value[#Tz] = -( (far+near)/(far-near) )
    \value[#Tw] = 1
    
    \value[#Xy] = 0
    \value[#Xz] = 0
    \value[#Xw] = 0
    \value[#Yx] = 0
    \value[#Yz] = 0
    \value[#Yw] = 0
    \value[#Zx] = 0
    \value[#Zy] = 0
  EndWith
EndProcedure

Procedure lookAt(*Self.MATRIXSTR, eyeX.f, eyeY.f, eyeZ.f, centerX.f, centerY.f, centerZ.f, upX.f, upY.f, upZ.f)
  Dim axe.f(2)
  Dim regard.f(2)
  Dim normal.f(2)
  Dim newaxe.f(2)
  Define tempmatrix.MATRIXSTR
  
  traceFunction("lookAt()")
  
  axe(0) = upX
  axe(1) = upY
  axe(2) = upZ
  regard(0) = centerX-eyeX
  regard(1) = centerY-eyeY
  regard(2) = centerZ-eyeZ
  
  crossProduct( regard(), axe(), normal() )
  crossProduct( normal(), regard(), newaxe() )
  
  normalize( normal() )
  normalize( newaxe() )
  normalize( regard() )
  
  With tempmatrix
    \value[#Xx] = normal(0)
    \value[#Xy] = newaxe(0)
    \value[#Xz] = -regard(0)
    \value[#Yx] = normal(1)
    \value[#Yy] = newaxe(1)
    \value[#Yz] = -regard(1)
    \value[#Zx] = normal(2)
    \value[#Zy] = newaxe(2)
    \value[#Zz] = -regard(2)
    \value[#Tw] = 1.0
  EndWith
  
  multiply(*self, tempmatrix)
  translate(*self, -eyeX, -eyeY, -eyeZ)
EndProcedure

Procedure push(*Self.MATRIXSTR)
  Define *tempstr.PARENTSTR = AllocateMemory( SizeOf(PARENTSTR) )
  
  traceFunction("push()")
  CopyMemory(@*Self\value[0], @*tempstr\value[0], #MATRIX_SIZEBYTE)
  
  If *Self\parent
    *tempstr\parent = *Self\parent
  EndIf
  
  *Self\parent = *tempstr
EndProcedure

Procedure pop(*Self.MATRIXSTR)
  Define *tempstr.PARENTSTR = *Self\parent
  
  traceFunction("pop()")
  
  If Not *Self\parent
    Debug "No matrix to restore"
    ProcedureReturn
  EndIf
  
  CopyMemory(@*tempstr\value[0], @*Self\value[0], #MATRIX_SIZEBYTE)
  
  If *tempstr\parent
    *Self\parent = *tempstr\parent
  Else
    *self\parent = #Null
  EndIf
  
  FreeMemory(*tempstr)
EndProcedure

Procedure destroy(*Self.MATRIXSTR)
  traceFunction("destroy()")
  FreeMemory(*self)
EndProcedure

Procedure.i createMatrix(label.s)
  Define *temp.MATRIXSTR
  
  traceFunction("createMatrix()")
    
  *temp.MATRIXSTR = AllocateMemory( SizeOf( MATRIXSTR ) )
  *temp\VTable = ?VTable
  *temp\label = label
  ProcedureReturn *temp
EndProcedure

DataSection
  VTable:
  Data.l @debugValues()
  Data.l @getAddress()
  Data.l @identity()
  Data.l @multiply()
  Data.l @rotateX()
  Data.l @rotateY()
  Data.l @rotateZ()
  Data.l @translate()
  Data.l @perspective()
  Data.l @orthogonal()
  Data.l @lookAt()
  Data.l @push()
  Data.l @pop()
  Data.l @destroy()
  
  MatrixIdentity:
  Data.f 1, 0, 0, 0
  Data.f 0, 1, 0, 0
  Data.f 0, 0, 1, 0
  Data.f 0, 0, 0, 1
EndDataSection