三维空间两直线/线段最短距离、线段计算算法

Segmentfault编辑器里对公式的现实暂时不正确，可以参考这里：
设有两空间线段

1. $L_s$，其起点、终点坐标为$ s_0、s_1 $，方向向量$vec u = s_1-s_0 $

2. $L_t$，其起点、终点坐标为$ t_0、t_1 $，方向向量$vec v = t_1-t_0 $
   记两线段对应的直线为$l_s、l_t$，采用向量表示法如下：

$$l_s = s_0+c_scdotvec u$$
$$l_t = t_0+c_tcdotvec v$$
当$0le c_s、c_tle1$时，上述两式表达
设最短距离两点分别为$s_j$、$t_j$，则有
$$s_j = s_0+s_ccdotvec u$$
$$t_j = t_0+s_ccdotvec v$$
其中$s_j$、$t_j$为$s_j$、$t_j$两点所对应的标量。

记向量$vec w$=$s_c-t_c$，记向量$vec w_0$=$s_0-t_0$，根据下图可以得出：

$$vec w=s_0+s_ccdotvec u-(t_0+t_ccdotvec v)$$ 即：
$$vec w=vec w_0+s_ccdotvec u-t_ccdotvec vqquad(公式1)$$
如果$s、t$两条直线不平行、重合，则存在唯一的两点$s_c、t_c$使线段$overrightarrow {s_ct_c}$为$l_s、l_t$最近两点的连线。同时，线段$overrightarrow {s_ct_c}$也是唯一与两条直线同时垂直的线段。转换为向量表达即为：
$$vec ucdotvec w=0qquadvec vcdotvec w=0$$
将公式1代入上述两式可得
$$（vec ucdotvec u）s_c-（vec ucdotvec v）t_c=-vec ucdotvec w_0qquad(公式2)$$
$$（vec vcdotvec u）s_c-（vec vcdotvec v）t_c=-vec vcdotvec w_0qquad(公式3)$$
记$a=vec ucdotvec u，b=vec ucdotvec v，c=vec vcdotvec v，d=vec ucdotvec w_0，e=vec vcdotvec w_0$，代入上述方程则可得：
$$qquad s_c = frac {be-cd}{ac-b^2}qquad(公式4)$$
$$qquad t_c = frac {ae-bd}{ac-b^2}qquad(公式5)$$
注意到上式中分母：
$$ac-b^2=vec ucdotvec utimesvec vcdotvec v-（vec ucdotvec v）^2$$
$$Rightarrow ac-b^2=vertvec uvert^2cdotvertvec vvert^2-(vertvec uvertcdotvertvec vvertcdot cosq)^2=(vertvec uvertcdotvertvec vvertcdot sinq)^2ge0$$
当$ac-b^2=0$时，公式2和公式3相互独立，则两直线平行，直线间的距离为一常数，我们可以在任意一条直线上指定一固定点，然后代入公式求距离。我们可以指定$s_c=0$然后可以求得$t_c=frac {d}{b}=frac{e}{c}$。
当求出$s_c、t_c$我们即可求得$s_j、t_j$两点，则两点之间的距离可按下式求解：
$$d(l_s,l_t)=vert s_j-t_jvert=vert s_0-t_0+frac {(be-cd)vec u- (ae-bd)vec v}{ac-b^2}vert$$

具体实现代码如下（C# 实现）：

public bool IsEqual(double d1, double d2)
{
if (Math.Abs(d1 - d2) sd)
{
//最近点在s终点以外(即sc>1,则取sc=1)
sn = sd;
tn = e + b; //按(公式3)计算
td = c;
}
}
if (tn a) //按(公式2)计算，如果sc大于1，取sc=1
sn = sd;
else
{
sn = -d;
sd = a;
}
}
else if (tn > td)
{
tn = td;
if ((-d + b) a)
sn = sd;
else
{
sn = (-d + b);
sd = a;
}
}

double sc = 0.0;double tc = 0.0;if (IsEqual(sn, 0.0))    sc = 0.0;else    sc = sn / sd;if (IsEqual(tn, 0.0))    tc = 0.0;else    tc = tn / td;double dx = wx + (sc * ux) - (tc * vx);double dy = wy + (sc * uy) - (tc * vy);double dz = wz + (sc * uz) - (tc * vz);return dx * dx + dy * dy + dz * dz;

}
参考文献：http://geomalgorithms.com/a07-_distance.html

关键字：算法, 空间计算, 几何, #c++#

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！