登山第六梯：线段求交（平面、空间）——有缘千里来相会

一 简介

    二维和三维空间线段求交参数方程推导，最终可得到一个能够统一表达的表达式。

二 算法推导

2.1 2D线段求交

 

（1）判断两条线段是否存在交点

如上图所示，线段AB和线段CD若相交，则点A和点B分别位于线段CD两侧，且点C和点D分别位于线段AB两侧。

而判断两个点是否位于线段的两侧可通过线段的叉积来判断。

满足(AB x CA)*(AB x DA)<0 && (CD x AC)*(CD x BC)<0，则线段AB和CD存在交点，否则，不相交。

由于二维向量无法进行叉积，则每个向量的Z值均设置为0。

（2）参数方程求解

建立方程组：

P = A + AB*t

P = C + CD*u

二者联立为：

A + AB*t = C + CD*u

->AB*t = (C-A) + CD*u

此时，如果要你解算t，则需要消去u。因此，两边同时叉乘CD（相同向量叉乘等于0）

t*(AB x CD) = (CA x CD)

在此处有两种解法：

①：根据共线向量相除可转换为向量模相除

t = (CA x CD)/(AB x CD)

->t = ||(CA x CD)||/||(AB x CD)||

由于三角形面积等于相邻两向量的叉积的模的1/2，有人又可以理解为：

t = S▲ACP/abs(S▲ADP - S▲CPB)

②：继续使用向量运算求解

t*(AB x CD) = (CA x CD)

->t*((AB x CD)*(AB x CD)) = (CA x CD)*(AB x CD)

->t = (CA x CD)*(AB x CD)/||AB x CD||^2

最后，将t代入

P = A + AB*t得到交点P的坐标。

2.2 3D线段求交

（1）空间线段共面判断

由于空间中的线段存在异面，因此，通过计算三点所在平面法向量是否共线进行判断：

若(AB x AC) // (AB x AD)，则两条线段共面。

（2）是否相交

首先，我们转换为直线求交，通过判断AB是否与CD平行进行确定，若不平行，则必定存在相交；

（3）参数方程求解

同2D线段参数求解一样：

t = (CA x CD)*(AB x CD)/||AB x CD||^2

但在空间上，由于浮点型数据的存在，空间直线交点会存在浮点误差，并不完全相交。

因此，可转换为计算两直线的公垂线，求得两直线的最近点对P1和P2，也即垂足。

这样，依然可使用上述参数方程求解：

t = (CA x CD)*(AB x CD)/||AB x CD||^2

从该参数表达式可观察到，

若AB与CD平行，则AB x CD等于0，则分母为0；

若要求P1和P2，则分别计算得到t和u，通过判断

||P1P2||≤eps进行判断二者是否相交，并可用P1P2的中点作为最终的交点结果。

刚刚之前将空间线段求交转换为了空间直线求交，若是空间线段，则t和u的取值均为(0,1)。

2.3 2-3D线段求交

最终可以总结得到：2D和3D线段求交均可通过一个参数表达式进行求解，仅仅需要将二维向量的Z值修改为0即可。

三 代码展示

空间直线求交代码：

//计算空间直线之间的公垂线垂足
//基于Eigen开发
/*
输入：line_a、line_b，均为6维向量，前三维表示直线上一点的坐标，后三维表示直线的方向向量
输出：pt1_seg、pt2_seg，最小公垂线的两个垂足，4维向量，前三维表示空间坐标，第四维是零
*/
void lineToLineSegment(const Eigen::VectorXd &line_a, const  Eigen::VectorXd &line_b,
       Eigen::Vector4d &pt1_seg, Eigen::Vector4d &pt2_seg)
{
       // point + direction = 2nd point  
       Eigen::Vector4d p1 = Eigen::Vector4d::Zero();
       Eigen::Vector4d p2 = Eigen::Vector4d::Zero();
       Eigen::Vector4d dir1 = Eigen::Vector4d::Zero();
       p1.head<3>() = line_a.head<3>();        //直线上点的坐标  
       dir1.head<3>() = line_a.segment<3>(3);   //方向向量  
       p2 = p1 + dir1;      //直线上一点加上直线方向向量，等于直线上另外一点；直线上两个点的差等于方向向量  
                                          // point + direction = 2nd point  
       Eigen::Vector4d q1 = Eigen::Vector4d::Zero();
       Eigen::Vector4d q2 = Eigen::Vector4d::Zero();
       Eigen::Vector4d dir2 = Eigen::Vector4d::Zero();
       q1.head<3>() = line_b.head<3>();
       dir2.head<3>() = line_b.segment<3>(3);
       q2 = q1 + dir2;
       // a = x2 - x1 = line_a[1] - line_a[0]  
       Eigen::Vector4d u = dir1;
       // b = x4 - x3 = line_b[1] - line_b[0]  
       Eigen::Vector4d v = dir2;
       // c = x2 - x3 = line_a[1] - line_b[0]  
       Eigen::Vector4d w = p2 - q1;
       double a = u.dot(u);
       double b = u.dot(v);
       double c = v.dot(v);
       double d = u.dot(w);
       double e = v.dot(w);
       double denominator = a*c - b*b;
       double sc, tc;
       // Compute the line parameters of the two closest points  
       if (denominator < 1e-5)          // The lines are almost parallel  
       {
              sc = 0.0;
              tc = (b > c ? d / b : e / c);  // Use the largest denominator  
       }
       else
       {
              sc = (b*e - c*d) / denominator;
              tc = (a*e - b*d) / denominator;
       }
       // Get the closest points  
       pt1_seg = Eigen::Vector4d::Zero();
       pt1_seg = p2 + sc * u;
       pt2_seg = Eigen::Vector4d::Zero();
       pt2_seg = q1 + tc * v;
}