直线相交的交点(Line-Line Intersection)

最近一直在写关于几何方面的代码。涉及到一些非常基础的计算。如在二维世界中，直线和直线的交点。之前一直没有太多优化这段代码。

直线表达：一般直线的表达为 $A x + B y + C = 0$

两个直线的表达：
$A_1x+B_1y+C_1=0 \\ A_2x+B_2y+C_2=0$

一般情况下直接计算方程组即可。使用高斯消元法得到：
$\frac{ B_1 * C_2 - B_2 * C_1}{A_1 * B_2 - A_2 * B_1} \\ y = \frac{ A_2 * C_1 - A_1 * C_2}{A_1 * B_2 - A_2 * B_1}$

c++代码如下：

#include <iostream>
#include<Eigen/Eigen>

const double KMIN_DOUBLE_LINE_THRESHOLD = 1e-8;

bool ComputeLine2LineIntersection(const Eigen::Vector3d& f1, const Eigen::Vector3d& f2, Eigen::Vector2d& pnt)
{
    
    
    double det = f1[0]*f2[1] - f2[0]*f1[1];
    if(std::abs(det) < KMIN_DOUBLE_LINE_THRESHOLD)
    {
    
    
        return false;
    }
    else
    {
    
    
        double x = (f1[1]*f2[2] - f2[1]*f1[2]) / det;
        double y = (f2[0]*f1[2] - f1[0]*f2[2]) / det;
        pnt[0] = x;
        pnt[1] = y;
        return true;
    }
}

int main(int argc, char** argv)
{
    
    
	Eigen::Vector3d a(1,2,3);
	Eigen::Vector3d b(4,5,6);
	Eigen::Vector2d p;
	ComputeLine2LineIntersection(a, b, p);
	std::cerr <<"pnt: " << p.transpose() << std::endl;
	return 0;
}

测试的结果（正确）：
在这里插入图片描述

补充：如果线段的表达方式为：(用首位顶点表达方式)
$l_1: (x_0, y_0) -> (x_1,y_1)$

$l_2: (x_2, y_2) -> (x_3,y_3)$

需要转化到上面的通用表达方式表示为：
$A = y_1-y_0 \\ B = x_0-x_1 \\ C =-( Ax_0+By_0)$
转化函数如下：

    //Ax+By+C=0(A,B,C)
    Eigen::Vector3d ComputeFunctionFromPnts(const Eigen::Vector2d& s, const Eigen::Vector2d& e)
    {
    
    
        double A = e[1] - s[1];
        double B = s[0] - e[0];
        double C = -(A*s[0] + B*s[1]);
        return Eigen::Vector3d(A,B,C);
    }

融合到上面得到：

#include <iostream>
#include<Eigen/Eigen>

const double KMIN_DOUBLE_LINE_THRESHOLD = 1e-8;

bool ComputeLine2LineIntersection(const Eigen::Vector3d& f1, const Eigen::Vector3d& f2, Eigen::Vector2d& pnt)
{
    
    
    double det = f1[0]*f2[1] - f2[0]*f1[1];
    if(std::abs(det) < KMIN_DOUBLE_LINE_THRESHOLD)
    {
    
    
        return false;
    }
    else
    {
    
    
        double x = (f1[1]*f2[2] - f2[1]*f1[2]) / det;
        double y = (f2[0]*f1[2] - f1[0]*f2[2]) / det;
        pnt[0] = x;
        pnt[1] = y;
        return true;
    }
}

//Ax+By+C=0(A,B,C)
Eigen::Vector3d ComputeFunctionFromPnts(const Eigen::Vector2d& s, const Eigen::Vector2d& e)
{
    
    
    double A = e[1] - s[1];
    double B = s[0] - e[0];
    double C = -(A*s[0] + B*s[1]);
    return Eigen::Vector3d(A,B,C);
}


int main(int argc, char** argv)
{
    
    
	Eigen::Vector2d s0(0.0,0.0);
	Eigen::Vector2d e0(1.0,1.0);
	Eigen::Vector2d s1(1.0,0.0);
	Eigen::Vector2d e1(0.0,1.0);

	Eigen::Vector3d a = ComputeFunctionFromPnts(s0,e0);
	Eigen::Vector3d b = ComputeFunctionFromPnts(s1,e1);

	Eigen::Vector2d p;
	ComputeLine2LineIntersection(a, b, p);
	std::cerr <<"pnt: " << p.transpose() << std::endl;
	return 0;
}

测试的结果（正确）：

直线相交的交点(Line-Line Intersection)

猜你喜欢