unity 四边形多点矫正

Posted 2021-03-24 g993

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

[System.Serializable]
public class fangkuai{
    public Vector3 a;
    public Vector3 b;
    public Vector3 c;
    public Vector3 d;
}

public class xiangliang : MonoBehaviour
{

    public GameObject game;
    


    

    public Vector3 a;
    public Vector3 b;
    public Vector3 c;
    public Vector3 d;
    
    public Vector3 p;

    [Header("设置横竖向校准点个数")]
    public int Width;
    public int Height;
    private int w;
    private int h;
    void Start()
    {
        if (Width <= 2) Width = 2;
        if (Height <= 2) Height = 2;       
         w=Width-1;
         h=Height-1;
        a = sa.position;
        b = sb.position;
        c = sc.position;
        d = sd.position;

        CreateQuadrilateralAnchor();
        CreateCalibrationPoint();


        StartThread();
    }

    [Header("四边形")]
    public Transform sa;
    public Transform sb;
    public Transform sc;
    public Transform sd;
    [Header("需要校准的点")]
    public Transform sp;


    [Header("矫正点个数")]
    private int index = 0;
    [Header("矫正点坐标数组")]
    public List<Vector3> vector3s;
    [Header("四边形数组")]
    public fangkuai[] fangkuais;

    //创建四边形定位点
    void CreateQuadrilateralAnchor() {


        for (float y = 0; y <= h; y++)
        {
            for (float x = 0; x <= w; x++)
            {
                Vector3 v1 = (sb.position - sa.position) * (x / w) + sa.position;
                Vector3 v2 = (sc.position - sd.position) * (x / w) + sd.position;

                Vector3 v3 = (sd.position - sa.position) * (y / h) + sa.position;
                Vector3 v4 = (sc.position - sb.position) * (y / h) + sb.position;
                Vector3 pos = CrossPoint(v1, v2, v3, v4);

                GameObject g = Instantiate(game, pos, Quaternion.identity);

                index++;
                g.name = index.ToString();
                vector3s.Add(pos);
            }
        }
       //设置长度
        fangkuais = new fangkuai[w * h];
        int Sindex = 0;
        for (int y = 0; y < h; y++)
        {
            for (int x = 0; x < w; x++)
            {
                fangkuai fangkuai = new fangkuai();
                int ps = (y * (w + 1)) + x;
                fangkuai.a = vector3s[ps];
                fangkuai.b = vector3s[ps + 1];
                fangkuai.d = vector3s[ps + w + 1];
                fangkuai.c = vector3s[ps + w + 2];
                fangkuais[Sindex] = fangkuai;
                Sindex++;
            }
        }


    }












    [Header("矫正后四边形")]
    public Transform fa;
    public Transform fb;
    public Transform fc;
    public Transform fd;

    [Header("分布点个数")]
    private int findex = 0;
    [Header("矫正后点坐标数组")]
    public List<Vector3> fvector3s;
    [Header("校正后四边形数组")]
    public fangkuai[] ffangkuais;

    [Header("矫正后四边形位置")]
    public Transform Jiao;
    public Vector3 JiaoPos;

    //创建校准点
    void CreateCalibrationPoint() {

        for (float y = 0; y <= h; y++)
        {
            for (float x = 0; x <= w; x++)
            {
                Vector3 v1 = (fb.position - fa.position) * (x / w) + fa.position;
                Vector3 v2 = (fc.position - fd.position) * (x / w) + fd.position;

                Vector3 v3 = (fd.position - fa.position) * (y / h) + fa.position;
                Vector3 v4 = (fc.position - fb.position) * (y / h) + fb.position;
                Vector3 pos = CrossPoint(v1, v2, v3, v4);

                GameObject g = Instantiate(game, pos, Quaternion.identity);

                findex++;
                g.name = "f" + findex.ToString();
                fvector3s.Add(pos);
            }
        }
        //设置长度
        ffangkuais = new fangkuai[w * h];
        int Findex = 0;
        for (int y = 0; y < h; y++)
        {
            for (int x = 0; x < w; x++)
            {
                fangkuai fangkuai = new fangkuai();
                int ps = (y * (w + 1)) + x;
                fangkuai.a = fvector3s[ps];
                fangkuai.b = fvector3s[ps + 1];                
                fangkuai.c = fvector3s[ps + w + 2];
                fangkuai.d = fvector3s[ps + w + 1];
                ffangkuais[Findex] = fangkuai;
                Findex++;
            }
        }


    }







