algorithm - 求两条线段之间的距离？

Question

给定两条线段，找到线段之间的距离为d的两个点。

这类似于“两条线段之间的最短距离问题”，只不过我们求解的是线段上相隔给定距离 d 的两点。

每条线段由两个 3 维点组成。

我通过谷歌搜索发现的数学既让我害怕又让我困惑。我是一名程序员，但我很难理解解决此类问题背后的证据和分析。

输入:2 条线段和一个距离d

输出:每段上的 2 个点彼此之间的距离为 d，如果不存在两个点，则为None

Answer 1

这是一个非迭代的解决方案。我担心数学会激怒你，尽管这里没有什么复杂的。

首先，在整个过程中使用平方距离是最简单的。

一条三线由点 P 和 Q 描述，另一条由点 R 和 S 描述，那么说明问题的一种方式是我们想要找到标量 m 和 n 使得点 a 之间的距离平方第一行的分数 m 和第二行的分数 n 是给定的 dsq。

我们必须将 m 和 n 限制在 0 和 1 之间(包括 0 和 1)，这样我们的点才真正在线段上。

如果有m和n，那么点是

X = P + m*(Q-P)
Y = R + n*(S-R)

假设我们首先要找到 Dsq 的最小值和最大值。这将告诉我们是否有解:如果 dsq 的给定值小于最小值或大于最大值，则没有解，我们可以停止。

令出现最小值的点的 m 和 n 值为 m_min 和 n_min，出现最大值的点的 m 和 n 值为 m_max 和 n_max。如果我们引入一个新变量 z(在 [0,1] 中)，那么我们可以考虑 m,n, 值的“线”:

m(z) = m_min + z*(m_max-m_min)
n(z) = n_min + z*(n_max-n_min)

当 z 为 0 时，这些是最小 Dsq 的值，而当 z=1 时，它们是最大 Dsq 的值。所以当我们把z从0增加到1的时候，Dsq的值一定要经过我们想要的值!也就是说，我们只需要搜索使Dsq成为我们想要的值的z值即可。

使问题(相对)简单的原因是 X 和 Y 之间的 distanceSquared 是 m 和 n 的二阶多项式。具体来说，一些繁琐的代数表明，如果 Dsq 是 X 和 Y 之间的距离的平方，

Dsq = a + 2*b*m + 2*c*m + d*m*m + 2*e*m*n + f*m*m
where, in terms of dot products
a = (P-R).(P-R)
b = (P-R).(Q-P)
c =-(P-R).(S-R)
d = (Q-P).(Q-P)
e =-(Q-P).(S-R)
f = (S-R).(S-R)

最大值和最小值必须出现在任一角((m,n)=(0,0) 或 (0,1) 或 (1,0) 或 (1,1))或沿其中一个边缘(对于某些 n，在 (0,n) 或 (1,n)，或对于某些 m，在 (m,0) 或 (m,1))或在中间的一个点，其中 Dsq 的导数(关于到 m 和 n) 都为 0)。请注意，例如，在边缘 say (0,n) 上，我们得到 Dsq 的 n 的二次方，因此很容易找到它的最大值。

此外，当我们沿着最小值和最大值之间的“线”看时，如果我们将 m(z) 和 n(z) 代入 Dsq 的公式，经过更繁琐的代数运算后，我们得到z 的二次方，因此很容易找到将给出 Dsq 所需值的 z 值。

好吧，这篇文章已经很长了，下面是实现这些想法的 C 代码。我为这些点尝试了一百万个随机值，当距离始终在最大值和最小值之间时，它总能找到合适的 3d 点。在我的(相当普通的)Linux 桌面上，这需要几秒钟。

   //   3d vectors
static  void    v3_sub( double* P, double* Q, double* D)
{   D[0] = P[0]-Q[0];
    D[1] = P[1]-Q[1];
    D[2] = P[2]-Q[2];
}
static  double  v3_dot( double* P, double* Q)
{   return P[0]*Q[0] + P[1]*Q[1] + P[2]*Q[2];
}

//  quadratic in one variable
// return *x so X -> r[0] + 2*r[1]*X + r[2]*X*X has minumum at *x
static  int quad_min( const double*r, double* x)
{   if ( r[2] <= 0.0)
    {   return 0;
    }
    *x = -r[1]/r[2];
    return 1;
}

// return x so r[0] + 2*r[1]*x + r[2]*x*x == d, and whether 0<=x<=1
static  int solve_quad( const double* r, double d, double* x)
{
double  ap = r[0] - d;
    if ( r[1] > 0.0)
    {
    double  root1 = -(r[1] + sqrt( r[1]*r[1] - ap*r[2]));   // < 0 
        *x = ap/root1;
    }
    else
    {
    double  root1 = (-r[1] + sqrt( r[1]*r[1] - ap*r[2]));   // >= 0
        if ( root1 < r[2])
        {   *x = root1/r[2];
        }
        else
        {   *x = ap/root1;
        }
    }
    return 0.0 <= *x && *x <= 1.0;
}

//  quadratic in 2 variables
typedef struct
{   double  a,b,c,d,e,f;
}   quad2T;

static  double  eval_quad2( const quad2T* q, double m, double n)
{
    return  q->a
    +   2.0*(m*q->b + n*q->c)
    +   m*m*q->d + 2.0*m*n*q->e + n*n*q->f
    ;
}

