Calculation of distance from space vertex to straight line and its source code

Purpose: A recent research project encountered the distance from a point to a line, a fundamental algorithm that is very important in spatial geometry, and is therefore documented.

Vector representation of a straight line: $L:\overrightarrow{r}=\overrightarrow{P_0}+t\overrightarrow{N_0}$；其中$\overrightarrow{P_0}=\left(\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right)$, $\overrightarrow{N_0}=\left(\begin{array}{c}{n}_x^0\\ n_y^0\\ n_z^0\end{array}\right)$
Vertex vector representation: $\overrightarrow{P}:\left(\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right)$。

There are many methods for the distance from a vertex to a straight line. First, a very simple method is introduced, which is obtained based on vector expression.

1) Method 1:
Suppose the vertex $\overrightarrow{P}$Projected onto the line to get $\overrightarrow{P^{\prime }}$, defined according to the distance from a point to a straight line, $|\overrightarrow{P{P}^{\prime }}|$ is the vertex$\overrightarrow{P}$to line $L$ distance. As shown below:

The calculation process will be introduced below:
because the vertex $\overrightarrow{P^{\prime }}$On a straight line, it can be expressed as: $\overrightarrow{P^{\prime }}=\overrightarrow{P_0}+t^{\prime }\overrightarrow{N_0}$，所以：
$$\overrightarrow{P{P}^{\prime }}=(\overrightarrow{P_0}+t^{\prime }\overrightarrow{N_0})-\overrightarrow{P}$$

Since $\overrightarrow{P{P}^{\prime }}$and straight line $L$ is vertical, we can get:${\overrightarrow{P{P}^{\prime }}}^T\cdot \overrightarrow{N_0}=0$

$$\begin{gathered} {\overrightarrow{P{P}^{\prime }}}^T\cdot \overrightarrow{N_0}=0 \\ =>(\overrightarrow{P_0}+t^{\prime }\overrightarrow{N_0}-\overrightarrow{P}{)}^T\cdot \overrightarrow{N_0}=0 \\ =>(t^{\prime }\overrightarrow{N_0}+\overrightarrow{P_0}-\overrightarrow{P}{)}^T\cdot \overrightarrow{N_0}=0 \\ =>t^{\prime }{\overrightarrow{N_0}}^T\cdot \overrightarrow{N_0}=({\overrightarrow{P}}^T-{\overrightarrow{P_0}}^T)\cdot \overrightarrow{N_0} \\ =>t^{\prime }=({\overrightarrow{P}}^T-{\overrightarrow{P_0}}^T)\cdot \overrightarrow{N_0}/({\overrightarrow{N_0}}^T\cdot \overrightarrow{N_0}) \end{gathered}$$

by $t^{\prime }$ Compute$P^{\prime }=\overrightarrow{P_0}+t^{\prime }\overrightarrow{N_0}$

$$d=|\overrightarrow{P{P}^{\prime }}|=|(\overrightarrow{P_0}+t^{\prime }\overrightarrow{N_0})-\overrightarrow{P}|$$

Get the distance dd from the point to the line$d$ . The C++ code is as follows:

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

void ComputePntProj2Line3D(Eigen::Vector3d& lpnt, Eigen::Vector3d& ldir, Eigen::Vector3d& pnt, Eigen::Vector3d& proj)
{
    
    
    //计算pi平面
    //std::cerr << "lpnt: "<< lpnt.transpose() <<std::endl;
    //std::cerr << "ldir: "<< ldir.transpose() <<std::endl;
    //std::cerr << "pnt: "<< pnt.transpose() <<std::endl;
    double t = (pnt.dot(ldir) - lpnt.dot(ldir)) / ldir.squaredNorm();
    //std::cerr <<"t: " << t <<std::endl;
    proj = t * ldir + lpnt; 
}
double ComputePnt2Line3D(Eigen::Vector3d& lpnt, Eigen::Vector3d& ldir, Eigen::Vector3d& pnt)
{
    
    
    //start to compute dist between pnt and line(lpnt, ldir)
    Eigen::Vector3d proj;
    ComputePntProj2Line3D(lpnt,  ldir,  pnt,  proj);
    //std::cerr << "proj: "<< proj.transpose() <<std::endl;
    double d = (proj - pnt).norm();
    return d;
}

int main(int argc, char** argv)
{
    
    
    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 <<std::endl;
    return 0;
}

Code test:

2) Method 2: (subsequent addition)