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

最新推荐文章于 2024-07-19 13:59:11 发布

Albert-C

最新推荐文章于 2024-07-19 13:59:11 发布

阅读量2.6w

点赞数 15

分类专栏：算法几何空间直线文章标签：三维算法几何

本文链接：https://blog.csdn.net/u011511587/article/details/52063663

版权

算法同时被 3 个专栏收录

1 篇文章 0 订阅

订阅专栏

几何

1 篇文章 0 订阅

订阅专栏

空间直线

1 篇文章 0 订阅

订阅专栏

设有两空间线段
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 s \cdot u ⃗

$l_s = s_0+c_s\cdot\vec u$

l t = t 0 + c t \cdot v ⃗

$l_t = t_0+c_t\cdot\vec v$
当

0≤cs、ct≤1 $0\le c_s、c_t\le1$ 时，上述两式表达
设最短距离两点分别为

sj $s_j$ 、

tj $t_j$ ，则有

s j = s 0 + s c \cdot u ⃗

$s_j = s_0+s_c\cdot\vec u$

t j = t 0 + s c \cdot v ⃗

$t_j = t_0+s_c\cdot\vec v$
其中

sc $s_c$ 、

tc $t_c$ 为

sj $s_j$ 、

tj $t_j$ 两点所对应的标量。

记向量 $\vec w$ = $s_j-t_j$ ，记向量 $\vec w_0$ = $s_0-t_0$ ，根据下图可以得出：
向量示意图

w ⃗ = s 0 + s c \cdot u ⃗ - (t 0 + t c \cdot v ⃗)

$\vec w=s_0+s_c\cdot\vec u-(t_0+t_c\cdot\vec v)$ 即：

w ⃗ = w ⃗ 0 + s c \cdot u ⃗ - t c \cdot v ⃗ (公 式 1)

$\vec w=\vec w_0+s_c\cdot\vec u-t_c\cdot\vec v\qquad(公式1)$
如果

s、t $s、t$ 两条直线不平行、重合，则存在唯一的两点

sc、tc $s_c、t_c$ 使线段

sctc−→− $\overrightarrow {s_ct_c}$ 为

ls、lt $l_s、l_t$ 最近两点的连线。同时，线段

sctc−→− $\overrightarrow {s_ct_c}$ 也是唯一与两条直线同时垂直的线段。转换为向量表达即为：

u ⃗ \cdot w ⃗ = 0 v ⃗ \cdot w ⃗ = 0

$\vec u\cdot\vec w=0\qquad\vec v\cdot\vec w=0$
将公式1代入上述两式可得

（ u ⃗ \cdot u ⃗ ） s c - （ u ⃗ \cdot v ⃗ ） t c = - u ⃗ \cdot w ⃗ 0 (公 式 2)

$（\vec u\cdot\vec u）s_c-（\vec u\cdot\vec v）t_c=-\vec u\cdot\vec w_0\qquad(公式2)$

（ v ⃗ \cdot u ⃗ ） s c - （ v ⃗ \cdot v ⃗ ） t c = - v ⃗ \cdot w ⃗ 0 (公 式 3)

$（\vec v\cdot\vec u）s_c-（\vec v\cdot\vec v）t_c=-\vec v\cdot\vec w_0\qquad(公式3)$
记

a=u⃗ ⋅u⃗ ，b=u⃗ ⋅v⃗ ，c=v⃗ ⋅v⃗ ，d=u⃗ ⋅w⃗ 0，e=v⃗ ⋅w⃗ 0 $a=\vec u\cdot\vec u，b=\vec u\cdot\vec v，c=\vec v\cdot\vec v，d=\vec u\cdot\vec w_0，e=\vec v\cdot\vec w_0$ ，代入上述方程则可得：

s c = b e - c d a c - b 2 (公 式 4)

$\qquad s_c = \frac {be-cd}{ac-b^2}\qquad(公式4)$

t c = a e - b d a c - b 2 (公 式 5)

$\qquad t_c = \frac {ae-bd}{ac-b^2}\qquad(公式5)$
注意到上式中分母：

a c - b 2 = u ⃗ \cdot u ⃗ \times v ⃗ \cdot v ⃗ - （ u ⃗ \cdot v ⃗ ） 2

$ac-b^2=\vec u\cdot\vec u\times\vec v\cdot\vec v-（\vec u\cdot\vec v）^2$

\Rightarrow a c - b 2 = | u ⃗ | 2 \cdot | v ⃗ | 2 - (| u ⃗ | \cdot | v ⃗ | \cdot c o s q) 2 = (| u ⃗ | \cdot | v ⃗ | \cdot s i n q) 2 \geq 0