    private Thread thread = null;
    //创建线程
    void StartThread()
    {
        thread = new Thread(F1);
        thread.Start();

    }

    /// <summary>
    /// 线程开启循环
    /// </summary>
    public bool Isthr = true;
    //线程循环
    void F1()
    {
        while (Isthr)
        {
            OnStart();
        }
        
    }


   


   

    
     void Update()
    {
        p = sp.position;   
        Jiao.position = JiaoPos;
    }






    //开始处理校准
    void OnStart() {

        if (IsThePointWithinTheQuadrilateral(p, a, b, c, d))
        {
            Debug.Log("在四边形内");


            for (int i = 0; i < fangkuais.Length; i++)           
            {
                if (IsThePointWithinTheQuadrilateral(p, fangkuais[i].a, fangkuais[i].b, fangkuais[i].c, fangkuais[i].d))
                {
                    Debug.Log("在四边形集合第" + i + "个四边形内");
                    SiBianXing(i, ffangkuais[i].a, ffangkuais[i].b, ffangkuais[i].c, ffangkuais[i].d, p, fangkuais[i].a, fangkuais[i].b, fangkuais[i].c, fangkuais[i].d);
                    return;
                }
                else {
                    //  Debug.Log("不在四边形集合第" + i + "个四边形内");

                    if (IsThePointOnTheInnerLineOfTheSegment(p, fangkuais[i].a, fangkuais[i].b))
                    {
                        Debug.Log("在四边形集合第" + i + "个四边形线段ab边上");
                        SiBianXingBian(i, ffangkuais[i].a, ffangkuais[i].b,p, fangkuais[i].a, fangkuais[i].b);
                        return;
                    }
                    else {
                        if (IsThePointOnTheInnerLineOfTheSegment(p, fangkuais[i].b, fangkuais[i].c))
                        {
                            Debug.Log("在四边形集合第" + i + "个四边形线段bc边上");
                            SiBianXingBian(i, ffangkuais[i].b, ffangkuais[i].c, p, fangkuais[i].b, fangkuais[i].c);
                            return;
                        }
                        else {
                            if (IsThePointOnTheInnerLineOfTheSegment(p, fangkuais[i].c, fangkuais[i].d))
                            {
                                Debug.Log("在四边形集合第" + i + "个四边形线段cd边上");
                                SiBianXingBian(i, ffangkuais[i].c, ffangkuais[i].d, p, fangkuais[i].c, fangkuais[i].d);
                                return;
                            }
                            else {
                                if (IsThePointOnTheInnerLineOfTheSegment(p, fangkuais[i].d, fangkuais[i].a))
                                {
                                    Debug.Log("在四边形集合第" + i + "个四边形线段da边上");
                                    SiBianXingBian(i, ffangkuais[i].d, ffangkuais[i].a, p, fangkuais[i].d, fangkuais[i].a);
                                    return;
                                }
                                else {
                                    Debug.Log("不在四边形集合第" + i + "个四边形内和四条边上");
                                }
                            }


                        }

                    }
                }

            }
        }
        else {

            Debug.Log("不在四边形内");
        }

    }






