【附代碼】判斷線段是否相交算法（Python，C++）

作者：小豬快跑

基礎數學&計算數學，從事優化領域5年+，主要研究方向：MIP求解器、整數規劃、隨機規劃、智能優化算法

如有錯誤，歡迎指正。如有更好的算法，也歡迎交流！！！——@小豬快跑

相關文獻

測試電腦配置

博主三千元電腦的渣渣配置：

CPU model: AMD Ryzen 7 7840HS w/ Radeon 780M Graphics, instruction set [SSE2|AVX|AVX2|AVX512]
Thread count: 8 physical cores, 16 logical processors, using up to 16 threads

基礎

這里假設：
$$\begin{gathered} \overrightarrow{OA}=\vec{a}=(x_a,y_a) \\ \overrightarrow{OB}=\vec{b}=(x_b,y_b) \\ \overrightarrow{OC}=\vec{p}=(x_c,y_c) \\ \overrightarrow{CB}=\vec{w} \\ \angle AOB=\alpha \end{gathered}$$

向量旋轉

任意向量都能表示成：
$$\left(\begin{array}{c}r\cos \alpha \\ r\sin \alpha \end{array}\right)$$
假設向量逆時針旋轉了 $\beta$，那么我們容易知道旋轉后向量是：
$$\left(\begin{array}{c}r\cos (\alpha +\beta )\\ r\sin (\alpha +\beta )\end{array}\right)$$
那么容易得到：
$$\left(\begin{array}{c}r\cos (\alpha +\beta )\\ r\sin (\alpha +\beta )\end{array}\right)=\left(\begin{array}{rr}\cos \alpha & ?\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right)\left(\begin{array}{c}r\cos \alpha \\ r\sin \alpha \end{array}\right)$$
于是旋轉向量就是：
$$\left(\begin{array}{rr}\cos \alpha & ?\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right)$$

向量縮放

$$\left(\begin{array}{rr}r& 0\\ 0& r\end{array}\right)$$

向量投影

推導

主要利用 $\overrightarrow{OC}$ 和 $\overrightarrow{CB}$ 垂直，點積為0：
$$\begin{array}{r}\begin{array}{l}\vec{w}=\vec{b}?\vec{p}\\ \vec{w}?\vec{p}=0\end{array}\}?(\vec{b}?\vec{p})?\vec{p}=0\\ \vec{p}=k\vec{a}\end{array}??\begin{array}{r}(\vec{b}?k\vec{a})?k\vec{a}=0\\ \vec{p}=k\vec{a}\end{array}\}?\vec{p}=\frac{\vec{a}?\vec{b}}{\vec{a}?\vec{a}}\vec{a}$$
那么投影矩陣
$$\begin{array}{lc}& P\vec{b}=\vec{p}=\vec{a}\frac{\vec{a}?\vec{b}}{\vec{a}?\vec{a}}\\ ?& P=\frac{\vec{a}{\vec{a}}^T}{{\vec{a}}^T\vec{a}}\end{array}$$

點乘

點乘（Dot Product）的結果是點積，又稱數量積或標量積（Scalar Product）

定義

$$\vec{a}?\vec{b}=|\vec{a}||\vec{b}|\cos \alpha$$

推導

那么如何用解析幾何來表示呢？

我們其實可以把 $\vec{a}$ 旋轉 $\alpha$ 再縮放 $|\vec{b}|/|\vec{a}|$ 倍，就是 $\vec{b}$ 了：
$$\begin{array}{lc}& \left(\begin{array}{cc}\frac{|\vec{b}|}{|\vec{a}|}& 0\\ 0& \frac{|\vec{b}|}{|\vec{a}|}\end{array}\right)\left(\begin{array}{rr}\cos \alpha & ?\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right)\left(\begin{array}{c}{x}_a\\ y_a\end{array}\right)=\left(\begin{array}{c}{x}_b\\ y_b\end{array}\right)\\ ?& \left(\begin{array}{c}(x_a\cos \alpha ?y_a\sin \alpha )|\vec{b}|\\ (x_a\sin \alpha +y_a\cos \alpha )|\vec{b}|\end{array}\right)=\left(\begin{array}{c}{x}_b|\vec{a}|\\ y_b|\vec{a}|\end{array}\right)\\ ?& |\vec{a}||\vec{b}|\cos \alpha =x_a{x}_b+y_a{y}_b\end{array}$$

幾何意義

點乘的結果表示 $\vec{a}$ 在 $\vec{b}$ 方向上的投影與 $\vec{b}$ 的乘積，反映了兩個向量在方向上的相似度，結果越大越相似。基于結果可以判斷這兩個向量是否是同一方向，是否正交垂直，具體對應關系為：

1. $\vec{a}?\vec{b}>0$ 則方向基本相同，夾角在0°到90°之間
2. $\vec{a}?\vec{b}=0$ 則正交，相互垂直
3. $\vec{a}?\vec{b}<0$ 則方向基本相反，夾角在90°到180°之間

叉乘

叉乘（Cross Product）又稱向量積（Vector Product）。