// eval coeffs of quad2 so that quad2(m,n) = distsq( P+m*(Q-P), R+n*(S-R))
static  quad2T  set_quad2( double* P, double* Q, double* R, double* S)
{
double  D[3];   v3_sub( P, R, D);
double  U[3];   v3_sub( Q, P, U);
double  V[3];   v3_sub( S, R, V);
quad2T  q;
    // expansion of lengthSq( D+m*U-n*V)
    q.a = v3_dot( D, D);
    q.b = v3_dot( D, U);
    q.c = -v3_dot( D, V);
    q.d = v3_dot( U, U);
    q.e = -v3_dot( U, V);
    q.f = v3_dot( V, V);
    return q;
}

// if gradient of q is 0 in [0,1]x[0,1], return (m,n) where it is zero
// gradient of q is 2*( q->b + m*q->d + n*q->e, q->c + m*q->e + n*q->f)
// so must solve    ( q->d  q->e ) * (m) = -(q->b)
//          ( q->e  q->f )   (n)    (q->c)
static  int dq_zero( const quad2T* q, double* m, double* n)
{
double  det = q->d*q->f - q->e*q->e;
    if ( det <= 0.0)
    {   // note matrix be semi-positive definite, so negative determinant is rounding error
        return 0;
    }
    *m  = -( q->f*q->b - q->e*q->c)/det;
    *n  = -(-q->e*q->b + q->d*q->c)/det;

    return  0.0 <= *m && *m <= 1.0
    &&  0.0 <= *n && *n <= 1.0
    ;
}

// fill *n with minimising value, if any in [0,1], of n -> q(m0,n)
static  int m_edge_min( const quad2T* q, double m0, double* n)
{
double  r[3];   // coeffs of poly in n when m == m0
    r[0] = q->a + 2.0*m0*q->b + m0*m0*q->d;
    r[1] = q->c + m0*q->e;
    r[2] = q->f;
    return  ( quad_min( r, n)
        && *n > 0.0 && *n < 1.0
        );
}

// fill *m with minimising value, if any in [0,1], of m -> q(m,n0)
static  int n_edge_min( const quad2T* q, double* m, double n0)
{
double  r[3];   // coeffs of poly in m when n == n0
    r[0] = q->a + 2.0*n0*q->c + n0*n0*q->f;
    r[1] = q->b + n0*q->e;
    r[2] = q->d;
    return  ( quad_min( r, m)
        && *m > 0.0 && *m < 1.0
        );
}

// candidates for min, man
typedef struct
{   double  m,n;    // steps along lines
    double  d;  // distance squared between points
}   candT;

static  int find_cands( const quad2T* q, candT* c)
{
int nc = 0;
double  x, y;
    // the corners
    c[nc++] = (candT){ 0.0,0.0, eval_quad2( q, 0.0, 0.0)};
    c[nc++] = (candT){ 0.0,1.0, eval_quad2( q, 0.0, 1.0)};
    c[nc++] = (candT){ 1.0,1.0, eval_quad2( q, 1.0, 1.0)};
    c[nc++] = (candT){ 1.0,0.0, eval_quad2( q, 1.0, 0.0)};
    // the edges
    if ( m_edge_min( q, 0.0, &x))
    {   c[nc++] = (candT){ 0.0,x, eval_quad2( q, 0.0, x)};
    }
    if ( m_edge_min( q, 1.0, &x))
    {   c[nc++] = (candT){ 1.0,x, eval_quad2( q, 1.0, x)};
    }
    if ( n_edge_min( q, &x, 0.0))
    {   c[nc++] = (candT){ x, 0.0, eval_quad2( q, x, 0.0)};
    }
    if ( n_edge_min( q, &x, 1.0))
    {   c[nc++] = (candT){ x, 1.0, eval_quad2( q, x, 1.0)};
    }
    // where the derivatives are 0
    if ( dq_zero( q, &x, &y))
    {   c[nc++] = (candT){ x, y, eval_quad2( q, x, y)};
    }
    return nc;
}

// fill in r so that
// r[0] + 2*r[1]*z + r[2]*z*z = q( minm+z*(maxm-minm), minn+x*(maxn-minn))
static  void    form_quad
( const quad2T* q
, double minm, double maxm
, double minn, double maxn
, double* r
)
{
double  a = minm;
double  c = maxm-minm;
double  b = minn;
double  d = maxn-minn;
    r[0] =  q->a + 2.0*q->b*a + 2.0*q->c*b + q->d*a*a + 2.0*q->e*a*b + q->f*b*b;
    r[1] =  q->b*c + q->c*d + q->d*a*c + q->e*(a*d+b*c) + q->f*b*d;
    r[2] =  q->d*c*c + 2.0*q->e*c*d + q->f*d*d;
}

static  int find_points
( double* P, double* Q, double* R, double* S, double dsq, double* X, double* Y
)
{
double  m, n;
quad2T  q = set_quad2( P, Q, R, S);
candT   c[9];
int nc = find_cands( &q, c);    // find candidates for max and min
    // find indices of max and min
int imin = 0;
int imax = 0;
    for( int i=1; i<nc; ++i)
    {   if ( c[i].d < c[imin].d)
        {   imin = i;
        }
        else if ( c[i].d > c[imax].d)
        {   imax = i;
        }
    }
    // check if solution is possible -- should allow some slack here!
    if ( c[imax].d < dsq || c[imin].d > dsq)
    {   return 0;
    }
    // find solution 
double  r[3];
    form_quad( &q, c[imin].m, c[imax].m, c[imin].n, c[imax].n, r);
double  z;
    if ( solve_quad( r, dsq, &z))
    {   // fill in distances along
        m = c[imin].m + z*(c[imax].m - c[imin].m);
        n = c[imin].n + z*(c[imax].n - c[imin].n);
        // compute points
        for( int i=0; i<3; ++i)
        {   X[i] = P[i] + m*(Q[i]-P[i]);
            Y[i] = R[i] + n*(S[i]-R[i]);
        }
        return 1;
    }
    return 0;
}