# HC3Mat.py
# Andrew Davison, ad@coe.psu.ac.th, April 2025
# https://en.wikipedia.org/wiki/Homogeneous_coordinates

'''
Support for 3D Homogenous coordinates.

See HC2Mat.py for 2D Homo. Coords.

Operations:
  * identity
  * translation
  * scaling
  * shearing (X, Y, and Z)
  * rotation about X, Y, and Z axes
  * reflection in planes XY, YZ, and XZ

Others:
  * matrix multiplication
  * matrix print
  * apply a matrix to a (x,y) coordinate, returning a new coord
'''


import math
from matplotlib.patches import Polygon


def identMat():
  return [ [1, 0, 0, 0],
           [0, 1, 0, 0],
           [0, 0, 1, 0],
           [0, 0, 0, 1] ]


def transMat(a, b, c):
  # translate by 'a' along the x-axis, 'b' along y-axis, 'c' along z
  return [ [1, 0, 0, a],
           [0, 1, 0, b],
           [0, 0, 1, c],
           [0, 0, 0, 1] ]

def scaleMat(a, b, c):
  # scale by 'a' along the x-axis, 'b' along y-axis, 'c' along z
  return [ [a, 0, 0, 0],
           [0, b, 0, 0],
           [0, 0, c, 0],
           [0, 0, 0, 1] ]


def shearXMat(dy, dz):
  # X is unchanged while Y and Z change
  return [ [1, dy, dz, 0],
           [0,  1,  0, 0],
           [0,  0,  1, 0],
           [0,  0,  0, 1] ]

def shearYMat(dx, dz):
  # Y is unchanged while X and Z change
  return [ [1,  0,  1, 0],
           [dx, 1, dz, 0],
           [0,  0,  1, 0],
           [0,  0,  0, 1] ]

def shearZMat(dx, dy):
  # Z is unchanged while X and Y change
  return [ [ 1,  0, 1, 0],
           [ 0,  1, 0, 0],
           [dx, dy, 1, 0],
           [ 0,  0, 0, 1] ]


def rotZMat(angle):
  # 'roll' is the angle of rotation about the z-axis
  theta = math.radians(angle)
  return [ [math.cos(theta), -math.sin(theta), 0, 0],
           [math.sin(theta),  math.cos(theta), 0, 0],
           [             0,                0,  1, 0],
           [             0,                0,  0, 1]  ]

def rotXMat(angle):
  # 'pitch' is the angle of rotation about the x-axis
  theta = math.radians(angle)
  return [ [1,               0,                0,  0],
           [0, math.cos(theta), -math.sin(theta),  0],
           [0, math.sin(theta),  math.cos(theta),  0],
           [0,               0,                0,  1]  ]

def rotYMat(angle):
  # 'yaw' is the angle of rotation about the y-axis
  theta = math.radians(angle)
  return [ [math.cos(theta),  0,   math.sin(theta), 0],
           [0,                1,                0,  0],
           [-math.sin(theta), 0,  math.cos(theta),  0],
           [0,                0,                0,  1]  ]


# reflection in planes (scaling variants)

def reflectXYMat(m):
  return [ [1, 0, 0, 0],
           [0, 1, 0, 0],
           [0, 0,-1, 0],
           [0, 0, 0, 1] ]
            
def reflectYZMat(m):
  return [ [-1, 0, 0, 0],
           [ 0, 1, 0, 0],
           [ 0, 0, 1, 0],
           [ 0, 0, 0, 1] ]
            
def reflectXZMat(m):
  return [ [1, 0, 0, 0],
           [0,-1, 0, 0],
           [0, 0, 1, 0],
           [0, 0, 0, 1] ]
            

# --------------------------------------

def multMats(matrixA, matrixB):
  result = [[0 for _ in range(4)] for _ in range(4)]
  for i in range(4):
    for j in range(4):
      for k in range(4):
        result[i][j] += matrixA[i][k] * matrixB[k][j]
  return result


def multMatsList(mats):
  result = identMat()
  for mat in mats:
    result = multMats(result, mat)
  return result


def printMat(matrix):
  for row in matrix:
    print("[" + "  ".join(f"{val:7.3f}" for val in row) + "]")


def applyMat(matrix, coord):
  vec = [coord[0], coord[1], coord[2], 1]
  nVec = [0, 0, 0, 0]
  for i in range(4):
    for k in range(4):
      nVec[i] += matrix[i][k] * vec[k]
  return (nVec[0]/nVec[3], nVec[1]/nVec[3], nVec[2]/nVec[3])



# --------------------------------------
if __name__ == "__main__":
  m1 = [ [1, 2, 3, 1],
         [4, 5, 6, 1],
         [4, 5, 6, 1],
         [7, 8, 9, 1] ]

  m2 = [ [9, 8, 7, 1],
         [6, 5, 4, 1],
         [6, 5, 4, 1],
         [3, 2, 1, 1] ]

  printMat( multMats(m1, m2))


  print("Example 2")
  coords = [(8,0,1),(9,0,1),(10,0,1),(11,0,1),(12,0,1)]
  m = multMats( transMat(10,0,1), 
                multMats( rotZMat(90), transMat(-10,0, -1)))
  printMat(m)
  nCoords = [applyMat(m, c) for c in coords]
  for x, y, z in nCoords:
    print(f"({x:.2f}, {y:.2f}, {z:.2f})", end =' ')
  print()