OffsetPolyLine.cs 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413
  1. /////////////////////////////////////////////
  2. //Copyright (c) 2011, 北京探索者软件公司
  3. //All rights reserved.
  4. //文件名称:
  5. //文件描述:
  6. //创 建 者: mjy
  7. //创建日期: 2011-11-02
  8. //版 本 号:4.0.0.0
  9. /////////////////////////////////////////////
  10. using System;
  11. using System.Collections.Generic;
  12. namespace SAGA.DotNetUtils.Geometry
  13. {
  14. public static class OffsetPolyLine
  15. {
  16. public static Geometry2D.PointD[] Offset(Geometry2D.PointD[] ps, double dOffset, int nRotType)
  17. {
  18. int nData = ps.Length;
  19. double[,] polyLine = new double[nData, 2];
  20. for (int i = 0; i < ps.Length; i++)
  21. {
  22. polyLine[i, 0] = ps[i].X;
  23. polyLine[i, 1] = ps[i].Y;
  24. }
  25. bool result = Offset(dOffset, nData, polyLine, nRotType);
  26. if (!result) return null;
  27. List<Geometry2D.PointD> listPoints = new List<Geometry2D.PointD>();
  28. for (int i = 0; i < polyLine.GetLength(0); i++)
  29. {
  30. Geometry2D.PointD p = new Geometry2D.PointD();
  31. p.X = polyLine[i, 0];
  32. p.Y = polyLine[i, 1];
  33. listPoints.Add(p);
  34. }
  35. return listPoints.ToArray();
  36. }
  37. private static bool Offset(double dOffset, int nData, double[,] polyLine, int nRotType)
  38. {
  39. List<double> raOffset = new List<double>(nData);
  40. for (int i = 0; i < nData; i++)
  41. raOffset.Add(dOffset);
  42. if (nData < 3) return false;
  43. double[,] polyLine_Org = new double[nData, 2];
  44. for (int i = 0; i < nData; i++)
  45. {
  46. for (int j = 0; j < 2; j++)
  47. polyLine_Org[i, j] = polyLine[i, j];
  48. }
  49. if (nRotType <= 0)
  50. {
  51. nRotType = mathGetRotType(nData, polyLine) ? 1 : 2;
  52. }
  53. double[] vector = new double[2];
  54. double[] cross = new double[2];
  55. double[] vectorOuter = new double[2];
  56. double[,] line1 = new double[2, 2];
  57. double[,] line2 = new double[2, 2];
  58. double dL_line1, dL_line2;
  59. int nStrID, nEndID;
  60. nStrID = nData - 1;
  61. nEndID = 0;
  62. vector[0] = polyLine_Org[nEndID, 0] - polyLine_Org[nStrID, 0];
  63. vector[1] = polyLine_Org[nEndID, 1] - polyLine_Org[nStrID, 1];
  64. mathNormalize(vector[0], vector[1], ref vector[0], ref vector[1]);
  65. if (nRotType == 1)
  66. {
  67. vectorOuter[0] = vector[1];
  68. vectorOuter[1] = -1.0*vector[0];
  69. }
  70. else
  71. {
  72. vectorOuter[0] = -1.0*vector[1];
  73. vectorOuter[1] = vector[0];
  74. }
  75. line1[0, 0] = polyLine_Org[nStrID, 0] + raOffset[nStrID]*vectorOuter[0];
  76. line1[0, 1] = polyLine_Org[nStrID, 1] + raOffset[nStrID]*vectorOuter[1];
  77. line1[1, 0] = polyLine_Org[nEndID, 0] + raOffset[nStrID]*vectorOuter[0];
  78. line1[1, 1] = polyLine_Org[nEndID, 1] + raOffset[nStrID]*vectorOuter[1];
  79. dL_line1 = mathLength(line1[0, 0], line1[0, 1], line1[1, 0], line1[1, 1]);
  80. for (int i = 0; i < nData; i++)
  81. {
  82. if (i < nData - 1)
  83. {
  84. nStrID = i;
  85. nEndID = i + 1;
  86. }
  87. else
  88. {
  89. nStrID = i;
  90. nEndID = 0;
  91. }
  92. vector[0] = polyLine_Org[nEndID, 0] - polyLine_Org[nStrID, 0];
  93. vector[1] = polyLine_Org[nEndID, 1] - polyLine_Org[nStrID, 1];
  94. mathNormalize(vector[0], vector[1], ref vector[0], ref vector[1]);
  95. if (nRotType == 1)
  96. {
  97. vectorOuter[0] = vector[1];
  98. vectorOuter[1] = -1.0*vector[0];
  99. }
  100. else
  101. {
  102. vectorOuter[0] = -1.0*vector[1];
  103. vectorOuter[1] = vector[0];
  104. }
  105. line2[0, 0] = polyLine_Org[nStrID, 0] + raOffset[nStrID]*vectorOuter[0];
  106. line2[0, 1] = polyLine_Org[nStrID, 1] + raOffset[nStrID]*vectorOuter[1];
  107. line2[1, 0] = polyLine_Org[nEndID, 0] + raOffset[nStrID]*vectorOuter[0];
  108. line2[1, 1] = polyLine_Org[nEndID, 1] + raOffset[nStrID]*vectorOuter[1];
  109. dL_line2 = mathLength(line2[0, 0], line2[0, 1], line2[1, 0], line2[1, 1]);
  110. if (mathLength(line1[1, 0], line1[1, 1], line2[0, 0], line2[0, 1])
  111. <= m_NormalZero*(dL_line1 + dL_line2))
  112. {
  113. polyLine[i, 0] = line2[0, 0];
  114. polyLine[i, 1] = line2[0, 1];
  115. }
  116. else
  117. {
  118. if (mathLineLineCross2D(line1, line2, cross) == 0)
  119. {
  120. return false;
  121. }
  122. else
  123. {
  124. polyLine[i, 0] = cross[0];
  125. polyLine[i, 1] = cross[1];
  126. }
  127. }
  128. line1[0, 0] = line2[0, 0];
  129. line1[0, 1] = line2[0, 1];
  130. line1[1, 0] = line2[1, 0];
  131. line1[1, 1] = line2[1, 1];
  132. dL_line1 = dL_line2;
  133. }
  134. return true;
  135. }
  136. private static bool mathGetRotType(int nData, double[,] polyLine)
  137. {
  138. int nRotType = 1;
  139. int iBegin = 0;
  140. for (int i = 0; i < nData; i++)
  141. {
  142. if (polyLine[i, 0] < polyLine[iBegin, 0] + m_NormalZero)
  143. {
  144. iBegin = i;
  145. }
  146. }
  147. double dblX1 = polyLine[iBegin + 0, 0];
  148. double dblY1 = polyLine[iBegin + 0, 1];
  149. double dblX2 = polyLine[0, 0];
  150. double dblY2 = polyLine[0, 1];
  151. if (iBegin < nData - 1)
  152. {
  153. dblX2 = polyLine[iBegin + 1, 0];
  154. dblY2 = polyLine[iBegin + 1, 1];
  155. }
  156. double[] vector = new double[2];
  157. vector[0] = dblX2 - dblX1;
  158. vector[1] = dblY2 - dblY1;
  159. mathNormalize(vector[0], vector[1], ref vector[0], ref vector[1]);
  160. double[] vectorLeft = new double[2];
  161. vectorLeft[0] = -1.0*vector[1];
  162. vectorLeft[1] = vector[0];
  163. double dDeltaLength = 10.0* /*m_NormalZero*/0.000001*mathLength(dblX2 - dblX1, dblY2 - dblY1);
  164. double[] dPoint = new double[2];
  165. dPoint[0] = (dblX2 + dblX1)/2.0 + dDeltaLength*vectorLeft[0];
  166. dPoint[1] = (dblY2 + dblY1)/2.0 + dDeltaLength*vectorLeft[1];
  167. bool bInside = mathIsInsidePoint2D(dPoint, nData, polyLine, true);
  168. if (bInside)
  169. nRotType = 1;
  170. else
  171. nRotType = 2;
  172. return nRotType == 1 ? true : false;
  173. }
  174. public static bool mathIsInsidePoint2D(double[] p1, int nData, double[,] polyLine, bool bIncludeOutLine = true)
  175. {
  176. int count = 0;
  177. int ic = 0;
  178. double[] p2 = new double[2];
  179. p2[0] = p1[1] + 10e10;
  180. p2[1] = p1[1];
  181. int i;
  182. for (i = 0; i < nData; i++)
  183. {
  184. if (p2[0] < polyLine[i, 0]) p2[0] = polyLine[i, 0] + 100;
  185. }
  186. double[] polyLineSub1 = new double[2];
  187. double[] polyLineSub2 = new double[2];
  188. for (i = 0; i < nData - 1; i++)
  189. {
  190. //ic = mathIntersect_ccw2D(p1, p2, polyLine[i], polyLine[i + 1]);
  191. polyLineSub1[0] = polyLine[i, 0];
  192. polyLineSub1[1] = polyLine[i, 0];
  193. polyLineSub2[0] = polyLine[i + 1, 0];
  194. polyLineSub2[1] = polyLine[i + 1, 0];
  195. ic = mathIntersect_ccw2D(p1, p2, polyLineSub1, polyLineSub2);
  196. if (ic > 0)
  197. count++;
  198. else if (ic == 0)
  199. {
  200. //if (mathIsPointOfLine2D(polyLine[i], polyLine[i + 1], p1))
  201. if (mathIsPointOfLine2D(polyLineSub1, polyLineSub2, p1))
  202. return bIncludeOutLine;
  203. if (p1[1] == Math.Min(polyLine[i, 1], polyLine[i + 1, 1]) && polyLine[i, 1] != polyLine[i + 1, 1])
  204. count++;
  205. }
  206. }
  207. polyLineSub1[0] = polyLine[nData - 1, 0];
  208. polyLineSub1[1] = polyLine[nData - 1, 0];
  209. polyLineSub2[0] = polyLine[0, 0];
  210. polyLineSub2[1] = polyLine[0, 0];
  211. //ic = mathIntersect_ccw2D(p1, p2, polyLine[nData - 1], polyLine[0]);
  212. ic = mathIntersect_ccw2D(p1, p2, polyLineSub1, polyLineSub2);
  213. if (ic > 0)
  214. count++;
  215. else if (ic == 0)
  216. {
  217. //if (mathIsPointOfLine2D(polyLine[nData - 1], polyLine[0], p1))
  218. if (mathIsPointOfLine2D(polyLineSub1, polyLineSub2, p1))
  219. return bIncludeOutLine;
  220. if (p1[1] == Math.Min(polyLine[nData - 1, 1], polyLine[0, 1]) &&
  221. polyLine[nData - 1, 1] != polyLine[0, 1])
  222. count++;
  223. }
  224. return (count%2) == 1;
  225. }
  226. private static bool mathIsPointOfLine2D(double[] bound1, double[] bound2, double[] targetPt,
  227. bool isOnLine = true)
  228. {
  229. double dNormalZero = m_NormalZero*10000.0;
  230. return mathIsPointOfLine2D(bound1, bound2, targetPt, isOnLine, dNormalZero);
  231. }
  232. private static bool mathIsPointOfLine2D(double[] bound1, double[] bound2, double[] targetPt, bool isOnLine,
  233. double dUserTol)
  234. {
  235. double dNormalZero = dUserTol;
  236. double dTol = Math.Max(mathLength(bound1[0] - bound2[0], bound1[1] - bound2[1]), m_NormalZero);
  237. if (Math.Abs(targetPt[0] - bound1[0]) < dNormalZero*dTol &&
  238. Math.Abs(targetPt[1] - bound1[1]) < dNormalZero*dTol)
  239. return isOnLine;
  240. if (Math.Abs(targetPt[0] - bound2[0]) < dNormalZero*dTol &&
  241. Math.Abs(targetPt[1] - bound2[1]) < dNormalZero*dTol)
  242. return isOnLine;
  243. double[] vector1 = new double[2];
  244. vector1[0] = bound1[0] - targetPt[0];
  245. vector1[1] = bound1[1] - targetPt[1];
  246. mathNormalize2D(vector1, vector1);
  247. double[] vector2 = new double[2];
  248. vector2[0] = bound2[0] - targetPt[0];
  249. vector2[1] = bound2[1] - targetPt[1];
  250. mathNormalize2D(vector2, vector2);
  251. if (Math.Abs(mathCross2D(vector1, vector2)) < dNormalZero*dTol &&
  252. !(Math.Abs(vector1[0] - vector2[0]) < dNormalZero*dTol &&
  253. Math.Abs(vector1[1] - vector2[1]) < dNormalZero*dTol))
  254. return true;
  255. return false;
  256. }
  257. private static int mathIntersect_ccw2D(double[] p1Org, double[] p2Org, double[] p3Org, double[] p4Org)
  258. {
  259. double[] p1 = new double[2];
  260. double[] p2 = new double[2];
  261. double[] p3 = new double[2];
  262. double[] p4 = new double[2];
  263. for (int i = 0; i < 2; i++)
  264. {
  265. p1[i] = p1Org[i];
  266. p2[i] = p2Org[i];
  267. p3[i] = p3Org[i];
  268. p4[i] = p4Org[i];
  269. }
  270. if (p1[0] > p2[0])
  271. {
  272. mathSwap2D(p1, p2);
  273. }
  274. if (p3[0] > p4[0])
  275. {
  276. mathSwap2D(p3, p4);
  277. }
  278. if (p1[1] > p2[1])
  279. {
  280. mathSwap2D(p1, p2);
  281. }
  282. if (p3[1] > p4[1])
  283. {
  284. mathSwap2D(p3, p4);
  285. }
  286. int r123 = math_ccw(p1[0], p1[1], p2[0], p2[1], p3[0], p3[1]);
  287. int r124 = math_ccw(p1[0], p1[1], p2[0], p2[1], p4[0], p4[1]);
  288. int r341 = math_ccw(p3[0], p3[1], p4[0], p4[1], p1[0], p1[1]);
  289. int r342 = math_ccw(p3[0], p3[1], p4[0], p4[1], p2[0], p2[1]);
  290. if (r123*r124 < 0 && r341*r342 < 0)
  291. return 1;
  292. if (r123 == 0 && r124 == 0)
  293. {
  294. if (!(p3[0] > p2[0] || p1[0] > p4[0]) && !(p3[1] > p2[1] || p1[1] > p4[1]))
  295. // JSOH (2008.8.29) y绵俊 措茄 厚背侥 眠啊
  296. return 0;
  297. else
  298. return -1;
  299. }
  300. if (r123 == 0)
  301. {
  302. if (p1[0] <= p3[0] && p3[0] <= p2[0] && p1[1] <= p3[1] && p3[1] <= p2[1]) return 0;
  303. else return -1;
  304. }
  305. if (r124 == 0)
  306. {
  307. if (p1[0] <= p4[0] && p4[0] <= p2[0] && p1[1] <= p4[1] && p4[1] <= p2[1]) return 0;
  308. else return -1;
  309. }
  310. if (r341 == 0)
  311. {
  312. if (p3[0] <= p1[0] && p1[0] <= p4[0] && p3[1] <= p1[1] && p1[1] <= p4[1]) return 0;
  313. else return -1;
  314. }
  315. if (r342 == 0)
  316. {
  317. if (p3[0] <= p2[0] && p2[0] <= p4[0] && p3[1] <= p2[1] && p2[1] <= p4[1]) return 0;
  318. else return -1;
  319. }
  320. return -1;
  321. }
  322. private static void mathSwap2D(double[] p1, double[] p2)
  323. {
  324. double tx = p1[0];
  325. double ty = p1[1];
  326. p1[0] = p2[0];
  327. p1[1] = p2[1];
  328. p2[0] = tx;
  329. p2[1] = ty;
  330. }
  331. private static int math_ccw(double ax, double ay, double bx, double by, double cx, double cy)
  332. {
  333. double l = bx*cy - ay*bx - ax*cy - by*cx + ax*by + ay*cx;
  334. if (Math.Abs(l) < 1.0e-10) return 0;
  335. else if (l > 0.0) return 1;
  336. else return -1;
  337. }
  338. private static double mathLength(double dx, double dy, double dz = 0)
  339. {
  340. return Math.Sqrt(dx*dx + dy*dy + dz*dz);
  341. }
  342. private static double mathLength(double dxi, double dyi, double dxj, double dyj)
  343. {
  344. double dblLength = (dxi - dxj)*(dxi - dxj) + (dyi - dyj)*(dyi - dyj);
  345. if (dblLength < 0)
  346. dblLength = 0;
  347. return Math.Sqrt(dblLength);
  348. }
  349. private static readonly double m_NormalZero = 0.0000000000001;
  350. private static double mathLineLineCross2D(double[,] line1, double[,] line2, double[] cross)
  351. {
  352. double[] vector1 = new double[2];
  353. vector1[0] = line1[0, 0] - line1[1, 0];
  354. vector1[1] = line1[0, 1] - line1[1, 1];
  355. mathNormalize2D(vector1, vector1);
  356. double[] vector2 = new double[2];
  357. vector2[0] = line2[0, 0] - line2[1, 0];
  358. vector2[1] = line2[0, 1] - line2[1, 1];
  359. mathNormalize2D(vector2, vector2);
  360. double product = mathCross2D(vector1, vector2);
  361. //if (fabs(product) < m_NormalZero*100000)
  362. if (Math.Abs(product) < m_NormalZero*100000)
  363. {
  364. cross[0] = 0.0;
  365. cross[1] = 0.0;
  366. return 0;
  367. }
  368. double[] vector3 = new double[2];
  369. vector3[0] = line1[0, 0] - line2[0, 0];
  370. vector3[1] = line1[0, 1] - line2[0, 1];
  371. double H = mathCross2D(vector2, vector3);
  372. cross[0] = line1[0, 0] + vector1[0]*(H/product);
  373. cross[1] = line1[0, 1] + vector1[1]*(H/product);
  374. return 1;
  375. }
  376. private static double mathCross2D(double[] vector1, double[] vector2)
  377. {
  378. return vector1[0]*vector2[1] - vector1[1]*vector2[0];
  379. }
  380. private static bool mathNormalize2D(double[] vector, double[] vectorn)
  381. {
  382. return mathNormalize(vector[0], vector[1], ref vectorn[0], ref vectorn[1]);
  383. }
  384. private static bool mathNormalize(double dx, double dy, ref double dxn, ref double dyn)
  385. {
  386. dxn = 0;
  387. dyn = 0;
  388. double Length = mathLength(dx, dy);
  389. if (Length < m_NormalZero)
  390. return false;
  391. dxn = dx/Length;
  392. dyn = dy/Length;
  393. return true;
  394. }
  395. }
  396. }