这里是一种非迭代的解决方案。我担心数学会让你感到烦恼,但这里没有什么复杂的内容。
首先,最好始终使用距离的平方来进行计算。
如果一个三维线由点P和Q描述,另一个由点R和S描述,则表述问题的一种方法是,我们要找到标量m和n,使得第一条线上分数为m的点与第二条线上分数为n的点之间的距离平方为给定的dsq。
我们必须限制m和n在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)),或沿着边缘之一(在 (0,n) 或 (1,n) 处,对于某些 n,或在 (m,0) 或 (m,1) 处,对于某些 m),或在中间的一个点上,在该点处,Dsq 的导数(相对于 m 和 n)均为 0。
例如,请注意在边缘 (0,n) 上我们得到了 Dsq 中 n 的二次项式,因此很容易找到其最大值。
此外,当我们沿着最小值和最大值之间的“线”寻找时,如果将 m(z) 和 n(z) 替换为我们的 Dsq 公式,则经过更多繁琐的代数运算,我们得到 z 的二次函数,因此很容易找到会给出所需 Dsq 值的 z 值。
好了,这篇文章已经相当长了,所以这里是实现这些想法的 C 代码。我随机尝试了一百万个点,当距离始终在最大值和最小值之间时,它总是能找到合适的三维点。在我的(相当普通的)Linux 桌面上,这需要几秒钟。
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];
}
static int quad_min( const double*r, double* x)
{ if ( r[2] <= 0.0)
{ return 0;
}
*x = -r[1]/r[2];
return 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]));
*x = ap/root1;
}
else
{
double root1 = (-r[1] + sqrt( r[1]*r[1] - ap*r[2]));
if ( root1 < r[2])
{ *x = root1/r[2];
}
else
{ *x = ap/root1;
}
}
return 0.0 <= *x && *x <= 1.0;
}
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
;
}
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;
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;
}
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)
{
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
;
}
static int m_edge_min( const quad2T* q, double m0, double* n)
{
double r[3];
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
);
}
static int n_edge_min( const quad2T* q, double* m, double n0)
{
double r[3];
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
);
}
typedef struct
{ double m,n;
double d;
} candT;
static int find_cands( const quad2T* q, candT* c)
{
int nc = 0;
double x, y;
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)};
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)};
}
if ( dq_zero( q, &x, &y))
{ c[nc++] = (candT){ x, y, eval_quad2( q, x, y)};
}
return nc;
}
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);
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;
}
}
if ( c[imax].d < dsq || c[imin].d > dsq)
{ return 0;
}
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))
{
m = c[imin].m + z*(c[imax].m - c[imin].m);
n = c[imin].n + z*(c[imax].n - c[imin].n);
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;
}