C++实现求两条直线的交点，以及已知直线外一点求垂足

已知直线上的两个点或者已知直线的斜率和截距两种情况下求交点和垂足。
两种情况来自于实际求解中可以找到边界上的两个点列出直线方程，也可以用更准确的求法即获取多个点拟合一条直线。

1.求两直线的交点（两点式）

直线L1的方程为：
y − y 1 y 2 − y 1 = x − x 1 x 2 − x 1 \frac{y - y_1}{y_2 - y_1} = \frac{x - x_1}{x_2 - x_1} y2−y1y−y1=x2−x1x−x1
直线L2的方程为：
y − y 3 y 4 − y 3 = x − x 3 x 4 − x 3 = t \frac{y - y_3}{y_4 - y_3} =\frac{x - x_3}{x_4 - x_3} =t y4−y3y−y3=x4−x3x−x3=t
可得：
y = ( y 4 − y 3 ) ∗ t + y 3 x = ( x 4 − x 3 ) ∗ t + x 3 y = (y_4 -y_3)*t+y_3\\ x = (x_4 -x_3)*t+x_3 y=(y4−y3)∗t+y3x=(x4−x3)∗t+x3
将上述x,y的等式代入直线L1的方程：
( y 4 − y 3 ) ∗ t + ( y 3 − y 1 ) y 2 − y 1 = ( x 4 − x 3 ) ∗ t + ( x 3 − x 1 ) x 2 − x 1 \frac{(y_4 - y_3)*t+(y_3-y_1)}{y_2 - y_1} =\frac{(x_4 - x_3)*t+(x_3-x_1)}{x_2 - x_1} y2−y1(y4−y3)∗t+(y3−y1)=x2−x1(x4−x3)∗t+(x3−x1)
令
a 1 = y 4 − y 3 , a 2 = y 3 − y 1 , a 3 = y 2 − y 1 b 1 = x 4 − x 3 , b 2 = x 3 − x 1 , b 3 = x 2 − x 1 a_1 = y_4-y_3,a_2 =y_3-y_1,a_3 =y_2-y_1\\ b_1 = x_4-x_3,b_2 =x_3-x_1,b_3 =x_2-x_1 a1=y4−y3,a2=y3−y1,a3=y2−y1b1=x4−x3,b2=x3−x1,b3=x2−x1
可得
a 1 t + a 2 a 3 = b 1 t + b 2 b 3 t = a 3 b 2 − a 2 b 3 a 1 b 3 − a 3 b 1 \frac{a_1t+a_2}{a_3} =\frac{b_1t+b_2}{b_3}\\ t = \frac{a_3b_2-a_2b_3}{a_1b_3-a_3b_1} a3a1t+a2=b3b1t+b2t=a1b3−a3b1a3b2−a2b3
将t代回原x,y的公式即可求得交点坐标。

C++实现：

 Point p1,p2,p3,p4;//对应的四个点，根据实际应用场景赋值double a1, a2, a3, b1, b2, b3;double t;Point pp1, pp2;a1 = p4.y - p3.y;a2 = p3.y - p1.y;a3 = p2.y - p1.y;b1 = p4.x - p3.x;b2 = p3.x - p1.x;b3 = p2.x - p1.x;t = (a3 * b2 - a2 * b3) / (a1 * b3 - a3 * b1);pp1.x = (p4.x - p3.x) * t + p3.x;pp1.y = (p4.y - p3.y) * t + p3.y;

2.求两直线的交点（斜截式）

直线L1的方程为：
y − y 1 y 2 − y 1 = x − x 1 x 2 − x 1 = t \frac{y - y_1}{y_2 - y_1} = \frac{x - x_1}{x_2 - x_1}=t y2−y1y−y1=x2−x1x−x1=t
直线L2的方程为
y = k x + b y=kx+b y=kx+b
由L1的方程可求得：
x = ( x 2 − x 1 ) ∗ t + x 1 y = ( y 2 − y 1 ) ∗ t + y 1 x = (x_2-x_1)*t+x_1\\y = (y_2-y_1)*t+y_1 x=(x2−x1)∗t+x1y=(y2−y1)∗t+y1
代入L2的方程可得：
( y 2 − y 1 ) ∗ t + y 1 = k ( ( x 2 − x 1 ) ∗ t + x 1 ) + b t = ( k x 1 + b − y 1 ) ( y 2 − y 1 ) − k ( x 2 − x 1 ) (y_2-y_1)*t+y_1=k((x_2-x_1)*t+x_1)+b\\ t=\frac{(kx_1+b-y_1)}{(y_2 - y_1)-k(x_2-x_1)} (y2−y1)∗t+y1=k((x2−x1)∗t+x1)+bt=(y2−y1)−k(x2−x1)(kx1+b−y1)
将t代回原x,y的公式即可求得交点坐标。

