空间顶点到直线的距离计算及其源码

目的：最近研究项目遇到点到直线的距离，在空间几何中，非常重要的一个基础的算法，因此记录。

直线的向量表达： L : r → = P 0 → + t N 0 → L:overrightarrow{r}=overrightarrow{P_0}+toverrightarrow{N_0} L:r =P0​ ​+tN0​ ​；其中 P 0 → = ( x 0 y 0 z 0 ) overrightarrow{P_0}=begin{pmatrix} x_0 \ y_0 \ z_0 end{pmatrix} P0​ ​=⎝⎛​x0​y0​z0​​⎠⎞​, N 0 → = ( n x 0 n y 0 n z 0 ) overrightarrow{N_0}=begin{pmatrix} n_x^0 \ n_y^0 \ n_z^0 end{pmatrix} N0​ ​=⎝⎛​nx0​ny0​nz0​​⎠⎞​
顶点的向量表达： P → : ( x 1 y 1 z 1 ) overrightarrow{P}: begin{pmatrix} x_1 \ y_1 \ z_1 end{pmatrix} P :⎝⎛​x1​y1​z1​​⎠⎞​。

顶点到直线的距离有多种方法，先介绍一个非常简单的方法，它基于向量表达得到。

1）方法一：
假设顶点 P → overrightarrow{P} P 投影到直线上得到 P ′ → overrightarrow{P'} P′ ,根据点到直线的距离定义， ∣ P P ′ → ∣ |overrightarrow{PP'}| ∣PP′ ∣为顶点 P → overrightarrow{P} P 到直线 L L L的距离。如下图：

下面将介绍计算过程：
因为顶点 P ′ → overrightarrow{P'} P′ 在直线上，它可以表达为： P ′ → = P 0 → + t ′ N 0 → overrightarrow{P'}=overrightarrow{P_0}+t'overrightarrow{N_0} P′ =P0​ ​+t′N0​ ​，所以：
P P ′ → = ( P 0 → + t ′ N 0 → ) − P → overrightarrow{PP'}=(overrightarrow{P_0}+t'overrightarrow{N_0})-overrightarrow{P} PP′ =(P0​ ​+t′N0​ ​)−P

因为 P P ′ → overrightarrow{PP'} PP′ 和直线 L L L垂直，可以得到： P P ′ → T ⋅ N 0 → = 0 overrightarrow{PP'}^T cdot overrightarrow{N_0}=0 PP′ T⋅N0​ ​=0

P P ′ → T ⋅ N 0 → = 0 = > ( P 0 → + t ′ N 0 → − P → ) T ⋅ N 0 → = 0 = > ( t ′ N 0 → + P 0 → − P → ) T ⋅ N 0 → = 0 = > t ′ N 0 → T ⋅ N 0 → = ( P → T − P 0 → T ) ⋅ N 0 → = > t ′ = ( P → T − P 0 → T ) ⋅ N 0 → / ( N 0 → T ⋅ N 0 → ) overrightarrow{PP'}^T cdot overrightarrow{N_0}=0 \ =>(overrightarrow{P_0}+t'overrightarrow{N_0}-overrightarrow{P})^T cdot overrightarrow{N_0}=0 \ =>(t'overrightarrow{N_0}+overrightarrow{P_0}-overrightarrow{P})^T cdot overrightarrow{N_0}=0 \ =>t'overrightarrow{N_0}^T cdot overrightarrow{N_0}=(overrightarrow{P}^T-overrightarrow{P_0}^T)cdot overrightarrow{N_0} \ =>t' = (overrightarrow{P}^T-overrightarrow{P_0}^T)cdot overrightarrow{N_0} / (overrightarrow{N_0}^Tcdot overrightarrow{N_0}) PP′ T⋅N0​ ​=0=>(P0​ ​+t′N0​ ​−P )T⋅N0​ ​=0=>(t′N0​ ​+P0​ ​−P )T⋅N0​ ​=0=>t′N0​ ​T⋅N0​ ​=(P T−P0​ ​T)⋅N0​ ​=>t′=(P T−P0​ ​T)⋅N0​ ​/(N0​ ​T⋅N0​ ​)

通过 t ′ t' t′计算 P ′ = P 0 → + t ′ N 0 → P'=overrightarrow{P_0}+t'overrightarrow{N_0} P′=P0​ ​+t′N0​ ​

d = ∣ P P ′ → ∣ = ∣ ( P 0 → + t ′ N 0 → ) − P → ∣ d=|overrightarrow{PP'}|=|(overrightarrow{P_0}+t'overrightarrow{N_0})-overrightarrow{P}| d=∣PP′ ∣=∣(P0​ ​+t′N0​ ​)−P ∣

得到点到直线的距离 d d d。C++代码如下：

#include
#include
#include 

void ComputePntProj2Line3D(Eigen::Vector3d& lpnt, Eigen::Vector3d& ldir, Eigen::Vector3d& pnt, Eigen::Vector3d& proj)
{
    //计算pi平面
    //std::cerr << "lpnt: "<< lpnt.transpose() <
    //start to compute dist between pnt and line(lpnt, ldir)
    Eigen::Vector3d proj;
    ComputePntProj2Line3D(lpnt,  ldir,  pnt,  proj);
    //std::cerr << "proj: "<< proj.transpose() <
    Eigen::Vector3d lp,ld;
    ld = Eigen::Vector3d(3.0, 1.0, 1.0);
    lp = Eigen::Vector3d(-4.0, -5.0, -1.0);
    Eigen::Vector3d p;
    p = Eigen::Vector3d(-6.0, 1.0, 21.0);
    double dist = ComputePnt2Line3D(lp, ld, p);
    std::cerr <<"dist: " << dist < 
代码测试：
 
 2）方法二：(后续添加)