定義

$$\vec{a}\times \vec{b}=|\vec{a}||\vec{b}|\sin \alpha$$

推導

那么如何用解析幾何來表示呢？

我們其實可以把 $\vec{a}$ 旋轉 $\alpha$ 再縮放 $|\vec{b}|/|\vec{a}|$ 倍，就是 $\vec{b}$ 了：
$$\begin{array}{lc}& \left(\begin{array}{cc}\frac{|\vec{b}|}{|\vec{a}|}& 0\\ 0& \frac{|\vec{b}|}{|\vec{a}|}\end{array}\right)\left(\begin{array}{rr}\cos \alpha & ?\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right)\left(\begin{array}{c}{x}_a\\ y_a\end{array}\right)=\left(\begin{array}{c}{x}_b\\ y_b\end{array}\right)\\ ?& \left(\begin{array}{c}(x_a\cos \alpha ?y_a\sin \alpha )|\vec{b}|\\ (x_a\sin \alpha +y_a\cos \alpha )|\vec{b}|\end{array}\right)=\left(\begin{array}{c}{x}_b|\vec{a}|\\ y_b|\vec{a}|\end{array}\right)\\ ?& |\vec{a}||\vec{b}|\sin \alpha =x_a{y}_b?x_b{y}_a\end{array}$$

幾何意義

如果以向量 $\vec{a}$ 和 $\vec{b}$ 為邊構成一個平行四邊形，那么這兩個向量外積的模長與這個平行四邊形的面積相等。

判斷線段是否相交

我們有了上面的基礎后，其實思路就一下打開了！

其實我們只要想著 $\overrightarrow{AB}$ 的兩邊是 $C$ 和 $D$ ，那么也就是說 $\overrightarrow{AB}\times \overrightarrow{AD}$ 和 $\overrightarrow{AB}\times \overrightarrow{AC}$ 有正有負，同時呢 $\overrightarrow{CD}\times \overrightarrow{CA}$ 和 $\overrightarrow{CD}\times \overrightarrow{CB}$ 有正有負（這里要注意一下叉乘可能為0的情況，比如說 $A$ 在 $\overrightarrow{CD}$ 上）。這里我們有正有負采用直接判斷而不是相乘小于零，這是因為相乘可能存在數值溢出等問題。而且一般的，和零的判斷比乘法快很多。

我們直接上測試用例看看效果！！！

代碼

C++

#include <iostream>
#include <chrono>using namespace std;int cross_product(int x1, int y1, int x2, int y2) {// 計算向量 (x1, y1) 和向量 (x2, y2) 的叉積return x1 * y2 - x2 * y1;
}int dot_product(int x1, int y1, int x2, int y2) {// 計算向量 (x1, y1) 和向量 (x2, y2) 的點乘return x1 * x2 + y1 * y2;
}bool is_intersected(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4) {/*判斷線段 (x1, y1)-(x2, y2) 和線段 (x3, y3)-(x4, y4) 是否相交AB×ACAB×ADCD×CACD×CB*/if ((max(x1, x2) < min(x3, x4)) or (max(x3, x4) < min(x1, x2)) or (max(y1, y2) < min(y3, y4)) or (max(y3, y4) < min(y1, y2))) {return false;}int abx = x2 - x1;int aby = y2 - y1;int acx = x3 - x1;int acy = y3 - y1;int adx = x4 - x1;int ady = y4 - y1;int bcx = x3 - x2;int bcy = y3 - y2;int cdx = x4 - x3;int cdy = y4 - y3;int cp1 = cross_product(abx, aby, acx, acy);int cp2 = cross_product(abx, aby, adx, ady);int cp3 = cross_product(cdx, cdy, -acx, -acy);int cp4 = cross_product(cdx, cdy, -bcx, -bcy);// 如果兩個叉積的乘積小于0，則兩個向量在向量 (x1, y1)-(x2, y2) 的兩側，即線段相交if (((cp1 > 0 and 0 > cp2) or (cp1 < 0 and 0 < cp2) or cp1 == 0 or cp2 == 0) and((cp3 > 0 and 0 > cp4) or (cp3 < 0 and 0 < cp4) or cp3 == 0 or cp4 == 0)) {return true;}return false;
}int test(int n) {int res = 0;for (auto x1 = 0; x1 < n; x1++) {for (auto y1 = 0; y1 < n; y1++) {for (auto x2 = 0; x2 < n; x2++) {for (auto y2 = 0; y2 < n; y2++) {if (x1 == x2 and y1 == y2) {continue;}for (auto x3 = 0; x3 < n; x3++) {for (auto y3 = 0; y3 < n; y3++) {for (auto x4 = 0; x4 < n; x4++) {for (auto y4 = 0; y4 < n; y4++) {if (x3 == x4 and y3 == y4) {continue;}res += is_intersected(x1, y1, x2, y2, x3, y3, x4, y4);}}}}}}}}return res;
}int main() {auto start = std::chrono::high_resolution_clock::now();std::cout << test(7) << std::endl;auto finish = std::chrono::high_resolution_clock::now();std::chrono::duration<double> elapsed = finish - start;std::cout << "Elapsed time: " << elapsed.count() << " s\n" << std::endl;return 0;
}

