求空间两条直线之间的距离

Posted 2022-06-21 枯萎的海风

1. 前言

最近老板让写一段空间点匹配的代码， 其中涉及到求空间两直线之间的距离，写起来满费劲的， 这里做一个记录。

2. 处理思路

空间两直线之间的位置关系主要可以分为： 重合， 平行， 相交， 异面。

2.1 异面情形（含相交）：

已知空间中两线段，如果它们无限变粗，判断是否相交。（主要讨论不在同一平面的情况）线段AB 线段CD
问题的关键是求出这两条任意直线之间的最短距离，以及在这个距离上的两线最接近点坐标，判断该点是否在线段AB和线段CD上。
首先将直线方程化为对称式，得到其方向向量n1=（a1,b1,c1),n2=(a2,b2,c2).

再将两向量叉乘得到其公垂向量N=（x,y,z）,在两直线上分别选取点A,B(任意)，得到向量AB，
求向量AB在向量N方向的投影即为两异面直线间的距离了（就是最短距离啦）。

最短距离的求法：d=|向量N*向量AB|/|向量N|（上面是两向量的数量积，下面是取模）。

设交点为C,D，带入公垂线N的对称式中，又因为C,D两点分别满足一开始的直线方程，所以得到关于C（或D）的两个连等方程，分别解出来就好了！

http://blog.sina.com.cn/s/blog_a401a1ea0101ij9z.html

2.2 平行的情况（含重合）

两平行直线
L1:(x-x1)/m=(y-y1)/n=(z-z1)/p,L2:(x-x2)/m=(y-y2)/n=(z-z2)/p,
记 M1(x1,y1,z1),M2(x2,y2,z2),直线方向向量 s = m,n,p
则 记向量 M1M2 = x2-x1,y2-y1,z2-z1 = a,b,c
故得平行线间的距离
d = | M1M2×s | / |s|
=√[(bp-cn)^2+(cm-ap)^2+(an-bm)^2]/√(m^2+n^2+p^2）

http://www.zybang.com/question/7708426051e596b099898bb1b5d8938d.html

3. 相关代码

getBisByTwoLines.m

%% getDisByTwoLines 用于求解两条直线之间的距离
% @description 利用 pt31 和 pt32 所构成的直线 向 直线1，2 的公垂线做投影
% @param line1 直线1 的法向量 1 x 3
% @param pt31 直线1 上的某个点 1 x 3 或者 1 x 4
% @param line2 直线2 的法向量 1 x 3
% @param pt32 直线2 上的某个点 1 x 3 或者 1 x 4
% @return dis 返回 两直线之间的中垂线的距离
%
function [dis] = getDisByTwoLines(line1, pt31, line2, pt32)
    assert(size(line1, 1) == 1 && size(line1, 2) == 3, 'check the input for line1');
    assert(size(pt31, 1) == 1 && (size(pt31, 2) == 3 || size(pt31, 2) == 4), 'check the input for pt31');
    assert(size(line2, 1) == 1 && size(line2, 2) == 3, 'check the input for line2');
    assert(size(pt32, 1) == 1 && (size(pt32, 2) == 3 || size(pt32, 2) == 4), 'check the input for pt32');

    tmp = pt32 - pt31;
    lineAB = tmp(1:3);

    line = getCoVertialLine(line1, line2);
    if all(line(:) == 0)
        % parallel
        dis = abs(norm(cross(line1, lineAB), 2) / norm(line1, 2));
    else
        % not parallel        
        dis = abs(dot(line, lineAB) / norm(line, 2));
    end
end

getCoVerticalLine.m

%% getCoVertialLine 用于求解两条直线的公垂线的向量部分
% @param line1 直线1 的法向量 1 x 3
% @param line2 直线2 的法向量 1 x 3
% @return line 返回 两直线之间的中垂线的法向量
%

function [line] = getCoVertialLine(line1, line2)
    assert(size(line1, 1) == 1 && size(line1, 2) == 3, 'check the input for line1');
    assert(size(line2, 1) == 1 && size(line2, 2) == 3, 'check the input for line2');

    line = cross(line1, line2);
end

test.m

%% 单元测试
function test()
    Test_getDisByTwoLines()
end

%% 测试两直线之间的距离关系
function Test_getDisByTwoLines()
    line1 = [0, 1, 0];
    line2 = [1, 0, 0];
    pt31 = [0 0 0 1];
    pt32 = [1, 0 ,0 ,1 ];

    %% not parellel and not intersect
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == 0, 'case 1 : something error');

    pt32 = [1 1 1 1];
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == 1, 'case 1 : something error');

    line2 = [0, 1, 1];
    pt32 = [1 1 1 1];
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == 1, 'case 1 : something error');

    %% parellel
    line2 = [0, 1, 0];
    pt32 = [1 1 1 1];
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == sqrt(2), 'case 2 : something error');

    %% intersect
    line2 = [1, 0, 1];
    pt32 = [1 1 1 1];
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == 0, 'case 3 : something error');

    %% collapse
    line2 = [0, 1, 0];
    pt32 = [0 1 0 1];
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == 0, 'case 4 : something error');

    line2 = [0, 1, 0];
    pt32 = [0 0 0 1];
    dis = getDisByTwoLines(line1, pt31, line2, pt32);
    assert(dis == 0, 'case 4 : something error');
end