空间顶点到平面的距离计算的证明及其源码

目的：最近写C++代码，遇到一些基础的算法。如计算顶点到平面的距离。

上篇文章已经介绍了平面以及它的数学表达形式：
几何表达形式$Plane$：平面法向量$N=\left(\begin{array}{c}{n}_0\\ n_1\\ n_2\end{array}\right)$，以及平面上的一个顶点$P_0=\left(\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right)$
顶点的表达形式：$P_1=\left(\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right)$

计算$P_1$到平面$Plane$的距离。
$Plane$的表达可以转化为$n_0(x-x_0)+n_1(y-y_0)+n_2(z-z_0)=0$，计算得到$n_{0}x+n_{1}y+n_{2}z+d=0$其中$d=-n_0{x}_0-n_1{y}_0-n_2{z}_0=-N^T\cdot P_0$

推导公式如下：

计算红色的顶点$P_1$到蓝色平面$Plane$的距离。黄色的顶点为平面上的一点$P_0$。顶点到平面距离定义：它垂直到平面的点的距离（在图中使用灰色线段表示）。

基于垂直三角形的算法得到灰色线段的长度为$dist$
$$dist=\Vert P_0-P_1\Vert \cdot cos(\theta )(1)$$
其中$\theta$是两个向量$P_0{P}_1$和平面法向量$N$的夹角，
$$cos(\theta )=\frac{|N^T\cdot (P_0-P_1)|}{\Vert P_0-P_1\Vert \cdot \Vert N\Vert }(2)$$
将公式(2)带入公式(1)可以得到如下:
$$dist=\frac{|N^T\cdot (P_0-P_1)|}{\Vert N\Vert }(3)$$
将向量$P_0=\left(\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right)$,$P_1=\left(\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right)$,$N=\left(\begin{array}{c}{n}_0\\ n_1\\ n_2\end{array}\right)$带入公式(3)得到的方程
$$dist=\frac{|N^T\cdot P_0-N^T\cdot P_1|}{\Vert N\Vert }=\frac{|-d-N^T\cdot P_1|}{\Vert N\Vert }=\frac{|N^T\cdot P_1+d|}{\Vert N\Vert }(4)$$

可以将上述向量的形式改写为代数形式为:
$$dist=\frac{|n_0{x}_1+n_1{y}_1+n_2{z}_1+d|}{\sqrt{n_0^2+n_1^2+n_2^2}}(5)$$

源码的如下,它是基于向量的模式写的。它需要你配置好Eigen库：

float Pnt3d2PlaneDist(Eigen::Vector4f& fn, Eigen::Vector3f& pnt)
	{
    
		Eigen::Vector3f n = Eigen::Vector3f(fn[0], fn[1], fn[2]);
		float sd = n.norm();
		if (sd < 1e-8)
			return std::numeric_limits<float>::max();
		float sb = std::abs(n.dot(pnt) + fn[3]);
		float d = sb / sd;
		return d;
	}

详细见网站：https://mathinsight.org/distance_point_plane