lvjincheng_infraredlocategamepc/Assets/o0/o0IdentifyLine.cs

using o0.Geometry;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;
using UnityEngine.UI;

namespace o0.Project
{
    public static partial class Extension
    {
        public static IEnumerable<((float, float) , (float, float), float, float)> lineSeg(MatrixF2D mat, MatrixF2D dir, float angle, (int, int)  start, (int, int)  end, bool reverseXY)
        {
            var graA = (angle + 90) % 180;
            int xDiff = end.Item1 - start.Item1;
            if (xDiff == 0)
                yield break;
            var xToYRate = (float)(end.Item2 - start.Item2) / xDiff;
            Func<float, float> xToYFunc = (x) => (x - start.Item1) * xToYRate + start.Item2;

            var A1 = start.Item1;
            var B1 = A1;
            var B1V = mat[B1, start.Item2];
            var Sum1 = B1V;
            var A2 = start.Item1;
            var B2 = A2;
            var B2V = mat[B2, start.Item2];
            var Sum2 = B2V;
            for (var x = start.Item1 + 1; x <= end.Item1; ++x)
            {
                var y = xToYFunc(x);
                var y1 = Mathf.FloorToInt(y);
                var y2 = Mathf.CeilToInt(y);
                var y0 = y1 - 1;
                var y3 = y2 + 1;
                int C0I, C3I;
                int C1I, C2I;
                if (reverseXY)
                {
                    C0I = mat.Index(y0, x);
                    C1I = mat.Index(y1, x);
                    C2I = mat.Index(y2, x);
                    C3I = mat.Index(y3, x);
                }
                else
                {
                    C0I = mat.Index(x, y0);
                    C1I = mat.Index(x, y1);
                    C2I = mat.Index(x, y2);
                    C3I = mat.Index(x, y3);
                }
                var C0A = dir.Element[C0I];
                var C1A = dir.Element[C1I];
                var C2A = dir.Element[C2I];
                var C3A = dir.Element[C3I];
                var CV = (y2 - y) * mat.Element[C1I] + (y - y1) * mat.Element[C2I];
                //var avaAngle2 = 22.5;
                var avaAngle2 = 50;
                //var avaAngle2 = 33.75;

                /*
                if ((MathF.Abs(C1A - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 360 - dirAngle) <= avaAngle2
                || MathF.Abs(C2A - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 360 - dirAngle) <= avaAngle2)
                && MathF.Abs(BV - CV) / MathF.Min(BV, CV) < 5.0f)
                {
                    B = x;
                    BV = CV;
                    Sum += CV;
                    if (x != end.Item1)
                        continue;
                }/**/
                /*
                if ((MathF.Abs(C0A - dirAngle) <= avaAngle2 || MathF.Abs(C0A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C0A - 360 - dirAngle) <= avaAngle2
                || MathF.Abs(C1A - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 360 - dirAngle) <= avaAngle2
                || MathF.Abs(C2A - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 360 - dirAngle) <= avaAngle2
                || MathF.Abs(C3A - dirAngle) <= avaAngle2 || MathF.Abs(C3A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C3A - 360 - dirAngle) <= avaAngle2)
                && MathF.Abs(BV - CV) / MathF.Min(BV, CV) < 10.0f)
                {
                    B = x;
                    BV = CV;
                    Sum += CV;
                    if (x != end.Item1)
                        continue;
                }/**/

                if (MathF.Abs(C0A - graA) <= avaAngle2 || MathF.Abs(C0A - 360 - graA) <= avaAngle2
                || MathF.Abs(C1A - graA) <= avaAngle2 || MathF.Abs(C1A - 360 - graA) <= avaAngle2
                || MathF.Abs(C2A - graA) <= avaAngle2 || MathF.Abs(C2A - 360 - graA) <= avaAngle2
                || MathF.Abs(C3A - graA) <= avaAngle2 || MathF.Abs(C3A - 360 - graA) <= avaAngle2)
                {
                    B1 = x;
                    //BV = CV;
                    Sum1 += CV;
                    if (x == end.Item1)
                        yield return ((A1, xToYFunc(A1)), (B1, xToYFunc(B1)), Sum1, graA);
                }
                else
                {
                    yield return ((A1, xToYFunc(A1)), (B1, xToYFunc(B1)), Sum1, graA);
                    A1 = x;
                    B1 = x;
                    //BV = CV;
                    Sum1 = CV;
                }/**/
                if (MathF.Abs(C0A - 180 - graA) <= avaAngle2
                || MathF.Abs(C1A - 180 - graA) <= avaAngle2
                || MathF.Abs(C2A - 180 - graA) <= avaAngle2
                || MathF.Abs(C3A - 180 - graA) <= avaAngle2)
                {
                    B2 = x;
                    //BV = CV;
                    Sum2 += CV;
                    if (x == end.Item1)
                        yield return ((A2, xToYFunc(A2)), (B2, xToYFunc(B2)), Sum2, graA + 180);
                }
                else
                {
                    yield return ((A2, xToYFunc(A2)), (B2, xToYFunc(B2)), Sum2, graA + 180);
                    A2 = x;
                    B2 = x;
                    //BV = CV;
                    Sum2 = CV;
                }/**/
            }
        }
        static public MatrixF2D IdentifyLine(this MatrixF2D mat, MatrixF2D dir, Geometry2D.Vector<int> polarSpaceSize = default)
        {
            var edgeCount = mat.Width * mat.Height / 2;
            SortedList<(int, float)> maxGraIndex = new SortedList<(int, float)>();
            for (var i = 0; i < mat.Element.Length; ++i)
            {
                var e = mat.Element[i];
                if (maxGraIndex.Count == 0)
                    maxGraIndex.Add((i, e));
                else if (e.CompareTo(maxGraIndex.First().Item2) > 0)
                {
                    maxGraIndex.Add((i, e));
                    if (maxGraIndex.Count > edgeCount)//最亮的多少个
                        maxGraIndex.RemoveAt(0);
                }
            }

            var polarSpaceMaxSize = new Geometry2D.Vector<int>(180, Mathf.CeilToInt(MathF.Sqrt(mat.Width * mat.Width + mat.Height * mat.Height)));
            if (polarSpaceSize == default)
                polarSpaceSize = new Geometry2D.Vector<int>(180, polarSpaceMaxSize.y / 1);
            //polarSpaceSize = new Geometry2D.Vector<int>(180, 1000);
            var polarSpaceStep = new Geometry2D.Vector<float>((float)polarSpaceMaxSize.x / polarSpaceSize.x, (float)polarSpaceMaxSize.y / polarSpaceSize.y);

            //Debug.Log("size " + polarSpaceMaxSize.y);
            var avaAngle = 22.5;
            //var avaAngle = 45;
            MatrixF2D polarSpace = new MatrixF2D(polarSpaceSize.x, polarSpaceSize.y);
            Matrix2D<List<(int, int)>> sourcePos = new Matrix2D<List<(int, int)>>(polarSpaceSize.x, polarSpaceSize.y);
            Parallel.For(0, polarSpaceSize.x, i =>
            {
                var dirAngle = (polarSpaceStep.x * i + 90) % 180;
                //var px = polarSpaceStep.x * i * Math.PI;
                var px = polarSpaceStep.x * i / 180.0 * Math.PI;
                //var px = polarSpaceStep.x * i;//这里很奇怪
                //Debug.Log("angle "+ (px / Math.PI * 180.0) % 180);
                //var pxInt = Mathf.FloorToInt(px);
                //var cosI = MathF.Cos(i);
                //double cosI = Math.Abs(Math.Cos(px)) / polarSpaceStep.y;
                //double sinI = Math.Sin(px) / polarSpaceStep.y;
                double cosI;
                double sinI;
                cosI = -Math.Cos(px) / polarSpaceStep.y;
                sinI = Math.Sin(px) / polarSpaceStep.y;
                foreach (var j in maxGraIndex)
                {
                    var pointAngle = dir.Element[j.Item1];
                    if (!(MathF.Abs(pointAngle - dirAngle) <= avaAngle || MathF.Abs(pointAngle - 180 - dirAngle) <= avaAngle || MathF.Abs(pointAngle - 360 - dirAngle) <= avaAngle))
                        continue;/**/

                    var (x, y) = mat.Position(j.Item1);
                    //var p = x * cosI + y * sinI;
                    var p = x * sinI + y * cosI;
                    var index = polarSpace.Index(i, Mathf.FloorToInt((float)p));
                    //polarSpace.Element[index] += j.Item2;
                    polarSpace.Element[index] += 1;
                    //polarSpace.Element[index] += 1;
                    if (sourcePos.Element[index] == null)
                        sourcePos.Element[index] = new List<(int, int)> { (x, y) };
                    else
                        sourcePos.Element[index].Add((x, y));
                }
            });
            /*

            var polarSpaceMaxSize = new Geometry2D.Vector<int>(180, Mathf.CeilToInt(MathF.Sqrt(mat.Width * mat.Width + mat.Height * mat.Height)));
            if (polarSpaceSize == default)
                polarSpaceSize = new Geometry2D.Vector<int>(80, 80);
            //polarSpaceSize = new Geometry2D.Vector<int>(180, 1000);
            var polarSpaceStep = new Geometry2D.Vector<float>((float)polarSpaceMaxSize.x / polarSpaceSize.x, (float)polarSpaceMaxSize.y / polarSpaceSize.y);

            Debug.Log("size " + polarSpaceMaxSize.y);

            MatrixF2D polarSpace = new MatrixF2D(polarSpaceSize.x, polarSpaceSize.y);
            Matrix2D<List<(int, int)>> sourcePos = new Matrix2D<List<(int, int)>>(polarSpaceSize.x, polarSpaceSize.y);
            Parallel.For(0, polarSpaceSize.x, i =>
            {
                var px = polarSpaceStep.x * i;
                //var pxInt = Mathf.FloorToInt(px);
                //var cosI = MathF.Cos(i);
                var cosI = MathF.Abs(MathF.Cos(px)) / polarSpaceStep.y;
                var sinI = MathF.Sin(px) / polarSpaceStep.y;
                foreach (var j in maxGraIndex)
                {
                    var (x, y) = mat.Position(j.Item1);
                    var p = x * cosI + y * sinI;
                    var index = polarSpace.Index(i, Mathf.FloorToInt(p));
                    polarSpace.Element[index] += j.Item2;
                    if (sourcePos.Element[index] == null)
                        sourcePos.Element[index] = new List<(int, int)> { (x, y) };
                    else
                        sourcePos.Element[index].Add((x, y));
                }
            });/**/

            var lineCount = polarSpace.Width * polarSpace.Height / 4;
            var minLineSegLength = Math.Min(mat.Height, mat.Width) / 10;
            var minLineLength = Math.Max(2, minLineSegLength / 2);
            //var lineCount = 10000;
            //Debug.Log("lineCount " + lineCount);
            SortedList<(int, float)> maxPolarIndex = new SortedList<(int, float)>();
            for (var i = 0; i < polarSpace.Element.Length; ++i)
            {
                var e = polarSpace.Element[i];
                var posI = sourcePos.Element[i];
                if (posI == null || posI.Count < minLineLength)
                    continue;
                if (maxPolarIndex.Count == 0)
                    maxPolarIndex.Add((i, e));
                else if (e.CompareTo(maxPolarIndex.First().Item2) > 0)
                {
                    maxPolarIndex.Add((i, e));
                    if (maxPolarIndex.Count > lineCount)//最可能存在直线的格子存多少个
                        maxPolarIndex.RemoveAt(0);
                }
            }

            //Debug.Log("polarSpace.Height: " + polarSpace.Height);
            //Debug.Log("polarSpace.Element.Length: " + polarSpace.Element.Length);

            //var lines = new List<Geometry2D.Line<float>>();
            var drawLineMap = new MatrixF2D(mat.Width, mat.Height);
            //var linesCount = 0;

            //var currentCount=0;
            //var wrongCount = 0;
            Matrix2D<(Geometry2D.Line<float>, float)> lineSegMat = new Matrix2D<(Geometry2D.Line<float>, float)>(polarSpaceSize.x, polarSpaceSize.y);
            List<(Geometry2D.Line<float>, float, float)> lineSegList = new List<(Geometry2D.Line<float>, float, float)>();

            //var minLineLength = Math.Min(mat.Width, mat.Height) / 64;
            Parallel.For(0, maxPolarIndex.Count, i =>
            {
                var index = maxPolarIndex[i].Item1;
                //var angle = polarSpace.Position(index).Item1 * polarSpaceStep.x;
                //var angle = ((index % polarSpace.Width) * polarSpaceStep.x + 90) % 180;
                var angle = (index % polarSpace.Width) * polarSpaceStep.x;

                var posI = sourcePos.Element[index];

                if (angle < 45)//0-45度，先x，后y
                    posI.Sort((a, b) => {
                        var c = a.Item1.CompareTo(b.Item1);
                        if (c != 0)
                            return c;
                        return a.Item2.CompareTo(b.Item2);
                    });
                else if (angle > 135)//135-180度，先x，后y
                    posI.Sort((a, b) => {
                        var c = a.Item1.CompareTo(b.Item1);
                        if (c != 0)
                            return c;
                        return a.Item2.CompareTo(b.Item2);
                    });
                else if (angle < 90)//45-90度，先y，后x
                    posI.Sort((a, b) => {
                        var c = a.Item2.CompareTo(b.Item2);
                        if (c != 0)
                            return c;
                        return a.Item1.CompareTo(b.Item1);
                    });
                else//90-135度，先y，后x
                    posI.Sort((a, b) => {
                        var c = a.Item2.CompareTo(b.Item2);
                        if (c != 0)
                            return c;
                        return a.Item1.CompareTo(b.Item1);
                    });




                var Start = posI.First();
                var End = posI.Last();

                if (angle < 45 || angle > 135)//先x
                {
                    foreach (var (A, B, sum, dir) in lineSeg(mat, dir, angle, Start, End, false))
                    {
                        var P1 = new Geometry2D.Vector<float>(A.Item1, A.Item2);
                        var P2 = new Geometry2D.Vector<float>(B.Item1, B.Item2);
                        var line = new Geometry2D.Line<float>(P1, P2);
                        if (line.Length > minLineSegLength)
                            if (sum > lineSegMat.Element[index].Item2)
                            {
                                lineSegMat.Element[index] = (line, sum);
                                //lineSegList.Add((line, sum, dir));
                            }
                        /*
                        lock (drawLineMap)
                        {
                            drawLineMap.DrawLine(line, sum / line.Length);
                            //drawLineMap.DrawLine(line, sum);
                            //Debug.Log("draw " + line.Length);
                        }/**/
                    }
                    /**/
                }
                else//先y
                {
                    foreach (var (A, B, sum, dir) in lineSeg(mat, dir, angle, (Start.Item2, Start.Item1), (End.Item2, End.Item1), true))
                    {
                        var P1 = new Geometry2D.Vector<float>(A.Item2, A.Item1);
                        var P2 = new Geometry2D.Vector<float>(B.Item2, B.Item1);
                        var line = new Geometry2D.Line<float>(P1, P2);
                        if (line.Length > minLineSegLength)
                            if (sum > lineSegMat.Element[index].Item2)
                            {
                                lineSegMat.Element[index] = (line, sum);
                                //lineSegList.Add((line, sum, dir));
                            }
                        /*
                        lock (drawLineMap)
                        {
                            drawLineMap.DrawLine(line, sum / line.Length);
                            //drawLineMap.DrawLine(line, sum);
                            //Debug.Log("draw " + line.Length);
                        }/**/
                    }/**/
                }
            });/**/

            //Debug.Log(lineSegList.Count);
            foreach(var (line, sum) in lineSegMat.Element)
            {
                if (line == default)
                    continue;
                drawLineMap.DrawLine(line, sum / line.Length);
            }/**/
            /*
            var maxIndex = lineSegMat.Element.MaxIndex((a,b)=>a.Item2.CompareTo(b.Item2));
            var maxValue = lineSegMat.Element[maxIndex];
            while (maxValue.Item2 != 0)
            {
                drawLineMap.DrawLine(maxValue.Item1, maxValue.Item2);
                var (x, y) = lineSegMat.Position(maxIndex);
                var center = new Geometry2D.Vector<int>(x, y);
                var halfSize = new Geometry2D.Vector<int>(45, minLineSegLength);
                lineSegMat.DrawRectangle(center - halfSize, center + halfSize, default);

                maxIndex = lineSegMat.Element.MaxIndex((a, b) => a.Item2.CompareTo(b.Item2));
                maxValue = lineSegMat.Element[maxIndex];
            }/**/

            //Debug.Log("wrongCount " + wrongCount);
            //Debug.Log("currentCount " + currentCount);
            //Debug.Log("linesCount " + linesCount);
            //drawLineMap.DrawLine(new Geometry2D.Line<float>(new Geometry2D.Vector<float>(0, 500), new Geometry2D.Vector<float>(500, 500)), 10000);
            /*
            foreach(var i in lines)
            {
                //if (i.A.y > mat.Height || i.B.y > mat.Height)
                //    Debug.Log("123123");
                drawLineMap.DrawLine(i);
            }/**/
            //drawLineMap.DrawLine(new Geometry2D.Line<float>(new Geometry2D.Vector<float>(0,0), new Geometry2D.Vector<float>(100,100)));

            return drawLineMap.Map(0,1,1000);
            //return polarSpace.Map(ignoreExtremum:10);
        }


    }
}