Unity3D C#数学系列之判断两条线段是否相交并求交点

Unity3D C#数学系列之判断两条线段是否相交并求交点

2021年3月3日 0 作者 老王

1 引言

问题:已知三维空间中四点A、B、C、D,如何判断线段AB与CD是否相交,若相交则求出交点。
判断AB、CD是否相交并求交点O
分析:

  • AB、CD要相交,则AB、CD必须要在同一平面内
  • 快速排斥和跨立实验判断是否相交
  • 几何法分析求出交点

先来看看效果,紫色小球为交点。
两线段交点效果

2 求解

2.1 AB、CD是否共面与平行

要判断AB、CD是否共面,其实就是判断A、B、C、D四个点是否共面。我们知道三点确定一个平面,如果AB垂直于ACD三点所在平面的法线,则说明A、B、C、D四点共面。
A、C、D三点所在平面的法线怎么求?
很简单,两个向量的叉乘就是这两个向量所在平面的法线。
两个向量的叉乘为这两个向量所在平面的法线
即ACD平面的法线\overrightarrow{ N } = \overrightarrow{ CA }×\overrightarrow{ CD }
然后又如何判断\overrightarrow{ N }是否与\overrightarrow{ AB }垂直呢?
如果\overrightarrow{ N }垂直于\overrightarrow{ AB },则它们的夹角为90°,这时我们可以根据\overrightarrow{ N }\overrightarrow{ AB }的点积或者叉积来判断。
\overrightarrow{ N }\overrightarrow{ AB }
用点积来判断,则有\overrightarrow{ N }·\overrightarrow{ AB } = |\overrightarrow{ N }||\overrightarrow{ AB }| \cos90° = 0;
用叉积来判断,需先把\overrightarrow{ N }\overrightarrow{ AB }化为单位向量,则有|\overrightarrow{ N }·\overrightarrow{ AB }| = |\overrightarrow{ N }||\overrightarrow{ AB }| \sin90° = 1。
这里我们用点积来写代码。

Vector3 ab = b - a;
Vector3 ca = a - c;
Vector3 cd = d - c;

Vector3 v1 = Vector3.Cross(ca, cd);

if (Mathf.Abs(Vector3.Dot(v1, ab)) > 1e-6)
{
    // 不共面
    return false;
}

AB和CD平行时,两者的夹角为0°或180°,也可用点积或叉积来判断,这里就不再多说了。

2.2 快速排斥与跨立实验判断AB、CD是否相交

2.2.1 快速排斥

快速排斥的作用是先预处理一下,先排出掉根本不可能相交的情况,以减少不必要的运算。
其原理就是判断AB的包围盒与CD的包围盒是否有重叠的部分。什么意思呢?看图就明白了。
二维快速排斥
三维的同理。
三维快速排斥
代码怎么写呢?如下。

// 快速排斥
if (Mathf.Min(a.x, b.x) > Mathf.Max(c.x, d.x) || Mathf.Max(a.x, b.x) < Mathf.Min(c.x, d.x)
   || Mathf.Min(a.y, b.y) > Mathf.Max(c.y, d.y) || Mathf.Max(a.y, b.y) < Mathf.Min(c.y, d.y)
   || Mathf.Min(a.z, b.z) > Mathf.Max(c.z, d.z) || Mathf.Max(a.z, b.z) < Mathf.Min(c.z, d.z)
)
    return false;

2.2.2 跨立实验

什么是跨立呢?比如A、B两点分别位于CD的左右两边,我们就说A、B跨立CD。
如果两条线段相交,它们必然是互相跨立的嘛。
怎么判断跨立呢?用向量的叉积。
如图,AB跨立CD的充要条件是\overrightarrow{ CA }×\overrightarrow{ CD } · \overrightarrow{ CD }×\overrightarrow{ CB } > 0。为什么?因为AB跨立CD时,\overrightarrow{ CA }×\overrightarrow{ CD } 所得法线的方向与\overrightarrow{ CD }×\overrightarrow{ CB }所得法线的方向是相同的,所以它们两者的点积>0。
AB跨立CD
叉积结果的方向是通过右手定则来判定的(为什么是右手定则而不是左手,因为叉积的数学定义就是用的右手),比如\overrightarrow{ CA }×\overrightarrow{ CD } ,我们拿出右手,大拇指垂直于CA、CD所在平面,然后食指~小指四指从CA弯向CD,此时大拇指的方向即\overrightarrow{ CA }×\overrightarrow{ CD }的方向(这里是指向屏幕内)。
代码如下:

