空间点到空间直线的距离求解

求解步骤

  1. 已知直线上两点,根据空间直线的点向式方程求解

空间点:假设经过直线两点 A ( x 1 , y 1 , z 1 ) A(x1,y1,z1) Ax1y1z1, B ( x 2 , y 2 , z 2 ) B(x2,y2,z2) B(x2y2z2) s ⃗ ( m , n , p ) \vec{s}(m,n,p) s (m,n,p)为空间直线的方向向量,
则直线的方程可表示为:
x − x 1 m = y − y 1 n = z − z 1 p = t (1) \frac{x-x1}{m}=\frac{y-y1}{n}=\frac{z-z1}{p}=t \tag{1} mxx1=nyy1=pzz1=t(1)
示意图如下所示:
空间点到空间直线的距离求解_第1张图片
2. 假设直线外存在一点 C ( x c , y c , z c ) C(xc,yc,zc) C(xc,yc,zc),c点在直线上的垂足坐标为 D ( x d , y d , z d ) D(xd,yd,zd) D(xd,yd,zd)
则 :
x d − x 1 m = y d − y 1 n = z d − z 1 p = t (2) \frac{xd-x1}{m}=\frac{yd-y1}{n}=\frac{zd-z1}{p}=t \tag{2} mxdx1=nydy1=pzdz1=t(2)

{ x d = m t + x 1 y d = n t + y 1 z d = p t + z 1 (3) \left\{\begin{matrix} xd = mt+x1\\ yd = nt+y1\\ zd = pt+z1 \end{matrix}\right.\tag{3} xd=mt+x1yd=nt+y1zd=pt+z1(3)
3 由垂线方向的方向向量 ( x c − x d , y c − y d , z c − z d ) (xc-xd,yc-yd,zc-zd) (xcxd,ycyd,zczd)和直线方向的方向向量 ( m , n , p ) (m,n,p) (m,n,p)的数量积为零,可得
m ( x c − x d ) + n ( y c − y d ) + p ( z c − z d ) = 0 (4) m(xc-xd)+n(yc-yd)+p(zc-zd)=0\tag{4} m(xcxd)+n(ycyd)+p(zczd)=0(4)
( 3 ) , ( 4 ) (3),(4) (3),(4)可得:
t = ( m ∗ ( x c − x 1 ) + n ∗ ( y c − y 1 ) + p ∗ ( z c − z 1 ) ) / ( m 2 + n 2 + p 2 ) (5) t = (m*(xc-x1)+n*(yc-y1)+p*(zc-z1))/(m^{2}+n^{2}+p^{2})\tag{5} t=(m(xcx1)+n(ycy1)+p(zcz1))/(m2+n2+p2)(5)
4求点c到直线的距离
d = ( x c − x d ) 2 + ( y c − y d ) 2 + ( z c − z d ) 2 (6) d =\sqrt{(xc-xd)^{2}+(yc-yd)^{2}+(zc-zd)^{2}}\tag{6} d=(xcxd)2+(ycyd)2+(zczd)2 (6)
( 3 ) , ( 5 ) (3),(5) (3),(5)整合带入 ( 6 ) (6) (6)中即可算出d

你可能感兴趣的:(机器人轨迹规划)