$\Rightarrow ac-b^2=\vert\vec u\vert^2\cdot\vert\vec v\vert^2-(\vert\vec u\vert\cdot\vert\vec v\vert\cdot cosq)^2=(\vert\vec u\vert\cdot\vert\vec v\vert\cdot sinq)^2\ge0$
当

ac−b2=0 $ac-b^2=0$ 时，公式2和公式3相互独立，则两直线平行，直线间的距离为一常数，我们可以在任意一条直线上指定一固定点，然后代入公式求距离。我们可以指定

sc=0 $s_c=0$ 然后可以求得

tc=db=ec $t_c=\frac {d}{b}=\frac{e}{c}$ 。
当求出

sc、tc $s_c、t_c$ 我们即可求得

sj、tj $s_j、t_j$ 两点，则两点之间的距离可按下式求解：

d (l s, l t) = | s j - t j | = | s 0 - t 0 + ( b e - c d ) u ⃗ - ( a e - b d ) v ⃗ a c - b 2 |

$d(l_s,l_t)=\vert s_j-t_j\vert=\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) < 1e-7)
        return true;
    return false;
}

public double SqureDistanceSegmentToSegment(double x1, double y1, double z1,
                                            double x2, double y2, double z2,
                                            double x3, double y3, double z3,
                                            double x4, double y4, double z4)
{
    // 解析几何通用解法，可以求出点的位置，判断点是否在线段上
    // 算法描述：设两条无限长度直线s、t,起点为s0、t0，方向向量为u、v
    // 最短直线两点：在s1上为s0+sc*u，在t上的为t0+tc*v
    // 记向量w为(s0+sc*u)-(t0+tc*v),记向量w0=s0-t0
    // 记a=u*u，b=u*v，c=v*v，d=u*w0，e=v*w0——(a)；
    // 由于u*w=、v*w=0，将w=-tc*v+w0+sc*u带入前两式得：
    // (u*u)*sc - (u*v)*tc = -u*w0  (公式2)
    // (v*u)*sc - (v*v)*tc = -v*w0  (公式3)
    // 再将前式(a)带入可得sc=(be-cd)/(ac-b2)、tc=(ae-bd)/(ac-b2)——（b）
    // 注意到ac-b2=|u|2|v|2-(|u||v|cosq)2=(|u||v|sinq)2不小于0
    // 所以可以根据公式（b）判断sc、tc符号和sc、tc与1的关系即可分辨最近点是否在线段内
    // 当ac-b2=0时，(公式2)(公式3)独立，表示两条直线平行。可令sc=0单独解出tc
    // 最终距离d（L1、L2）=|（P0-Q0)+[(be-cd)*u-(ae-bd)v]/(ac-b2)|
    double ux = x2 - x1;
    double uy = y2 - y1;
    double uz = z2 - z1;

    double vx = x4 - x3;
    double vy = y4 - y3;
    double vz = z4 - z3;

    double wx = x1 - x3;
    double wy = y1 - y3;
    double wz = z1 - z3;

    double a = (ux * ux + uy * uy + uz * uz); //u*u
    double b = (ux * vx + uy * vy + uz * vz); //u*v
    double c = (vx * vx + vy * vy + vz * vz); //v*v
    double d = (ux * wx + uy * wy + uz * wz); //u*w 
    double e = (vx * wx + vy * wy + vz * wz); //v*w
    double dt = a * c - b * b;

    double sd = dt;
    double td = dt;

    double sn = 0.0;//sn = be-cd
    double tn = 0.0;//tn = ae-bd

    if (IsEqual(dt, 0.0))
    {
        //两直线平行
        sn = 0.0;    //在s上指定取s0
        sd = 1.00;   //防止计算时除0错误

        tn = e;      //按(公式3)求tc
        td = c;
    }
    else
    {
        sn = (b * e - c * d);
        tn = (a * e - b * d);
        if (sn < 0.0)
        {
            //最近点在s起点以外，同平行条件
            sn = 0.0;
            tn = e;
            td = c;
        }
        else if (sn > sd)
        {
            //最近点在s终点以外(即sc>1,则取sc=1)
            sn = sd;
            tn = e + b; //按(公式3)计算
            td = c;
        }
    }
    if (tn < 0.0)
    {
        //最近点在t起点以外
        tn = 0.0;
        if (-d < 0.0) //按(公式2)计算，如果等号右边小于0，则sc也小于零，取sc=0
            sn = 0.0;
        else if (-d > a) //按(公式2)计算，如果sc大于1，取sc=1
            sn = sd;
        else
        {
            sn = -d;
            sd = a;
        }
    }
    else if (tn > td)
    {
        tn = td;
        if ((-d + b) < 0.0)
            sn = 0.0;
        else 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;
}