目的:最近写C++代码,遇到一些基础的算法。如计算顶点到平面的距离。
上篇文章已经介绍了平面以及它的数学表达形式:
几何表达形式
P
l
a
n
e
Plane
Plane:平面法向量
N
=
(
n
0
n
1
n
2
)
N=\begin{pmatrix} n_0 \\ n_1 \\ n_2 \end{pmatrix}
N=⎝⎛n0n1n2⎠⎞,以及平面上的一个顶点
P
0
=
(
x
0
y
0
z
0
)
P_0=\begin{pmatrix} x_0 \\ y_0 \\ z_0 \end{pmatrix}
P0=⎝⎛x0y0z0⎠⎞
顶点的表达形式:
P
1
=
(
x
1
y
1
z
1
)
P_1=\begin{pmatrix} x_1 \\ y_1 \\ z_1 \end{pmatrix}
P1=⎝⎛x1y1z1⎠⎞
计算
P
1
P_1
P1到平面
P
l
a
n
e
Plane
Plane的距离。
P
l
a
n
e
Plane
Plane的表达可以转化为
n
0
(
x
−
x
0
)
+
n
1
(
y
−
y
0
)
+
n
2
(
z
−
z
0
)
=
0
n_0(x-x_0)+n_1(y-y_0)+n_2(z-z_0)=0
n0(x−x0)+n1(y−y0)+n2(z−z0)=0,计算得到
n
0
x
+
n
1
y
+
n
2
z
+
d
=
0
n_0x+n_1y+n_2z+d=0
n0x+n1y+n2z+d=0其中
d
=
−
n
0
x
0
−
n
1
y
0
−
n
2
z
0
=
−
N
T
⋅
P
0
d=-n_0x_0-n_1y_0-n_2z_0=-N^T \cdot P_0
d=−n0x0−n1y0−n2z0=−NT⋅P0
推导公式如下:
计算红色的顶点 P 1 P_1 P1到蓝色平面 P l a n e Plane Plane的距离。黄色的顶点为平面上的一点 P 0 P_0 P0。顶点到平面距离定义:它垂直到平面的点的距离(在图中使用灰色线段表示)。
基于垂直三角形的算法得到灰色线段的长度为
d
i
s
t
dist
dist
d
i
s
t
=
∥
P
0
−
P
1
∥
⋅
c
o
s
(
θ
)
(
1
)
dist=\lVert P_0-P_1 \rVert \cdot cos(\theta) \space \space (1)
dist=∥P0−P1∥⋅cos(θ) (1)
其中
θ
\theta
θ是两个向量
P
0
P
1
P_0P_1
P0P1和平面法向量
N
N
N的夹角,
c
o
s
(
θ
)
=
∣
N
T
⋅
(
P
0
−
P
1
)
∣
∥
P
0
−
P
1
∥
⋅
∥
N
∥
(
2
)
cos(\theta) = \cfrac {|N^T \cdot (P_0-P_1)|}{\lVert P_0-P_1 \rVert \cdot \lVert N \rVert} \space \space (2)
cos(θ)=∥P0−P1∥⋅∥N∥∣NT⋅(P0−P1)∣ (2)
将公式(2)带入公式(1)可以得到如下:
d
i
s
t
=
∣
N
T
⋅
(
P
0
−
P
1
)
∣
∥
N
∥
(
3
)
dist=\cfrac {|N^T \cdot (P_0-P_1)|}{ \lVert N \rVert} \space \space (3)
dist=∥N∥∣NT⋅(P0−P1)∣ (3)
将向量
P
0
=
(
x
0
y
0
z
0
)
P_0=\begin{pmatrix} x_0 \\ y_0 \\ z_0 \end{pmatrix}
P0=⎝⎛x0y0z0⎠⎞,
P
1
=
(
x
1
y
1
z
1
)
P_1=\begin{pmatrix} x_1 \\ y_1 \\ z_1 \end{pmatrix}
P1=⎝⎛x1y1z1⎠⎞,
N
=
(
n
0
n
1
n
2
)
N=\begin{pmatrix} n_0 \\ n_1 \\ n_2 \end{pmatrix}
N=⎝⎛n0n1n2⎠⎞带入公式(3)得到的方程
d
i
s
t
=
∣
N
T
⋅
P
0
−
N
T
⋅
P
1
∣
∥
N
∥
=
∣
−
d
−
N
T
⋅
P
1
∣
∥
N
∥
=
∣
N
T
⋅
P
1
+
d
∣
∥
N
∥
(
4
)
dist=\cfrac {|N^T \cdot P_0 - N^T \cdot P_1|}{ \lVert N \rVert}=\cfrac {|-d - N^T \cdot P_1|}{ \lVert N \rVert} =\cfrac {|N^T \cdot P_1 + d|}{ \lVert N \rVert} \space \space (4)
dist=∥N∥∣NT⋅P0−NT⋅P1∣=∥N∥∣−d−NT⋅P1∣=∥N∥∣NT⋅P1+d∣ (4)
可以将上述向量的形式改写为代数形式为:
d
i
s
t
=
∣
n
0
x
1
+
n
1
y
1
+
n
2
z
1
+
d
∣
n
0
2
+
n
1
2
+
n
2
2
(
5
)
dist = \cfrac{|n_0x_1 + n_1y_1+n_2z_1+d|}{\sqrt{n_0^2+n_1^2+n_2^2}} \space \space (5)
dist=n02+n12+n22∣n0x1+n1y1+n2z1+d∣ (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