Python

from time import time
import math
from numba import njit@njit
def cross_product(x1, y1, x2, y2):"""計算向量 (x1, y1) 和向量 (x2, y2) 的叉積"""return x1 * y2 - x2 * y1@njit
def dot_product(x1, y1, x2, y2):"""計算向量 (x1, y1) 和向量 (x2, y2) 的點乘"""return x1 * x2 + y1 * y2@njit
def is_intersected(x1, y1, x2, y2, x3, y3, x4, y4):"""判斷線段 (x1, y1)-(x2, y2) 和線段 (x3, y3)-(x4, y4) 是否相交AB×ACAB×ADCD×CACD×CB"""if (max(x1, x2) < min(x3, x4)) or (max(x3, x4) < min(x1, x2)) or (max(y1, y2) < min(y3, y4)) or (max(y3, y4) < min(y1, y2)):return Falseabx = x2 - x1aby = y2 - y1acx = x3 - x1acy = y3 - y1adx = x4 - x1ady = y4 - y1bcx = x3 - x2bcy = y3 - y2cdx = x4 - x3cdy = y4 - y3cp1 = cross_product(abx, aby, acx, acy)cp2 = cross_product(abx, aby, adx, ady)cp3 = cross_product(cdx, cdy, -acx, -acy)cp4 = cross_product(cdx, cdy, -bcx, -bcy)# 如果兩個叉積的乘積小于0，則兩個向量在向量 (x1, y1)-(x2, y2) 的兩側，即線段相交if ((cp1 > 0 > cp2) or (cp1 < 0 < cp2) or cp1 == 0 or cp2 == 0) and ((cp3 > 0 > cp4) or (cp3 < 0 < cp4) or cp3 == 0 or cp4 == 0):return Truereturn Falsedef test(n):res = 0for x1 in range(n):for y1 in range(n):for x2 in range(n):for y2 in range(n):if x1 == x2 and y1 == y2:continuefor x3 in range(n):for y3 in range(n):for x4 in range(n):for y4 in range(n):if x3 == x4 and y3 == y4:continueres += is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)return resif __name__ == '__main__':s = time()print(test(7))print(time() - s)

畫圖代碼

# main.py
import matplotlib.pyplot as plt
from shapely.geometry import Point, LineString, Polygon
from shapely.plotting import plot_polygon, plot_points, plot_linefrom csdn_line_intersect import is_intersected
from figures import BLUE, GRAY, set_limitsfig = plt.figure(1, figsize=(9, 9), dpi=300)
fig.subplots_adjust(wspace=0.5, hspace=0.5)  # 調整邊距和子圖的間距ax = fig.add_subplot(4, 4, 1)
x1, y1, x2, y2 = 1, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 3, 2
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 2)
x1, y1, x2, y2 = 1, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 3, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 3)
x1, y1, x2, y2 = 1, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 2, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 4)
x1, y1, x2, y2 = 1, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 5)
x1, y1, x2, y2 = 1, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 1, 2
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 6)
x1, y1, x2, y2 = 2, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 1, 2
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 7)
x1, y1, x2, y2 = 2, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 8)
x1, y1, x2, y2 = 2, 2, 3, 1
x3, y3, x4, y4 = 1, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 9)
x1, y1, x2, y2 = 1, 2, 3, 1
x3, y3, x4, y4 = 1, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 10)
x1, y1, x2, y2 = 1, 2, 3, 2
x3, y3, x4, y4 = 1, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 11)
x1, y1, x2, y2 = 2, 2, 3, 2
x3, y3, x4, y4 = 1, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 12)
x1, y1, x2, y2 = 2, 2, 3, 2
x3, y3, x4, y4 = 2, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 13)
x1, y1, x2, y2 = 2, 2, 3, 2
x3, y3, x4, y4 = 3, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 14)
x1, y1, x2, y2 = 1, 2, 3, 2
x3, y3, x4, y4 = 3, 3, 1, 1
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 15)
x1, y1, x2, y2 = 2, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 3, 3
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)ax = fig.add_subplot(4, 4, 16)
x1, y1, x2, y2 = 1, 1, 3, 1
x3, y3, x4, y4 = 1, 3, 3, 3
a = LineString([(x1, y1), (x2, y2)])
b = LineString([(x3, y3), (x4, y4)])
plot_line(a, ax=ax, color=GRAY)
plot_line(b, ax=ax, color=GRAY)
plot_points(a.intersection(b), ax=ax, color=BLUE)
ax.set_title(f'is_intersected:{is_intersected(x1, y1, x2, y2, x3, y3, x4, y4)}')
set_limits(ax, 0, 4, 0, 4)plt.savefig('output.png')
plt.show()

測試結果

C++: 0.0157648 s
Python(numba): 1.3376786708831787 s
Python(no numba): 3.585803985595703 s
Python(shapely): 73.45080494880676 s