// 跨立试验
if (Vector3.Dot(Vector3.Cross(-ca, ab), Vector3.Cross(ab, ad)) > 0
    && Vector3.Dot(Vector3.Cross(ca, cd), Vector3.Cross(cd, cb)) > 0)
{
    // 相交
    return true;
}

2.3 计算交点

咱们先写结论。
结论
AO与AB的比值求到了,O点也就能求到了。
结论怎么来的?分析如下:

① 咱们先做条辅助线CE,CE平行于AB,且与AB等长。
做辅助线
② 则有ΔAFO相似于ΔEGC,则有
推导1
③ 则有
推导2
辅助线2
④根据叉积的定义,又有
推导3
⑤于是乎,就有

推导4
⑥我们设\overrightarrow{ v1 }=\overrightarrow{ CA }×\overrightarrow{ CD }\overrightarrow{ v2 }=\overrightarrow{ CD }×\overrightarrow{ AB } ,我们知道\overrightarrow{ v1 }\overrightarrow{ v2 }的夹角α为0°或180°,
\overrightarrow{ v1 }·\overrightarrow{ v2 } = |\overrightarrow{ v1 }|·|\overrightarrow{ v2 }| \cosα= ±|\overrightarrow{ v1 }|·|\overrightarrow{ v2 }|,
就有
推导6
这里我们用点积来计算,主要是为了能够求出AB和CD不直接相交,但AB的延长线和CD相交的交点。比如我们把下面提供的代码中的快速排斥和跨立试验给注释掉,就能求出延长线的交点。
用点乘的目的

3 完整项目

如下。

    /// 
    /// 计算AB与CD两条线段的交点.
    /// 
    /// A点
    /// B点
    /// C点
    /// D点
    /// AB与CD的交点
    /// 是否相交 true:相交 false:未相交
    private bool TryGetIntersectPoint(Vector3 a, Vector3 b, Vector3 c, Vector3 d, out Vector3 intersectPos)
    {
        intersectPos = Vector3.zero;

        Vector3 ab = b - a;
        Vector3 ca = a - c;
        Vector3 cd = d - c;

        Vector3 v1 = Vector3.Cross(ca, cd);

        if (Mathf.Abs(Vector3.Dot(v1, ab)) > 1e-6)
        {
            // 不共面
            return false;
        }

        if (Vector3.Cross(ab, cd).sqrMagnitude <= 1e-6)
        {
            // 平行
            return false;
        }

        Vector3 ad = d - a;
        Vector3 cb = b - c;
        // 快速排斥
        if (Mathf.Min(a.x, b.x) > Mathf.Max(c.x, d.x) || Mathf.Max(a.x, b.x) < Mathf.Min(c.x, d.x)
           || Mathf.Min(a.y, b.y) > Mathf.Max(c.y, d.y) || Mathf.Max(a.y, b.y) < Mathf.Min(c.y, d.y)
           || Mathf.Min(a.z, b.z) > Mathf.Max(c.z, d.z) || Mathf.Max(a.z, b.z) < Mathf.Min(c.z, d.z)
        )
            return false;

        // 跨立试验
        if (Vector3.Dot(Vector3.Cross(-ca, ab), Vector3.Cross(ab, ad)) > 0
            && Vector3.Dot(Vector3.Cross(ca, cd), Vector3.Cross(cd, cb)) > 0)
        {
            Vector3 v2 = Vector3.Cross(cd, ab);
            float ratio = Vector3.Dot(v1, v2) / v2.sqrMagnitude;
            intersectPos = a + ab * ratio;
            return true;
        }

        return false;
    }

项目链接:https://pan.baidu.com/s/1FMi1J9ZuUMnYlz3vH6WETQ
提取码:o3zq

4 参考文章

文中的方法来自这两篇文章,由衷感谢两位博主。