Здравствуйте, дорогие хабровчане, недавно столкнулся с проблемой, связанной с написанием алгоритма из названия в turboprolog2.0, более того я не нашел нигде готовой реализации в трехмерном пространстве на нормальных языках программирования.
Имеется решение только для двухмерного пространства. В связи с чем пришлось придумывать его самому.
![Начальные условия. Первая прямая может быть отрезком Начальные условия. Первая прямая может быть отрезком](https://habrastorage.org/getpro/habr/upload_files/4f6/219/409/4f621940995661943a20cb384332216d.png)
Первая прямая задана точками 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 можно найти координаты точки пересечения.
Далее для удобства заменим нашу систему уравнений на матрицу коэффициентов.
![](https://habrastorage.org/getpro/habr/upload_files/60e/b02/163/60eb02163b16c55610fb1be43a2d6135.png)
Которую можно схлопнуть.
![](https://habrastorage.org/getpro/habr/upload_files/49f/65c/aac/49f65caac04eab8915bd255bd65e8933.png)
Дальше все решаем через Крамера, привожу код к решению системы на 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