博客
关于我
强烈建议你试试无所不能的chatGPT,快点击我
三维空间两直线/线段最短距离、线段计算算法
阅读量:6144 次
发布时间:2019-06-21

本文共 4646 字,大约阅读时间需要 15 分钟。

设有两空间线段

  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\vec u$$

$$l_t = t_0+c_t\cdot\vec v$$
当$0\le c_s、c_t\le1$时,上述两式表达
设最短距离两点分别为$s_j$、$t_j$,则有
$$s_j = s_0+s_c\cdot\vec u$$
$$t_j = t_0+s_c\cdot\vec v$$
其中$s_c$、$t_c$为$s_j$、$t_j$两点所对应的标量。

记向量$\vec w$=$s_j-t_j$,记向量$\vec w_0$=$s_0-t_0$,根据下图可以得出:

图片描述
$$\vec w=s_0+s_c\cdot\vec u-(t_0+t_c\cdot\vec v)$$ 即:
$$\vec w=\vec w_0+s_c\cdot\vec u-t_c\cdot\vec v\qquad(公式1)$$
如果$s、t$两条直线不平行、重合,则存在唯一的两点$s_c、t_c$使线段$\overrightarrow {s_ct_c}$为$l_s、l_t$最近两点的连线。同时,线段$\overrightarrow {s_ct_c}$也是唯一与两条直线同时垂直的线段。转换为向量表达即为:
$$\vec u\cdot\vec w=0\qquad\vec v\cdot\vec w=0$$
将公式1代入上述两式可得
$$(\vec u\cdot\vec u)s_c-(\vec u\cdot\vec v)t_c=-\vec u\cdot\vec w_0\qquad(公式2)$$
$$(\vec v\cdot\vec u)s_c-(\vec v\cdot\vec v)t_c=-\vec v\cdot\vec w_0\qquad(公式3)$$
记$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$,代入上述方程则可得:
$$\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 u\cdot\vec u\times\vec v\cdot\vec v-(\vec u\cdot\vec v)^2$$
$$\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-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_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;}

参考文献:

转载地址:http://osjya.baihongyu.com/

你可能感兴趣的文章
图解SSH原理及两种登录方法
查看>>
[转载] 七龙珠第一部——第058话 魔境圣地
查看>>
【总结整理】JQuery基础学习---样式篇
查看>>
查询个人站点的文章、分类和标签查询
查看>>
基础知识:数字、字符串、列表 的类型及内置方法
查看>>
JSP的隐式对象
查看>>
JS图片跟着鼠标跑效果
查看>>
[SCOI2005][BZOJ 1084]最大子矩阵
查看>>
学习笔记之Data Visualization
查看>>
Leetcode 3. Longest Substring Without Repeating Characters
查看>>
数学之美系列二十 -- 自然语言处理的教父 马库斯
查看>>
Android实现自定义位置无标题Dialog
查看>>
面试总结
查看>>
Chrome浏览器播放HTML5音频没声音的解决方案
查看>>
easyui datagrid 行编辑功能
查看>>
HDU 2818 (矢量并查集)
查看>>
实验二 Java面向对象程序设计
查看>>
------__________________________9余数定理-__________ 1163______________
查看>>
webapp返回上一页 处理
查看>>
新安装的WAMP中phpmyadmin的密码问题
查看>>