3D Similarity Transformation

3d_trans.py

import numpy as np


def trans(coordinates_to_go, scale, ta, tb, tc, theta, phi, gamma):
    """
    :param coordinates_to_go: [x, y, z].T
    :param scale:
    :param ta:
    :param tb:
    :param tc:
    :param theta:
    :param phi:
    :param gamma:
    :return:
    """
    # Rotation matrices around the X, Y, and Z axis
    RX = np.array([[1, 0, 0, 0],
                   [0, np.cos(theta), -np.sin(theta), 0],
                   [0, np.sin(theta), np.cos(theta), 0],
                   [0, 0, 0, 1]])

    RY = np.array([[np.cos(phi), 0, -np.sin(phi), 0],
                   [0, 1, 0, 0],
                   [np.sin(phi), 0, np.cos(phi), 0],
                   [0, 0, 0, 1]])

    RZ = np.array([[np.cos(gamma), -np.sin(gamma), 0, 0],
                   [np.sin(gamma), np.cos(gamma), 0, 0],
                   [0, 0, 1, 0],
                   [0, 0, 0, 1]])

    # Composed rotation matrix with (RX, RY, RZ)
    R = np.dot(np.dot(RX, RY), RZ)

    # Translation matrix
    T = np.array([[1, 0, 0, ta],
                  [0, 1, 0, tb],
                  [0, 0, 1, tc],
                  [0, 0, 0, 1]])

    return np.dot(T, np.dot(R, coordinates_to_go * scale))


if __name__ == '__main__':
    coordinates_to_go = np.array([[3793336.837361, 5070760.003423, 0, 1]]).T
    scale = -4.07242e-6 + 1
    ta = -565.7346
    tb = -50.4058
    tc = -465.2895
    theta = -1.91513e-6
    phi = 1.60365e-6
    gamma = -9.09546e-6
    print(trans(coordinates_to_go, scale, ta, tb, tc, theta, phi, gamma))