C++实现：

 Point p1,p2;//对应的两个点，根据实际应用场景赋值vector<Point> points;//用来拟合直线的点Vec4f line;fitLine(points, line, DIST_L2, 0, 0.01, 0.01);//拟合得到直线，具体求解根据实际情况double cos_theta = line[0];double sin_theta = line[1];double x0 = line[2], y0 = line[3];double k = sin_theta / cos_theta;double b = y0 - k * x0;t = (k * p1.x + b - p1.y) / (p2.y - p1.y - k * (p2.x - p1.x));Point pp1;pp1.x = (p2.x - p1.x) * t + p1.x;pp1.y = (p2.y - p1.y) * t + p1.y;

3.求垂足（两点式）

直线L的方程为：
y − y 1 y 2 − y 1 = x − x 1 x 2 − x 1 \frac{y - y_1}{y_2 - y_1} = \frac{x - x_1}{x_2 - x_1} y2−y1y−y1=x2−x1x−x1
L的方向向量为：
( a , b ) = ( x 2 − x 1 , y 2 − y 1 ) (a,b) = (x_2-x_1,y_2-y_1) (a,b)=(x2−x1,y2−y1)
则垂直于L的垂线的方向向量为：
( − b , a ) = ( y 1 − y 2 , x 2 − x 1 ) (-b,a) = (y_1-y_2,x_2-x_1) (−b,a)=(y1−y2,x2−x1)
经过点(x3,y3)的垂线的点向式为：
x − x 3 − b = y − y 3 a = t \frac{x-x_3}{-b} = \frac{y-y_3}{a} = t −bx−x3=ay−y3=t
可得
x = − b t + x 3 y = a t + y 3 x = -bt+x_3\\ y=at+y_3 x=−bt+x3y=at+y3
代入直线L的方程可求出t：
t = a ( y 1 − y 3 ) + b ( x 3 − x 1 ) a 2 + b 2 t = \frac{a(y_1 - y_3)+b(x_3-x_1)}{a^2 + b^2} t=a2+b2a(y1−y3)+b(x3−x1)
将t代回原x,y的公式即可求得垂足。

C++实现：

 Point p1,p2;//对应的两个点，根据实际应用场景赋值  int a = p2.x - p1.x;int b = p2.y - p1.y;double t = (a * (p1.y - p3.y) + b * (p3.x - p1.x));t = t / (a * a + b * b);//分两步计算，直接一步实现会由于精度问题导致结果出错Point p;p.x = -b * t + p3.x;p.y = a * t + p3.y;

4.求垂足（斜截式）

直线L的方程为：
y = k 1 x + b 1 y=k_1x+b_1 y=k1x+b1
假设垂线的方程为：
y = k 2 x + b 2 y=k_2x+b_2 y=k2x+b2
互相垂直的两条直线斜率满足以下关系（斜率不存在的情况需要另外求）：
k 2 = − 1 k 1 k_2 = -\frac{1}{k_1} k2=−k11
由（x3,y3)在垂线上可求得
b 2 = y 3 − k 2 x 3 b_2 = y_3-k_2x_3 b2=y3−k2x3
联立方程组
y = k 1 x + b 1 y = k 2 x + b 2 y = k_1x+b_1\\ y = k_2x+b_2 y=k1x+b1y=k2x+b2
得到
x = b 2 − b 1 k 1 − k 2 x=\frac{b_2-b_1}{k_1-k_2} x=k1−k2b2−b1
将x代入以上任何一个直线方程即可得到y，即求得垂足(x,y)。

C++实现：

 Point p3;//直线外一点vector<Point> points;//用来拟合直线的点Vec4f pline;fitLine(points, pline, DIST_L2, 0, 0.01, 0.01);cos_theta = pline[0];sin_theta = pline[1];x0 = pline[2], y0 = pline[3];double k1 = sin_theta / cos_theta;double b1 = y0 - k1 * x0;double b2 = 0;double k2 = -1 / k1;b2 = p3.y - k2 * p3.x ;pp1.x = (b2 - b1) / (k1 - k2);pp1.y = k2 * pp1.x + b2;

5.斜率不存在

若直线是横向的，即直线方程为y=c，则直线外一点找的垂足的y为c，x与直线外一点的x相同。竖向同理，不赘述。