import numpy as np

'''求直线方向向量[m,n,p]


    def Direction_vector(x1, y1, z1, x2, y2, z2):
    m = x1 - x2
    n = y1 - y2
    p = z1 - z2
    print('m', m)
    print('n', n)
    print('p', p)
    return m, n, p'''

'''三点法平面方程求解'''


def Find_plane_equation(xo1, yo1, zo1, xo2, yo2, zo2, xo3, yo3, zo3):
    a = (yo2 - yo1) * (zo3 - zo1) - (zo2 - zo1) * (yo3 - yo1)
    b = (xo3 - xo1) * (zo2 - zo1) - (xo2 - xo1) * (zo3 - zo1)
    c = (xo2 - xo1) * (yo3 - yo1) - (xo3 - xo1) * (yo2 - yo1)
    d = -(a * xo1 + b * yo1 + c * zo1)
    Equation_parameters = np.array([a, b, c, d])
    print('Plane equation parameters', Equation_parameters)

    return Equation_parameters


'''求解直线与平面的交点'''


def Find_intersection(x1, y1, z1, x2, y2, z2, a, b, c, d):
    m = x1 - x2
    n = y1 - y2
    p = z1 - z2
    t = (-a * x1 - b * y1 - c * z1 - d) / (a * m + b * n + c * p)

    x = m * t + x1
    y = n * t + y1
    z = p * t + z1
    X = np.array([x, y, z])
    print('Intersection', X)

    return X


a, b, c, d = Find_plane_equation(727.34, 143.62, 0, 544.14, 161.87, 0, 471.0, 120.24, 0)

X = Find_intersection(555.45, 120.24, 155.28, 511.61, 120.24, 74.67, a, b, c, d)