空间两直线求交点

import numpy as np


def calCoordinateFrom2Lines(P_1, s_1, Q_1, s_2):
    m_1 = np.array([s_1[0]])
    n_1 = np.array([s_1[1]])
    p_1 = np.array([s_1[2]])
    m_2 = np.array([s_2[0]])
    n_2 = np.array([s_2[1]])
    x_1 = np.array([P_1[0]])
    y_1 = np.array([P_1[1]])
    z_1 = np.array([P_1[2]])
    x_2 = np.array([Q_1[0]])
    y_2 = np.array([Q_1[1]])
    t = (m_2 * (y_2 - y_1) + n_2 * (x_1 - x_2)) / ((m_2 * n_1) - (m_1 * n_2))
    x = x_1 + m_1 * t
    y = y_1 + n_1 * t
    z = z_1 + p_1 * t
    M = np.array([x, y, z]).transpose()
    return M


pass

'''if __name__ == '__main__':
    P_1 = np.array([-108.45, 174.45, 0])
    s_1 = np.array([-335.6, -77.27, 0])
    Q_1 = np.array([227.15, 174.45, 0])
    s_2 = np.array([335.6, -77.27, 0])'''
if __name__ == '__main__':
    P_1 = [-108.45, 174.45, 0]
    s_1 = [-335.6, -77.27, 0]
    Q_1 = [227.15, 174.45, 0]
    s_2 = [335.6, -77.27, 0]
'''if __name__ == '__main__':
    P_1 = np.array([-55.88, -531.80, 0])
    P_2 = np.array([55.88, -233.23, 0])
    Q_1 = np.array([55.88, -531.80, 0])
    Q_2 = np.array([-55.88, -233.23, 0])'''
print(calCoordinateFrom2Lines(P_1, s_1, Q_1, s_2))