目的:最近写C++代码,遇到一些基础的算法。需要空间直线和平面的交点。
直线的向量表达: L : r → = P 0 → + t N 0 → L:\overrightarrow{r}=\overrightarrow{P_0}+t\overrightarrow{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=⎝⎛x0y0z0⎠⎞, 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=⎝⎛nx0ny0nz0⎠⎞
平面的向量表达: π : N 1 → T ⋅ X + d 1 = 0 \pi: \overrightarrow{N_1}^T \cdot X+d_1=0 π:N1T⋅X+d1=0;其中 N 1 → = ( n x 1 n y 1 n z 1 ) \overrightarrow{N_1}=\begin{pmatrix} n_x^1 \\ n_y^1 \\ n_z^1 \end{pmatrix} N1=⎝⎛nx1ny1nz1⎠⎞, d d d为标量。
对于直线到平面的交点它分为三种情况:
1)在平面上
2)平行平面
3)和平面相交
图示如下:
左图和中间图可以看到,它和平面没有交点。因此需要判断直线是否和平面相交。在传统的空间中,平面和直线平面的话,直线和平面的发向量是垂直的。因此可以通过计算 N 0 → \overrightarrow{N_0} N0和 N 1 → \overrightarrow{N_1} N1之间的夹角。如果它是垂直表面直线和平面平行。
c o s ( θ ) − > N 0 → T ⋅ N 1 → cos(\theta)->\overrightarrow{N_0}^T \cdot \overrightarrow{N_1} cos(θ)−>N0T⋅N1
如果它接近于0表示它平行。
下面将介绍相交的情况
如果平面和直线相交,对于直线来说,只需要计算 t t t即可。
L : r → = P 0 → + t N 0 → L:\overrightarrow{r}=\overrightarrow{P_0}+t\overrightarrow{N_0} L:r=P0+tN0带入公式 π : N 1 → T ⋅ X + d 1 = 0 \pi: \overrightarrow{N_1}^T \cdot X+d_1=0 π:N1T⋅X+d1=0得到如下:
N 1 → T ⋅ ( P 0 → + t N 0 → ) + d 1 = 0 = > N 1 → T ⋅ P 0 → + t N 1 → T ⋅ N 0 → + d 1 = 0 t = ( − d 1 − N 1 → T ⋅ P 0 → ) / N 1 → T ⋅ N 0 → \overrightarrow{N_1}^T \cdot (\overrightarrow{P_0}+t\overrightarrow{N_0})+d_1=0 \\ =>\overrightarrow{N_1}^T \cdot \overrightarrow{P_0}+t \overrightarrow{N_1}^T \cdot \overrightarrow{N_0}+d_1=0 \\ t=(-d_1 - \overrightarrow{N_1}^T \cdot \overrightarrow{P_0})/\overrightarrow{N_1}^T \cdot \overrightarrow{N_0} N1T⋅(P0+tN0)+d1=0=>N1T⋅P0+tN1T⋅N0+d1=0t=(−d1−N1T⋅P0)/N1T⋅N0
因为向量 P 0 → \overrightarrow{P_0} P0, N 0 → \overrightarrow{N_0} N0, N 1 → \overrightarrow{N_1} N1都是已知,因此可以求的 t = t n t=t_n t=tn。
带入得到
r n → = P 0 → + t n N 0 → \overrightarrow{r_n}=\overrightarrow{P_0}+t_n\overrightarrow{N_0} rn=P0+tnN0
基于上述的公式,源码如下:
bool LineRayToPlanePnt(Eigen::Vector3f& o_orign, Eigen::Vector3f& o_dir, Eigen::Vector4f& fn, Eigen::Vector3f& inter_pnt)
{
Eigen::Vector3f N = Eigen::Vector3f(fn[0], fn[1], fn[2]);
float D = fn[3];
if (std::abs(o_dir.dot(N)) < 1e-8)
{
return false;
}
float t = -(o_orign.dot(N) + D) / (o_dir.dot(N));
inter_pnt = o_orign + t*o_dir;
}