o0IdentifyLine.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389
  1. using o0.Geometry;
  2. using System;
  3. using System.Collections.Generic;
  4. using System.Linq;
  5. using System.Text;
  6. using System.Threading.Tasks;
  7. using UnityEngine;
  8. using UnityEngine.UI;
  9. namespace o0.Project
  10. {
  11. public static partial class Extension
  12. {
  13. public static IEnumerable<((float, float) , (float, float), float, float)> lineSeg(MatrixF2D mat, MatrixF2D dir, float angle, (int, int) start, (int, int) end, bool reverseXY)
  14. {
  15. var graA = (angle + 90) % 180;
  16. int xDiff = end.Item1 - start.Item1;
  17. if (xDiff == 0)
  18. yield break;
  19. var xToYRate = (float)(end.Item2 - start.Item2) / xDiff;
  20. Func<float, float> xToYFunc = (x) => (x - start.Item1) * xToYRate + start.Item2;
  21. var A1 = start.Item1;
  22. var B1 = A1;
  23. var B1V = mat[B1, start.Item2];
  24. var Sum1 = B1V;
  25. var A2 = start.Item1;
  26. var B2 = A2;
  27. var B2V = mat[B2, start.Item2];
  28. var Sum2 = B2V;
  29. for (var x = start.Item1 + 1; x <= end.Item1; ++x)
  30. {
  31. var y = xToYFunc(x);
  32. var y1 = Mathf.FloorToInt(y);
  33. var y2 = Mathf.CeilToInt(y);
  34. var y0 = y1 - 1;
  35. var y3 = y2 + 1;
  36. int C0I, C3I;
  37. int C1I, C2I;
  38. if (reverseXY)
  39. {
  40. C0I = mat.Index(y0, x);
  41. C1I = mat.Index(y1, x);
  42. C2I = mat.Index(y2, x);
  43. C3I = mat.Index(y3, x);
  44. }
  45. else
  46. {
  47. C0I = mat.Index(x, y0);
  48. C1I = mat.Index(x, y1);
  49. C2I = mat.Index(x, y2);
  50. C3I = mat.Index(x, y3);
  51. }
  52. var C0A = dir.Element[C0I];
  53. var C1A = dir.Element[C1I];
  54. var C2A = dir.Element[C2I];
  55. var C3A = dir.Element[C3I];
  56. var CV = (y2 - y) * mat.Element[C1I] + (y - y1) * mat.Element[C2I];
  57. //var avaAngle2 = 22.5;
  58. var avaAngle2 = 50;
  59. //var avaAngle2 = 33.75;
  60. /*
  61. if ((MathF.Abs(C1A - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 360 - dirAngle) <= avaAngle2
  62. || MathF.Abs(C2A - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 360 - dirAngle) <= avaAngle2)
  63. && MathF.Abs(BV - CV) / MathF.Min(BV, CV) < 5.0f)
  64. {
  65. B = x;
  66. BV = CV;
  67. Sum += CV;
  68. if (x != end.Item1)
  69. continue;
  70. }/**/
  71. /*
  72. if ((MathF.Abs(C0A - dirAngle) <= avaAngle2 || MathF.Abs(C0A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C0A - 360 - dirAngle) <= avaAngle2
  73. || MathF.Abs(C1A - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C1A - 360 - dirAngle) <= avaAngle2
  74. || MathF.Abs(C2A - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C2A - 360 - dirAngle) <= avaAngle2
  75. || MathF.Abs(C3A - dirAngle) <= avaAngle2 || MathF.Abs(C3A - 180 - dirAngle) <= avaAngle2 || MathF.Abs(C3A - 360 - dirAngle) <= avaAngle2)
  76. && MathF.Abs(BV - CV) / MathF.Min(BV, CV) < 10.0f)
  77. {
  78. B = x;
  79. BV = CV;
  80. Sum += CV;
  81. if (x != end.Item1)
  82. continue;
  83. }/**/
  84. if (MathF.Abs(C0A - graA) <= avaAngle2 || MathF.Abs(C0A - 360 - graA) <= avaAngle2
  85. || MathF.Abs(C1A - graA) <= avaAngle2 || MathF.Abs(C1A - 360 - graA) <= avaAngle2
  86. || MathF.Abs(C2A - graA) <= avaAngle2 || MathF.Abs(C2A - 360 - graA) <= avaAngle2
  87. || MathF.Abs(C3A - graA) <= avaAngle2 || MathF.Abs(C3A - 360 - graA) <= avaAngle2)
  88. {
  89. B1 = x;
  90. //BV = CV;
  91. Sum1 += CV;
  92. if (x == end.Item1)
  93. yield return ((A1, xToYFunc(A1)), (B1, xToYFunc(B1)), Sum1, graA);
  94. }
  95. else
  96. {
  97. yield return ((A1, xToYFunc(A1)), (B1, xToYFunc(B1)), Sum1, graA);
  98. A1 = x;
  99. B1 = x;
  100. //BV = CV;
  101. Sum1 = CV;
  102. }/**/
  103. if (MathF.Abs(C0A - 180 - graA) <= avaAngle2
  104. || MathF.Abs(C1A - 180 - graA) <= avaAngle2
  105. || MathF.Abs(C2A - 180 - graA) <= avaAngle2
  106. || MathF.Abs(C3A - 180 - graA) <= avaAngle2)
  107. {
  108. B2 = x;
  109. //BV = CV;
  110. Sum2 += CV;
  111. if (x == end.Item1)
  112. yield return ((A2, xToYFunc(A2)), (B2, xToYFunc(B2)), Sum2, graA + 180);
  113. }
  114. else
  115. {
  116. yield return ((A2, xToYFunc(A2)), (B2, xToYFunc(B2)), Sum2, graA + 180);
  117. A2 = x;
  118. B2 = x;
  119. //BV = CV;
  120. Sum2 = CV;
  121. }/**/
  122. }
  123. }
  124. static public MatrixF2D IdentifyLine(this MatrixF2D mat, MatrixF2D dir, Geometry2D.Vector<int> polarSpaceSize = default)
  125. {
  126. var edgeCount = mat.Width * mat.Height / 2;
  127. SortedList<(int, float)> maxGraIndex = new SortedList<(int, float)>();
  128. for (var i = 0; i < mat.Element.Length; ++i)
  129. {
  130. var e = mat.Element[i];
  131. if (maxGraIndex.Count == 0)
  132. maxGraIndex.Add((i, e));
  133. else if (e.CompareTo(maxGraIndex.First().Item2) > 0)
  134. {
  135. maxGraIndex.Add((i, e));
  136. if (maxGraIndex.Count > edgeCount)//最亮的多少个
  137. maxGraIndex.RemoveAt(0);
  138. }
  139. }
  140. var polarSpaceMaxSize = new Geometry2D.Vector<int>(180, Mathf.CeilToInt(MathF.Sqrt(mat.Width * mat.Width + mat.Height * mat.Height)));
  141. if (polarSpaceSize == default)
  142. polarSpaceSize = new Geometry2D.Vector<int>(180, polarSpaceMaxSize.y / 1);
  143. //polarSpaceSize = new Geometry2D.Vector<int>(180, 1000);
  144. var polarSpaceStep = new Geometry2D.Vector<float>((float)polarSpaceMaxSize.x / polarSpaceSize.x, (float)polarSpaceMaxSize.y / polarSpaceSize.y);
  145. //Debug.Log("size " + polarSpaceMaxSize.y);
  146. var avaAngle = 22.5;
  147. //var avaAngle = 45;
  148. MatrixF2D polarSpace = new MatrixF2D(polarSpaceSize.x, polarSpaceSize.y);
  149. Matrix2D<List<(int, int)>> sourcePos = new Matrix2D<List<(int, int)>>(polarSpaceSize.x, polarSpaceSize.y);
  150. Parallel.For(0, polarSpaceSize.x, i =>
  151. {
  152. var dirAngle = (polarSpaceStep.x * i + 90) % 180;
  153. //var px = polarSpaceStep.x * i * Math.PI;
  154. var px = polarSpaceStep.x * i / 180.0 * Math.PI;
  155. //var px = polarSpaceStep.x * i;//这里很奇怪
  156. //Debug.Log("angle "+ (px / Math.PI * 180.0) % 180);
  157. //var pxInt = Mathf.FloorToInt(px);
  158. //var cosI = MathF.Cos(i);
  159. //double cosI = Math.Abs(Math.Cos(px)) / polarSpaceStep.y;
  160. //double sinI = Math.Sin(px) / polarSpaceStep.y;
  161. double cosI;
  162. double sinI;
  163. cosI = -Math.Cos(px) / polarSpaceStep.y;
  164. sinI = Math.Sin(px) / polarSpaceStep.y;
  165. foreach (var j in maxGraIndex)
  166. {
  167. var pointAngle = dir.Element[j.Item1];
  168. if (!(MathF.Abs(pointAngle - dirAngle) <= avaAngle || MathF.Abs(pointAngle - 180 - dirAngle) <= avaAngle || MathF.Abs(pointAngle - 360 - dirAngle) <= avaAngle))
  169. continue;/**/
  170. var (x, y) = mat.Position(j.Item1);
  171. //var p = x * cosI + y * sinI;
  172. var p = x * sinI + y * cosI;
  173. var index = polarSpace.Index(i, Mathf.FloorToInt((float)p));
  174. //polarSpace.Element[index] += j.Item2;
  175. polarSpace.Element[index] += 1;
  176. //polarSpace.Element[index] += 1;
  177. if (sourcePos.Element[index] == null)
  178. sourcePos.Element[index] = new List<(int, int)> { (x, y) };
  179. else
  180. sourcePos.Element[index].Add((x, y));
  181. }
  182. });
  183. /*
  184. var polarSpaceMaxSize = new Geometry2D.Vector<int>(180, Mathf.CeilToInt(MathF.Sqrt(mat.Width * mat.Width + mat.Height * mat.Height)));
  185. if (polarSpaceSize == default)
  186. polarSpaceSize = new Geometry2D.Vector<int>(80, 80);
  187. //polarSpaceSize = new Geometry2D.Vector<int>(180, 1000);
  188. var polarSpaceStep = new Geometry2D.Vector<float>((float)polarSpaceMaxSize.x / polarSpaceSize.x, (float)polarSpaceMaxSize.y / polarSpaceSize.y);
  189. Debug.Log("size " + polarSpaceMaxSize.y);
  190. MatrixF2D polarSpace = new MatrixF2D(polarSpaceSize.x, polarSpaceSize.y);
  191. Matrix2D<List<(int, int)>> sourcePos = new Matrix2D<List<(int, int)>>(polarSpaceSize.x, polarSpaceSize.y);
  192. Parallel.For(0, polarSpaceSize.x, i =>
  193. {
  194. var px = polarSpaceStep.x * i;
  195. //var pxInt = Mathf.FloorToInt(px);
  196. //var cosI = MathF.Cos(i);
  197. var cosI = MathF.Abs(MathF.Cos(px)) / polarSpaceStep.y;
  198. var sinI = MathF.Sin(px) / polarSpaceStep.y;
  199. foreach (var j in maxGraIndex)
  200. {
  201. var (x, y) = mat.Position(j.Item1);
  202. var p = x * cosI + y * sinI;
  203. var index = polarSpace.Index(i, Mathf.FloorToInt(p));
  204. polarSpace.Element[index] += j.Item2;
  205. if (sourcePos.Element[index] == null)
  206. sourcePos.Element[index] = new List<(int, int)> { (x, y) };
  207. else
  208. sourcePos.Element[index].Add((x, y));
  209. }
  210. });/**/
  211. var lineCount = polarSpace.Width * polarSpace.Height / 4;
  212. var minLineSegLength = Math.Min(mat.Height, mat.Width) / 10;
  213. var minLineLength = Math.Max(2, minLineSegLength / 2);
  214. //var lineCount = 10000;
  215. //Debug.Log("lineCount " + lineCount);
  216. SortedList<(int, float)> maxPolarIndex = new SortedList<(int, float)>();
  217. for (var i = 0; i < polarSpace.Element.Length; ++i)
  218. {
  219. var e = polarSpace.Element[i];
  220. var posI = sourcePos.Element[i];
  221. if (posI == null || posI.Count < minLineLength)
  222. continue;
  223. if (maxPolarIndex.Count == 0)
  224. maxPolarIndex.Add((i, e));
  225. else if (e.CompareTo(maxPolarIndex.First().Item2) > 0)
  226. {
  227. maxPolarIndex.Add((i, e));
  228. if (maxPolarIndex.Count > lineCount)//最可能存在直线的格子存多少个
  229. maxPolarIndex.RemoveAt(0);
  230. }
  231. }
  232. //Debug.Log("polarSpace.Height: " + polarSpace.Height);
  233. //Debug.Log("polarSpace.Element.Length: " + polarSpace.Element.Length);
  234. //var lines = new List<Geometry2D.Line<float>>();
  235. var drawLineMap = new MatrixF2D(mat.Width, mat.Height);
  236. //var linesCount = 0;
  237. //var currentCount=0;
  238. //var wrongCount = 0;
  239. Matrix2D<(Geometry2D.Line<float>, float)> lineSegMat = new Matrix2D<(Geometry2D.Line<float>, float)>(polarSpaceSize.x, polarSpaceSize.y);
  240. List<(Geometry2D.Line<float>, float, float)> lineSegList = new List<(Geometry2D.Line<float>, float, float)>();
  241. //var minLineLength = Math.Min(mat.Width, mat.Height) / 64;
  242. Parallel.For(0, maxPolarIndex.Count, i =>
  243. {
  244. var index = maxPolarIndex[i].Item1;
  245. //var angle = polarSpace.Position(index).Item1 * polarSpaceStep.x;
  246. //var angle = ((index % polarSpace.Width) * polarSpaceStep.x + 90) % 180;
  247. var angle = (index % polarSpace.Width) * polarSpaceStep.x;
  248. var posI = sourcePos.Element[index];
  249. if (angle < 45)//0-45度,先x,后y
  250. posI.Sort((a, b) => {
  251. var c = a.Item1.CompareTo(b.Item1);
  252. if (c != 0)
  253. return c;
  254. return a.Item2.CompareTo(b.Item2);
  255. });
  256. else if (angle > 135)//135-180度,先x,后y
  257. posI.Sort((a, b) => {
  258. var c = a.Item1.CompareTo(b.Item1);
  259. if (c != 0)
  260. return c;
  261. return a.Item2.CompareTo(b.Item2);
  262. });
  263. else if (angle < 90)//45-90度,先y,后x
  264. posI.Sort((a, b) => {
  265. var c = a.Item2.CompareTo(b.Item2);
  266. if (c != 0)
  267. return c;
  268. return a.Item1.CompareTo(b.Item1);
  269. });
  270. else//90-135度,先y,后x
  271. posI.Sort((a, b) => {
  272. var c = a.Item2.CompareTo(b.Item2);
  273. if (c != 0)
  274. return c;
  275. return a.Item1.CompareTo(b.Item1);
  276. });
  277. var Start = posI.First();
  278. var End = posI.Last();
  279. if (angle < 45 || angle > 135)//先x
  280. {
  281. foreach (var (A, B, sum, dir) in lineSeg(mat, dir, angle, Start, End, false))
  282. {
  283. var P1 = new Geometry2D.Vector<float>(A.Item1, A.Item2);
  284. var P2 = new Geometry2D.Vector<float>(B.Item1, B.Item2);
  285. var line = new Geometry2D.Line<float>(P1, P2);
  286. if (line.Length > minLineSegLength)
  287. if (sum > lineSegMat.Element[index].Item2)
  288. {
  289. lineSegMat.Element[index] = (line, sum);
  290. //lineSegList.Add((line, sum, dir));
  291. }
  292. /*
  293. lock (drawLineMap)
  294. {
  295. drawLineMap.DrawLine(line, sum / line.Length);
  296. //drawLineMap.DrawLine(line, sum);
  297. //Debug.Log("draw " + line.Length);
  298. }/**/
  299. }
  300. /**/
  301. }
  302. else//先y
  303. {
  304. foreach (var (A, B, sum, dir) in lineSeg(mat, dir, angle, (Start.Item2, Start.Item1), (End.Item2, End.Item1), true))
  305. {
  306. var P1 = new Geometry2D.Vector<float>(A.Item2, A.Item1);
  307. var P2 = new Geometry2D.Vector<float>(B.Item2, B.Item1);
  308. var line = new Geometry2D.Line<float>(P1, P2);
  309. if (line.Length > minLineSegLength)
  310. if (sum > lineSegMat.Element[index].Item2)
  311. {
  312. lineSegMat.Element[index] = (line, sum);
  313. //lineSegList.Add((line, sum, dir));
  314. }
  315. /*
  316. lock (drawLineMap)
  317. {
  318. drawLineMap.DrawLine(line, sum / line.Length);
  319. //drawLineMap.DrawLine(line, sum);
  320. //Debug.Log("draw " + line.Length);
  321. }/**/
  322. }/**/
  323. }
  324. });/**/
  325. //Debug.Log(lineSegList.Count);
  326. foreach(var (line, sum) in lineSegMat.Element)
  327. {
  328. if (line == default)
  329. continue;
  330. drawLineMap.DrawLine(line, sum / line.Length);
  331. }/**/
  332. /*
  333. var maxIndex = lineSegMat.Element.MaxIndex((a,b)=>a.Item2.CompareTo(b.Item2));
  334. var maxValue = lineSegMat.Element[maxIndex];
  335. while (maxValue.Item2 != 0)
  336. {
  337. drawLineMap.DrawLine(maxValue.Item1, maxValue.Item2);
  338. var (x, y) = lineSegMat.Position(maxIndex);
  339. var center = new Geometry2D.Vector<int>(x, y);
  340. var halfSize = new Geometry2D.Vector<int>(45, minLineSegLength);
  341. lineSegMat.DrawRectangle(center - halfSize, center + halfSize, default);
  342. maxIndex = lineSegMat.Element.MaxIndex((a, b) => a.Item2.CompareTo(b.Item2));
  343. maxValue = lineSegMat.Element[maxIndex];
  344. }/**/
  345. //Debug.Log("wrongCount " + wrongCount);
  346. //Debug.Log("currentCount " + currentCount);
  347. //Debug.Log("linesCount " + linesCount);
  348. //drawLineMap.DrawLine(new Geometry2D.Line<float>(new Geometry2D.Vector<float>(0, 500), new Geometry2D.Vector<float>(500, 500)), 10000);
  349. /*
  350. foreach(var i in lines)
  351. {
  352. //if (i.A.y > mat.Height || i.B.y > mat.Height)
  353. // Debug.Log("123123");
  354. drawLineMap.DrawLine(i);
  355. }/**/
  356. //drawLineMap.DrawLine(new Geometry2D.Line<float>(new Geometry2D.Vector<float>(0,0), new Geometry2D.Vector<float>(100,100)));
  357. return drawLineMap.Map(0,1,1000);
  358. //return polarSpace.Map(ignoreExtremum:10);
  359. }
  360. }
  361. }