空间两直线最近的两个点、距离

最新推荐文章于 2023-03-03 15:41:25 发布

蛋_蛋

最新推荐文章于 2023-03-03 15:41:25 发布

阅读量562

点赞数

分类专栏： Unity C# 文章标签： unity 游戏引擎

本文链接：https://blog.csdn.net/qq_33997587/article/details/128329027

版权

Unity 同时被 2 个专栏收录

8 篇文章 0 订阅

订阅专栏

2 篇文章 0 订阅

订阅专栏

已知空间中有直线lineA和直线lineB，求两直线间最近的两点和距离。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class TwoLineMinDis : MonoBehaviour
{
    [SerializeField]
    private Vector3[] _lineA = new Vector3[2];
    [SerializeField]
    private Vector3[] _lineB = new Vector3[2];

    void Update()
    {
        #region 推算过程
        /*
        * 假设_lineA上垂足点为 Pa，距离起点的距离为X，Pa=_lineA[0]+dirA*X；
        * 假设_lineB上垂足点为 Pb，距离起点的距离为Y，Pb=_lineB[0]+dirB*Y；
        * 则垂足向量为dirAB，dirAB=Pb-Pa， dirAB=_lineB[0]+dirB*Y-_lineA[0]-dirA*X;
        * 垂足向量dirAB垂直于_lineA和_lineB
        * dirAB.dirA=0 dirAB.dirB=0
        * (_lineB[0]+dirB*Y-_lineA[0]-dirA*X).dirA=0
        * (_lineB[0]+dirB*Y-_lineA[0]-dirA*X).dirB=0
        * _lineB[0].dirA+dirB.dirA*Y-lineA[0].dirA-dirA.dirA*X=0
        * _lineB[0].dirB+dirB.dirB*Y-lineA[0].dirB-dirA.dirB*X=0
        * X=(_lineB[0].dirA+dirB.dirA*Y-lineA[0].dirA)/dirA.dirA
        * Y=(_lineA[0].dirB+dirA.dirB*X-_lineB[0].dirB)/dirB.dirB;
        * 
        * _lineA和_lineB存在三种情况 1.空间垂直   2.空间平行  3.空间不平行也不垂直
        * 1.空间垂直 则dirA.dirB=0
        *       _lineB[0].dirA+dirB.dirA*Y-lineA[0].dirA-dirA.dirA*X=0
        *       _lineB[0].dirB+dirB.dirB*Y-lineA[0].dirB-dirA.dirB*X=0
        *       _lineB[0].dirA-lineA[0].dirA-dirA.dirA*X=0
        *       _lineB[0].dirB+dirB.dirB*Y-lineA[0].dirB=0
        *       X=(_lineB[0].dirA-_lineA[0].dirA)/dirA.dirA
        *       Y=(_lineA[0].dirB-_lineB[0].dirB)/dirB.dirB;
        * 2.空间平行 则dirA.dirB=1
        *       取_lineA的起点为垂足，则X=0 
        *       _lineB[0].dirB+dirB.dirB*Y-lineA[0].dirB=0
        *       Y=(_lineA[0].dirA-_lineB[0].dirA)/dirB.dirA
        *       Y=(_lineA[0].dirB-_lineB[0].dirB)/dirB.dirB
        *       
        * 3.空间不平行也不垂直
        *       _lineB[0].dirA+dirB.dirA*Y-lineA[0].dirA-dirA.dirA*X=0
        *       _lineB[0].dirB+dirB.dirB*Y-lineA[0].dirB-dirA.dirB*X=0
        *       X=(_lineB[0].dirA+dirB.dirA*Y-lineA[0].dirA)/dirA.dirA
        *       Y=(_lineA[0].dirB+dirA.dirB*X-_lineB[0].dirB)/dirB.dirB;
        *       
        *           dirB.dirB(_lineA[0].dirA - _lineB[0].dirA) + dirA.dirB(_lineB[0].dirB-_lineA[0].dirB)
        *       X=  ———————————————————————————————————————————
        *                                dirA.dirB * dirA.dirB - dirA.dirA * dirB.dirB

        *           dirA.dirA(_lineA[0].dirB - _lineB[0].dirB) + dirA.dirB(_lineB[0].dirA-_lineA[0].dirA)
        *       Y=  ———————————————————————————————————————————
        *                                dirA.dirA * dirB.dirB - dirA.dirB * dirA.dirB
        */
        #endregion

        Vector3 dirLineA = _lineA[1] - _lineA[0];
        Vector3 dirLineB = _lineB[1] - _lineB[0];

        Vector3 posLineA = Vector3.zero;
        Vector3 posLineB = Vector3.zero;

        float dot = Vector3.Dot(dirLineA, dirLineB);
        //两向量空间垂直
        if (dot == 0)
        {
            posLineA = _lineA[0] + dirLineA * (Vector3.Dot(_lineB[0], dirLineA) - Vector3.Dot(_lineA[0], dirLineA)) / Vector3.Dot(dirLineA, dirLineA);
            posLineB = _lineB[0] + dirLineB * (Vector3.Dot(_lineA[0], dirLineB) - Vector3.Dot(_lineB[0], dirLineB)) / Vector3.Dot(dirLineB, dirLineB);
        }
        //两向量空间平行
        else if (dot == 1)
        {
            posLineA = _lineA[0];
            posLineB = _lineB[0] + dirLineB * (Vector3.Dot(_lineA[0], dirLineA) - Vector3.Dot(_lineB[0], dirLineA)) / Vector3.Dot(dirLineB, dirLineA);
        }
        //两向量不平行也不垂直
        else
        {
            posLineA = _lineA[0] + dirLineA * (Vector3.Dot(dirLineB, dirLineB) * (Vector3.Dot(_lineA[0], dirLineA) - Vector3.Dot(dirLineA, _lineB[0])) + Vector3.Dot(dirLineA, dirLineB) * (Vector3.Dot(_lineB[0], dirLineB) - Vector3.Dot(_lineA[0], dirLineB)))
                                       / (Vector3.Dot(dirLineA, dirLineB) * Vector3.Dot(dirLineA, dirLineB) - Vector3.Dot(dirLineA, dirLineA) * Vector3.Dot(dirLineB, dirLineB));
            posLineB = _lineB[0] + dirLineB * (Vector3.Dot(dirLineA, dirLineA) * (Vector3.Dot(_lineA[0], dirLineB) - Vector3.Dot(_lineB[0], dirLineB)) + Vector3.Dot(dirLineA, dirLineB) * (Vector3.Dot(_lineB[0], dirLineA) - Vector3.Dot(_lineA[0], dirLineA)))
                                        / (Vector3.Dot(dirLineA, dirLineA) * Vector3.Dot(dirLineB, dirLineB) - Vector3.Dot(dirLineA, dirLineB) * Vector3.Dot(dirLineA, dirLineB));
        }

        if (Vector3.Dot(posLineA - _lineA[0], dirLineA) < 0)
        {
            posLineA = _lineA[0]; //起点到目标点方向 和 线的方向不一致，则点在线段外面
        }
        else if (Vector3.Dot(posLineA - _lineA[1], dirLineA) > 0)
        {
            posLineA = _lineA[1];//终点到目标点方向 和 线的方向一致，则点在线段外面
        }

        if (Vector3.Dot(posLineB - _lineB[0], dirLineB) < 0)
        {
            posLineB = _lineB[0]; //起点到目标点方向 和 线的方向不一致，则点在线段外面
        }
        else if (Vector3.Dot(posLineB - _lineB[1], dirLineB) > 0)
        {
            posLineB = _lineB[1];//终点到目标点方向 和 线的方向一致，则点在线段外面
        }

        Debug.DrawLine(_lineA[0], _lineA[1], Color.red);
        Debug.DrawLine(_lineB[0], _lineB[1], Color.red);

        Debug.DrawLine(posLineA, posLineB, Color.yellow);

    }
}