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))
Intersection of two straight lines in space
Guess you like
Origin blog.csdn.net/m0_61509658/article/details/122583529
Recommended
Ranking