    //在四边形内
    void SiBianXing(int i, Vector3 fa, Vector3 fb, Vector3 fc, Vector3 fd, Vector3 p,Vector3 a,Vector3 b,Vector3 c,Vector3 d ) {
        Vector3 SJiaoDian = CrossPoint(a, c, b, d);
        Vector3 FJiaoDian = CrossPoint(fa, fc, fb, fd);
        if (SJiaoDian == p)
        {
             Debug.Log("在四边形集合第" + i + "个四边形的焦点上");
            JiaoPos = FJiaoDian;
        }
        else
        {

            if (InTriangleOrNot(p, SJiaoDian, a, b))
            {
                Debug.Log("在四边形集合第" + i + "个四边形的ab侧的三角形内");
                SanJiaoXing(i, FJiaoDian, fa, fb, p, SJiaoDian, a, b);
                return;
            }
            else
            {
                if (InTriangleOrNot(p, SJiaoDian, b, c))
                {

                    Debug.Log("在四边形集合第" + i + "个四边形的bc侧的三角形内");
                    SanJiaoXing(i, FJiaoDian, fb, fc, p, SJiaoDian, b, c);
                    return;

                }
                else
                {
                    if (InTriangleOrNot(p, SJiaoDian, c, d))
                    {

                        Debug.Log("在四边形集合第" + i + "个四边形的cd侧的三角形内");
                        SanJiaoXing(i, FJiaoDian, fc, fd, p, SJiaoDian, c, d);
                        return;

                    }
                    else
                    {
                        if (InTriangleOrNot(p, SJiaoDian, d, a))
                        {

                            Debug.Log("在四边形集合第" + i + "个四边形的da侧的三角形内");
                            SanJiaoXing(i, FJiaoDian, fd, fa, p, SJiaoDian, d, a);
                            return;

                        }
                        else
                        {

                            if (IsThePointOnTheDiagonal(p, a, c))
                            {
                                Debug.Log("在四边形集合第" + i + "个四边形的ac对角线内");
                                DuiJiaoXian(i, FJiaoDian, fa, fc,p,SJiaoDian,a,c);
                                return;
                            }
                            else
                            {
                                if (IsThePointOnTheDiagonal(p, b, d))
                                {
                                    Debug.Log("在四边形集合第" + i + "个四边形的bd对角线内");
                                    DuiJiaoXian(i, FJiaoDian, fb, fd, p, SJiaoDian, b, d);
                                    return;
                                }
                                else
                                {

                                    Debug.Log("不在四边形集合第" + i + "个四边形内");
                                }


                            }



                        }

                    }

                }

            }
        }
    }

    //在三角形内
    void SanJiaoXing(int i, Vector3 fa, Vector3 fb, Vector3 fc, Vector3 p, Vector3 a, Vector3 b ,Vector3 c) {

        float Di1 = Vector3.Distance(p, a);
        Vector2 SJiaoDian = CrossPoint(p,a,b,c);
        float Di2 = Vector3.Distance(SJiaoDian, a);
        float BiLi1 = Di1 / Di2;

        float Di3 = Vector3.Distance(SJiaoDian, b);
        float Di4 = Vector3.Distance(c, b);
        float BiLi2 = Di3 / Di4;
        Vector3 FJiaoDian = (fc - fb) * BiLi2 + fb;       
        Vector3 JiaoZheng = (FJiaoDian-fa) * BiLi1 + fa;
        JiaoPos = JiaoZheng;
        return;
    }

    //在对角线上
    void DuiJiaoXian(int i, Vector3 fj,Vector3 fa,Vector3 fb,Vector3 p,Vector3 sj,Vector3 a,Vector3 b) {

        float Di1 = Vector3.Distance(a,sj );
        float Di2 = Vector3.Distance(p, a);
        float Di3 = Vector3.Distance(p, sj);
        if (Di1 > Di2)
        {
            Debug.Log("在四边形集合第" + i + "个四边形的ac对角线上半段");
           

            float Bili1 = Di3 / Di1;

            Vector3 JiaoZheng = (fa-fj) * Bili1 + fj;
            JiaoPos = JiaoZheng;
            
        }
        else {
            Debug.Log("在四边形集合第" + i + "个四边形的ac对角线下半段");
            float Di4 = Vector3.Distance(sj, b);
            float Bili2 = Di3 / Di4;
            Vector3 JiaoZheng = ( fb- fj ) * Bili2 + fj;
            JiaoPos = JiaoZheng;
        }
        return;
    }

    //在四边形边上
    void SiBianXingBian(int i,  Vector3 fa, Vector3 fb, Vector3 p, Vector3 a, Vector3 b) {

        float Di1 = Vector3.Distance(p, a);
        float Di2 = Vector3.Distance(a, b);
        float BiLi = Di1 / Di2;
        Vector3 JiaoZheng = (fb - fa) * BiLi + fa;
        JiaoPos = JiaoZheng;
        return;
    }

