class Point3D:
    X = 0.0
    Y = 0.0
    Z = 0.0

    def getX(self):
        return self.X;

    def getY(self):
        return self.Y;

    def getZ(self):
        return self.Z;

    def __init__(self, x, y, z=0):
        self.X = x
        self.Y = y
        self.Z = z

import numpy as np
import math
from decimal import Decimal
from app.static.test.Point3D import Point3D


class CoordinateConversion:
    four_parameter = []

    def __init__(self, original_coordinates, target_coordinates):
        if len(original_coordinates) != len(target_coordinates):
            print("坐标对不匹配")
        else:
            pairs = len(original_coordinates)
            L1 = []
            B1 = []
            for i in range(pairs):
                # 自动构造list
                L1.append([target_coordinates[i].getX() - original_coordinates[i].getX()])
                L1.append([target_coordinates[i].getY() - original_coordinates[i].getY()])

                B1.append([1, 0, original_coordinates[i].getX(), -original_coordinates[i].getY()])
                B1.append([0, 1, original_coordinates[i].getY(), original_coordinates[i].getX()])

            P1 = np.identity(2 * pairs)

            # 将LIST转换成矩阵
            L = np.array(L1)
            P = np.array(P1)
            B = np.array(B1)
			# 根据公式中的矩阵计算公式进行计算
            # 计算第一 矩阵
            first = np.dot(B.T, P)
            # 计算第二矩阵
            second = np.dot(first, B)
            # 计算第三 矩阵的逆
            third = np.linalg.inv(second)
            # 计算第四 矩阵
            fourth = np.dot(third, B.T)
            # 计算第五 矩阵
            fifth = np.dot(fourth, P)
            # 计算第六 矩阵
            sixth = np.dot(fifth, L)

            # 计算m和angle
            m = np.sqrt(np.power(sixth[2, 0] + 1, 2) + np.power(sixth[3, 0], 2))
            angle = np.arctan(sixth[3, 0] / (sixth[2, 0] - 1))

            self.four_parameter = [sixth[0, 0], sixth[1, 0], abs(angle), m]

    def CoordinateConversion(self, point):
        old_x = Decimal(point.getX())
        old_y = Decimal(point.getY())

        cos = Decimal(math.cos(self.four_parameter[2]))  # 计算cos
        sin = Decimal(math.sin(self.four_parameter[2]))  # 计算sin

        part1 = cos * old_x
        part2 = old_y * sin
        part3 = part1 - part2
        part4 = Decimal(self.four_parameter[3]) * part3
        x = Decimal(self.four_parameter[0]) + part4
        y = Decimal(self.four_parameter[1]) + Decimal(self.four_parameter[3]) * (old_x * sin + old_y * cos)
        return [round(x, 4), round(y, 4)]

# 构造控制点对  
original1 = Point3D(4362289.9410, 423919.5280)
original2 = Point3D(4363111.4550, 422745.9980)
original3 = Point3D(4306670.108, 511168.332)
target1 = Point3D(4362215.4200, 423920.8270)
target2 = Point3D(4363036.9200, 422747.3170)
target3 = Point3D(4306596.5348, 511168.1440)

original = [original1, original2, original3]
target = [target1, target2, target3]

CC = CoordinateConversion(original, target)

point1 = Point3D(43060.108, 5118.332)  # 请根据实际情况替换这些值 
newXY = CC.CoordinateConversion(point1)
print(newXY)