3d_matrix.py

#!/usr/bin/env python3
# Licensed under Public Domain

from math import atan, atan2, inf
import numpy as np

class Matrix3D():
    def __init__(self) -> None:
        self.matrix = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1],
        ])

    def move(self, x, y, z):
        self.matrix = np.array([
            [1, 0, 0, x],
            [0, 1, 0, y],
            [0, 0, 1, z],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def rotX(self, rad):
        self.matrix = np.array([
            [1, 0, 0, 0],
            [0, np.cos(rad), -np.sin(rad), 0],
            [0, np.sin(rad), np.cos(rad), 0],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def rotY(self, rad):
        self.matrix = np.array([
            [np.cos(rad), 0, np.sin(rad), 0],
            [0, 1, 0, 0],
            [-np.sin(rad), 0, np.cos(rad), 0],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def rotZ(self, rad):
        self.matrix = np.array([
            [np.cos(rad), -np.sin(rad), 0, 0],
            [np.sin(rad), np.cos(rad), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def multiply(self, x, y, z):
        self.matrix = np.array([
            [x, 0, 0, 0],
            [0, y, 0, 0],
            [0, 0, z, 0],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def swapXY(self):
        self.matrix = np.array([
            [0, 1, 0, 0],
            [1, 0, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def swapXZ(self):
        self.matrix = np.array([
            [0, 0, 1, 0],
            [0, 1, 0, 0],
            [1, 0, 0, 0],
            [0, 0, 0, 1],
        ]) @ self.matrix

    def swapYZ(self):
        self.matrix = np.array([
            [1, 0, 0, 0],
            [0, 0, 1, 0],
            [0, 1, 0, 0],
            [0, 0, 0, 1],
        ]) @ self.matrix
    
    # def lookAt(self, C, P, U):
    #     C = np.array(C)
    #     P = np.array(P)
    #     U = np.array(U).T
    #     # print(C, P, U)
    #     z = nm(C - P)
    #     x = nm(U * z)
    #     y = nm(z * x)

    #     x = x[0]
    #     y = y[0]
    #     z = z[0]
    #     # print(x, y, z)

    #     self.matrix = np.array([
    #         [x[0], x[1], x[2], -x.dot(C[0])],
    #         [y[0], y[1], y[2], -y.dot(C[0])],
    #         [z[0], z[1], z[2], -z.dot(C[0])],
    #         [0, 0, 0, 1],
    #     ])

    def focusing(self, x, y, z):
        print(self.matrix @ np.array([[x], [y], [z], [1]]))

    def printMatrix(self):
        print(self.matrix)

def nm(v: np.ndarray):
    return v / np.linalg.norm(v)

m = Matrix3D()
position = np.array([10, -5, 15])
focusing = np.array([-6, 7, 0])

focus_after_move = position - focusing
# focus_after_move = focusing

m.move(*(-position))
m.printMatrix()
print()

m.rotZ(atan2(focus_after_move[1], focus_after_move[0]))
m.printMatrix()
m.focusing(*focusing)
print()

m.rotX(atan2(focus_after_move[2], focus_after_move[1]))
m.printMatrix()
m.focusing(*focusing)
print()

m.swapYZ()
m.printMatrix()
m.focusing(*focusing)
print()