    // 点是否在三角形内
    bool InTriangleOrNot(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
    {
        Vector3 pa = a - p;
        Vector3 pb = b - p;
        Vector3 pc = c - p;
        Vector3 pab = Vector3.Cross(pa, pb);
        Vector3 pbc = Vector3.Cross(pb, pc);
        Vector3 pca = Vector3.Cross(pc, pa);

        float d1 = Vector3.Dot(pab, pbc);
        float d2 = Vector3.Dot(pab, pca);
        float d3 = Vector3.Dot(pbc, pca);

        if (d1 > 0 && d2 > 0 && d3 > 0) return true;
        return false;

    }

    //点是否在对角线上
    bool IsThePointOnTheDiagonal(Vector2 p, Vector2 a, Vector2 b)
    {
        float A = a.y - b.y;
        float B = b.x - a.x;
        float C = (a.x * b.y) - (b.x * a.y);
        if ((A * p.x) + (B * p.y) + C == 0)
        {
            return true;
        }

        return false;
    }

    //点是否在线段内线上
    bool IsThePointOnTheInnerLineOfTheSegment(Vector2 p, Vector2 a, Vector2 b)
    {
        float A = a.y - b.y;
        float B = b.x - a.x;
        float C = (a.x * b.y) - (b.x * a.y);
        if ((A * p.x) + (B * p.y) + C == 0)
        {

            if (Vector3.Distance(p, a) <= Vector3.Distance(a, b) && Vector3.Distance(p, b) <= Vector3.Distance(a, b)) {
                return true;
            }
            
           
        }

        return false;
    }

    // 点是否在四边形内
    bool IsThePointWithinTheQuadrilateral(Vector3 p,Vector3 a,Vector3 b,Vector3 c,Vector3 d)
    {

        Vector3 pa = a - p;
        Vector3 pb = b - p;
        Vector3 pc = c - p;
        Vector3 pd = d - p;
        Vector3 pab = Vector3.Cross(pa, pb);
        Vector3 pbc = Vector3.Cross(pb, pc);
        Vector3 pcd = Vector3.Cross(pc, pd);
        Vector3 pca = Vector3.Cross(pd, pa);

        float d1 = Vector3.Dot(pab, pbc);
        float d2 = Vector3.Dot(pab, pcd);
        float d3 = Vector3.Dot(pab, pca);
        float d4 = Vector3.Dot(pbc, pcd);
        float d5 = Vector3.Dot(pbc, pca);
        float d6 = Vector3.Dot(pcd, pca);
        // float d4 = Vector3.Dot(pbc, pca);
        if (d1 > 0 && d2 > 0 && d3 > 0 && d4 > 0 && d5 > 0 && d6 > 0)
        {
            return true;
        }

        return false;
    }

    // 两条直线焦点
    Vector2 CrossPoint(Vector2 line1, Vector2 line2, Vector2 line3, Vector2 line4) //交点
    {
        float x_member, x_denominator, y_member, y_denominator;
        Vector2 cross_point;
        x_denominator = line4.x * line2.y - line4.x * line1.y - line3.x * line2.y + line3.x * line1.y
            - line2.x * line4.y + line2.x * line3.y + line1.x * line4.y - line1.x * line3.y;

        x_member = line3.y * line4.x * line2.x - line4.y * line3.x * line2.x - line3.y * line4.x * line1.x + line4.y * line3.x * line1.x
            - line1.y * line2.x * line4.x + line2.y * line1.x * line4.x + line1.y * line2.x * line3.x - line2.y * line1.x * line3.x;

        if (x_denominator == 0)
            cross_point.x = 0;
        else
            cross_point.x = x_member / x_denominator;

        y_denominator = line4.y * line2.x - line4.y * line1.x - line3.y * line2.x + line1.x * line3.y
            - line2.y * line4.x + line2.y * line3.x + line1.y * line4.x - line1.y * line3.x;

        y_member = -line3.y * line4.x * line2.y + line4.y * line3.x * line2.y + line3.y * line4.x * line1.y - line4.y * line3.x * line1.y
            + line1.y * line2.x * line4.y - line1.y * line2.x * line3.y - line2.y * line1.x * line4.y + line2.y * line1.x * line3.y;

        if (y_denominator == 0)
            cross_point.y = 0;
        else
            cross_point.y = y_member / y_denominator;

        return cross_point;  //平行返回(0,0)
    }



    void OnDestroy()
    {
        ThreadQuit();
    }
    void ThreadQuit()
    {
        Isthr = false;
        thread.Abort();
     //   thread.Interrupt();
    }
    void OnApplicationQuit()
    {
        ThreadQuit();
    }




}

View Code