Поиск пересечений между отрезком и прямой или прямой и прямой в трехмерном пространстве

Здравствуйте, дорогие хабровчане, недавно столкнулся с проблемой, связанной с написанием алгоритма из названия в turboprolog2.0, более того я не нашел нигде готовой реализации в трехмерном пространстве на нормальных языках программирования.

Имеется решение только для двухмерного пространства. В связи с чем пришлось придумывать его самому.

Первая прямая задана точками P00 и P01,а вторая — Р10 и Р11. Каждая точка задана координатами X,Y,Z (например, P00 — X00,Y00,Z00). Требуется найти точку пересечения, если таковая имеется. И проверить принадлежит ли она первому отрезку или находится на его продолжении (первой прямой).

Введем вектора DP0 и DP1, которые параллельны соответствующим им прямым. Вычислим их компоненты.

# вектор первой прямой DX0 = X01 - X00 DY0 = Y01 - Y00 DZ0 = Z01 - Z00 # вектор второй прямой DX1 = X11 - X10 DY1 = Y11 - Y10 DZ1 = Z11 - Z10

Точка пересечения прямых принадлежит обеим прямым (очевидно?), и её можно задать формулой.

P = P00 + DP0 * t = P10 + DP1 * s

Разложим в систему:

X00 + DX0 * t = X10 + DX1 * s

Y00 + DY0 * t = Y10 + DY1 * s

Z00 + DZ0 * t = Z10 + DZ1 * s

Удобнее представить систему в таком виде:

DX0 * t — DX1 * s = X10 — X00

DY0 * t — DY1 * s = Y10 — Y00

DZ0 * t — DZ1 * s = Z10 — Z00

Если система имеет решение, то через s или t можно найти координаты точки пересечения.

Далее для удобства заменим нашу систему уравнений на матрицу коэффициентов.

Которую можно схлопнуть.

Дальше все решаем через Крамера, привожу код к решению системы на Python.

# первая прямая X00 = 0 Y00 = 0 Z00 = 0 X01 = 0 Y01 = 0 Z01 = 5 # вторая прямая X10 = 5 Y10 = 0 Z10 = 0 X11 = 5 Y11 = 0 Z11 = 5  # вектор первой прямой DX0 = X01 - X00 DY0 = Y01 - Y00 DZ0 = Z01 - Z00 # вектор второй прямой DX1 = X11 - X10 DY1 = Y11 - Y10 DZ1 = Z11 - Z10  # компоненты системы A = DX0 B = -DX1 C = X10 - X00 D = DY0 E = -DY1 F = Y10 - Y00 G = DZ0 H = -DZ1 I = Z10 - Z00  # схлопнем систему ;) D += A E += B F += C G += A H += B I += C   # Принадлежность точки пересечения к первой прямой def belong0(X, Y, Z):     Q1 = (X - X00) * DY0     Q2 = (Y - Y00) * DX0     Q3 = (Y - Y00) * DZ0     Q4 = (Z - Z00) * DY0     return Q1 == Q2 and Q3 == Q4   # Принадлежность точки пересечения ко второй прямой def belong1(X, Y, Z):     Q1 = (X - X10) * DY1     Q2 = (Y - Y10) * DX1     Q3 = (Y - Y10) * DZ1     Q4 = (Z - Z10) * DY1     return Q1 == Q2 and Q3 == Q4   # Определение координат пересечения через параметр s, принадлежности точки пересечения к первой прямой/отрезку def s_to_p(S):     X = X10 + DX1 * S     Y = Y10 + DY1 * S     Z = Z10 + DZ1 * S     print(X, Y, Z)     if belong0(X, Y, Z):         if belong_section(X, Y, Z):             print("on section")             print(X, Y, Z)         else:             print("on line, not on section")             print(X, Y, Z)      else:         print("no intersection points, crossing lines")   # Определение координат пересечения через параметр t, принадлежности точки пересечения к первой прямой/отрезку def t_to_p(T):     X = X00 + DX0 * T     Y = Y00 + DY0 * T     Z = Z00 + DZ0 * T     print(X,Y,Z)     if belong1(X, Y, Z):         if belong_section(X, Y, Z):             print("on section")             print(X, Y, Z)         else:             print("on line, not on section")             print(X, Y, Z)     else:         X = X01 + DX0 * T         Y = Y01 + DY0 * T         Z = Z01 + DZ0 * T         if belong1(X, Y, Z):             if belong_section(X, Y, Z):                 print("on section  ")                 print(X, Y, Z)             else:                 print("on line, not on section ")                 print(X, Y, Z)         else:             print("no intersection points, crossing lines")   def between(A, B, C):     return A <= B <= C or C <= B <= A   # проверка лежит ли точка пересечения на первом отрезке или на первой прямой, другими словами принадлежность к отрезку def belong_section(X, Y, Z):     return between(X00, X, X01) and between(Y00, Y, Y01) and between(Z00, Z, Z01)  det =  D*H - E*G if det != 0:     detASS= F*H-I*E     T = detASS/det     t_to_p(T)  else:     v = DX0 * DX1 + DY0 * DY1 + DZ0 * DZ1     l0 = (DX0**2+DY0**2+DZ0**2)**0.5     l1 = (DX1 ** 2 + DY1 ** 2 + DZ1 ** 2) ** 0.5     cos = v / l0 / l1     if abs(1-cos) < 0.00000000001:         if belong_section(X10,Y10,Z10):             print("same lines")         else:             print("parallel lines")      else:         print(cos)         print("no intersection points")

Ссылка на полный код на Python && turboprolog 2.0