123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413 |
- /////////////////////////////////////////////
- //Copyright (c) 2011, 北京探索者软件公司
- //All rights reserved.
- //文件名称:
- //文件描述:
- //创 建 者: mjy
- //创建日期: 2011-11-02
- //版 本 号:4.0.0.0
- /////////////////////////////////////////////
- using System;
- using System.Collections.Generic;
- namespace SAGA.DotNetUtils.Geometry
- {
- public static class OffsetPolyLine
- {
- public static Geometry2D.PointD[] Offset(Geometry2D.PointD[] ps, double dOffset, int nRotType)
- {
- int nData = ps.Length;
- double[,] polyLine = new double[nData, 2];
- for (int i = 0; i < ps.Length; i++)
- {
- polyLine[i, 0] = ps[i].X;
- polyLine[i, 1] = ps[i].Y;
- }
- bool result = Offset(dOffset, nData, polyLine, nRotType);
- if (!result) return null;
- List<Geometry2D.PointD> listPoints = new List<Geometry2D.PointD>();
- for (int i = 0; i < polyLine.GetLength(0); i++)
- {
- Geometry2D.PointD p = new Geometry2D.PointD();
- p.X = polyLine[i, 0];
- p.Y = polyLine[i, 1];
- listPoints.Add(p);
- }
- return listPoints.ToArray();
- }
- private static bool Offset(double dOffset, int nData, double[,] polyLine, int nRotType)
- {
- List<double> raOffset = new List<double>(nData);
- for (int i = 0; i < nData; i++)
- raOffset.Add(dOffset);
- if (nData < 3) return false;
- double[,] polyLine_Org = new double[nData, 2];
- for (int i = 0; i < nData; i++)
- {
- for (int j = 0; j < 2; j++)
- polyLine_Org[i, j] = polyLine[i, j];
- }
- if (nRotType <= 0)
- {
- nRotType = mathGetRotType(nData, polyLine) ? 1 : 2;
- }
- double[] vector = new double[2];
- double[] cross = new double[2];
- double[] vectorOuter = new double[2];
- double[,] line1 = new double[2, 2];
- double[,] line2 = new double[2, 2];
- double dL_line1, dL_line2;
- int nStrID, nEndID;
- nStrID = nData - 1;
- nEndID = 0;
- vector[0] = polyLine_Org[nEndID, 0] - polyLine_Org[nStrID, 0];
- vector[1] = polyLine_Org[nEndID, 1] - polyLine_Org[nStrID, 1];
- mathNormalize(vector[0], vector[1], ref vector[0], ref vector[1]);
- if (nRotType == 1)
- {
- vectorOuter[0] = vector[1];
- vectorOuter[1] = -1.0*vector[0];
- }
- else
- {
- vectorOuter[0] = -1.0*vector[1];
- vectorOuter[1] = vector[0];
- }
- line1[0, 0] = polyLine_Org[nStrID, 0] + raOffset[nStrID]*vectorOuter[0];
- line1[0, 1] = polyLine_Org[nStrID, 1] + raOffset[nStrID]*vectorOuter[1];
- line1[1, 0] = polyLine_Org[nEndID, 0] + raOffset[nStrID]*vectorOuter[0];
- line1[1, 1] = polyLine_Org[nEndID, 1] + raOffset[nStrID]*vectorOuter[1];
- dL_line1 = mathLength(line1[0, 0], line1[0, 1], line1[1, 0], line1[1, 1]);
- for (int i = 0; i < nData; i++)
- {
- if (i < nData - 1)
- {
- nStrID = i;
- nEndID = i + 1;
- }
- else
- {
- nStrID = i;
- nEndID = 0;
- }
- vector[0] = polyLine_Org[nEndID, 0] - polyLine_Org[nStrID, 0];
- vector[1] = polyLine_Org[nEndID, 1] - polyLine_Org[nStrID, 1];
- mathNormalize(vector[0], vector[1], ref vector[0], ref vector[1]);
- if (nRotType == 1)
- {
- vectorOuter[0] = vector[1];
- vectorOuter[1] = -1.0*vector[0];
- }
- else
- {
- vectorOuter[0] = -1.0*vector[1];
- vectorOuter[1] = vector[0];
- }
- line2[0, 0] = polyLine_Org[nStrID, 0] + raOffset[nStrID]*vectorOuter[0];
- line2[0, 1] = polyLine_Org[nStrID, 1] + raOffset[nStrID]*vectorOuter[1];
- line2[1, 0] = polyLine_Org[nEndID, 0] + raOffset[nStrID]*vectorOuter[0];
- line2[1, 1] = polyLine_Org[nEndID, 1] + raOffset[nStrID]*vectorOuter[1];
- dL_line2 = mathLength(line2[0, 0], line2[0, 1], line2[1, 0], line2[1, 1]);
- if (mathLength(line1[1, 0], line1[1, 1], line2[0, 0], line2[0, 1])
- <= m_NormalZero*(dL_line1 + dL_line2))
- {
- polyLine[i, 0] = line2[0, 0];
- polyLine[i, 1] = line2[0, 1];
- }
- else
- {
- if (mathLineLineCross2D(line1, line2, cross) == 0)
- {
- return false;
- }
- else
- {
- polyLine[i, 0] = cross[0];
- polyLine[i, 1] = cross[1];
- }
- }
- line1[0, 0] = line2[0, 0];
- line1[0, 1] = line2[0, 1];
- line1[1, 0] = line2[1, 0];
- line1[1, 1] = line2[1, 1];
- dL_line1 = dL_line2;
- }
- return true;
- }
- private static bool mathGetRotType(int nData, double[,] polyLine)
- {
- int nRotType = 1;
- int iBegin = 0;
- for (int i = 0; i < nData; i++)
- {
- if (polyLine[i, 0] < polyLine[iBegin, 0] + m_NormalZero)
- {
- iBegin = i;
- }
- }
- double dblX1 = polyLine[iBegin + 0, 0];
- double dblY1 = polyLine[iBegin + 0, 1];
- double dblX2 = polyLine[0, 0];
- double dblY2 = polyLine[0, 1];
- if (iBegin < nData - 1)
- {
- dblX2 = polyLine[iBegin + 1, 0];
- dblY2 = polyLine[iBegin + 1, 1];
- }
- double[] vector = new double[2];
- vector[0] = dblX2 - dblX1;
- vector[1] = dblY2 - dblY1;
- mathNormalize(vector[0], vector[1], ref vector[0], ref vector[1]);
- double[] vectorLeft = new double[2];
- vectorLeft[0] = -1.0*vector[1];
- vectorLeft[1] = vector[0];
- double dDeltaLength = 10.0* /*m_NormalZero*/0.000001*mathLength(dblX2 - dblX1, dblY2 - dblY1);
- double[] dPoint = new double[2];
- dPoint[0] = (dblX2 + dblX1)/2.0 + dDeltaLength*vectorLeft[0];
- dPoint[1] = (dblY2 + dblY1)/2.0 + dDeltaLength*vectorLeft[1];
- bool bInside = mathIsInsidePoint2D(dPoint, nData, polyLine, true);
- if (bInside)
- nRotType = 1;
- else
- nRotType = 2;
- return nRotType == 1 ? true : false;
- }
- public static bool mathIsInsidePoint2D(double[] p1, int nData, double[,] polyLine, bool bIncludeOutLine = true)
- {
- int count = 0;
- int ic = 0;
- double[] p2 = new double[2];
- p2[0] = p1[1] + 10e10;
- p2[1] = p1[1];
- int i;
- for (i = 0; i < nData; i++)
- {
- if (p2[0] < polyLine[i, 0]) p2[0] = polyLine[i, 0] + 100;
- }
- double[] polyLineSub1 = new double[2];
- double[] polyLineSub2 = new double[2];
- for (i = 0; i < nData - 1; i++)
- {
- //ic = mathIntersect_ccw2D(p1, p2, polyLine[i], polyLine[i + 1]);
- polyLineSub1[0] = polyLine[i, 0];
- polyLineSub1[1] = polyLine[i, 0];
- polyLineSub2[0] = polyLine[i + 1, 0];
- polyLineSub2[1] = polyLine[i + 1, 0];
- ic = mathIntersect_ccw2D(p1, p2, polyLineSub1, polyLineSub2);
- if (ic > 0)
- count++;
- else if (ic == 0)
- {
- //if (mathIsPointOfLine2D(polyLine[i], polyLine[i + 1], p1))
- if (mathIsPointOfLine2D(polyLineSub1, polyLineSub2, p1))
- return bIncludeOutLine;
- if (p1[1] == Math.Min(polyLine[i, 1], polyLine[i + 1, 1]) && polyLine[i, 1] != polyLine[i + 1, 1])
- count++;
- }
- }
- polyLineSub1[0] = polyLine[nData - 1, 0];
- polyLineSub1[1] = polyLine[nData - 1, 0];
- polyLineSub2[0] = polyLine[0, 0];
- polyLineSub2[1] = polyLine[0, 0];
- //ic = mathIntersect_ccw2D(p1, p2, polyLine[nData - 1], polyLine[0]);
- ic = mathIntersect_ccw2D(p1, p2, polyLineSub1, polyLineSub2);
- if (ic > 0)
- count++;
- else if (ic == 0)
- {
- //if (mathIsPointOfLine2D(polyLine[nData - 1], polyLine[0], p1))
- if (mathIsPointOfLine2D(polyLineSub1, polyLineSub2, p1))
- return bIncludeOutLine;
- if (p1[1] == Math.Min(polyLine[nData - 1, 1], polyLine[0, 1]) &&
- polyLine[nData - 1, 1] != polyLine[0, 1])
- count++;
- }
- return (count%2) == 1;
- }
- private static bool mathIsPointOfLine2D(double[] bound1, double[] bound2, double[] targetPt,
- bool isOnLine = true)
- {
- double dNormalZero = m_NormalZero*10000.0;
- return mathIsPointOfLine2D(bound1, bound2, targetPt, isOnLine, dNormalZero);
- }
- private static bool mathIsPointOfLine2D(double[] bound1, double[] bound2, double[] targetPt, bool isOnLine,
- double dUserTol)
- {
- double dNormalZero = dUserTol;
- double dTol = Math.Max(mathLength(bound1[0] - bound2[0], bound1[1] - bound2[1]), m_NormalZero);
- if (Math.Abs(targetPt[0] - bound1[0]) < dNormalZero*dTol &&
- Math.Abs(targetPt[1] - bound1[1]) < dNormalZero*dTol)
- return isOnLine;
- if (Math.Abs(targetPt[0] - bound2[0]) < dNormalZero*dTol &&
- Math.Abs(targetPt[1] - bound2[1]) < dNormalZero*dTol)
- return isOnLine;
- double[] vector1 = new double[2];
- vector1[0] = bound1[0] - targetPt[0];
- vector1[1] = bound1[1] - targetPt[1];
- mathNormalize2D(vector1, vector1);
- double[] vector2 = new double[2];
- vector2[0] = bound2[0] - targetPt[0];
- vector2[1] = bound2[1] - targetPt[1];
- mathNormalize2D(vector2, vector2);
- if (Math.Abs(mathCross2D(vector1, vector2)) < dNormalZero*dTol &&
- !(Math.Abs(vector1[0] - vector2[0]) < dNormalZero*dTol &&
- Math.Abs(vector1[1] - vector2[1]) < dNormalZero*dTol))
- return true;
- return false;
- }
- private static int mathIntersect_ccw2D(double[] p1Org, double[] p2Org, double[] p3Org, double[] p4Org)
- {
- double[] p1 = new double[2];
- double[] p2 = new double[2];
- double[] p3 = new double[2];
- double[] p4 = new double[2];
- for (int i = 0; i < 2; i++)
- {
- p1[i] = p1Org[i];
- p2[i] = p2Org[i];
- p3[i] = p3Org[i];
- p4[i] = p4Org[i];
- }
- if (p1[0] > p2[0])
- {
- mathSwap2D(p1, p2);
- }
- if (p3[0] > p4[0])
- {
- mathSwap2D(p3, p4);
- }
- if (p1[1] > p2[1])
- {
- mathSwap2D(p1, p2);
- }
- if (p3[1] > p4[1])
- {
- mathSwap2D(p3, p4);
- }
- int r123 = math_ccw(p1[0], p1[1], p2[0], p2[1], p3[0], p3[1]);
- int r124 = math_ccw(p1[0], p1[1], p2[0], p2[1], p4[0], p4[1]);
- int r341 = math_ccw(p3[0], p3[1], p4[0], p4[1], p1[0], p1[1]);
- int r342 = math_ccw(p3[0], p3[1], p4[0], p4[1], p2[0], p2[1]);
- if (r123*r124 < 0 && r341*r342 < 0)
- return 1;
- if (r123 == 0 && r124 == 0)
- {
- if (!(p3[0] > p2[0] || p1[0] > p4[0]) && !(p3[1] > p2[1] || p1[1] > p4[1]))
- // JSOH (2008.8.29) y绵俊 措茄 厚背侥 眠啊
- return 0;
- else
- return -1;
- }
- if (r123 == 0)
- {
- if (p1[0] <= p3[0] && p3[0] <= p2[0] && p1[1] <= p3[1] && p3[1] <= p2[1]) return 0;
- else return -1;
- }
- if (r124 == 0)
- {
- if (p1[0] <= p4[0] && p4[0] <= p2[0] && p1[1] <= p4[1] && p4[1] <= p2[1]) return 0;
- else return -1;
- }
- if (r341 == 0)
- {
- if (p3[0] <= p1[0] && p1[0] <= p4[0] && p3[1] <= p1[1] && p1[1] <= p4[1]) return 0;
- else return -1;
- }
- if (r342 == 0)
- {
- if (p3[0] <= p2[0] && p2[0] <= p4[0] && p3[1] <= p2[1] && p2[1] <= p4[1]) return 0;
- else return -1;
- }
- return -1;
- }
- private static void mathSwap2D(double[] p1, double[] p2)
- {
- double tx = p1[0];
- double ty = p1[1];
- p1[0] = p2[0];
- p1[1] = p2[1];
- p2[0] = tx;
- p2[1] = ty;
- }
- private static int math_ccw(double ax, double ay, double bx, double by, double cx, double cy)
- {
- double l = bx*cy - ay*bx - ax*cy - by*cx + ax*by + ay*cx;
- if (Math.Abs(l) < 1.0e-10) return 0;
- else if (l > 0.0) return 1;
- else return -1;
- }
- private static double mathLength(double dx, double dy, double dz = 0)
- {
- return Math.Sqrt(dx*dx + dy*dy + dz*dz);
- }
- private static double mathLength(double dxi, double dyi, double dxj, double dyj)
- {
- double dblLength = (dxi - dxj)*(dxi - dxj) + (dyi - dyj)*(dyi - dyj);
- if (dblLength < 0)
- dblLength = 0;
- return Math.Sqrt(dblLength);
- }
- private static readonly double m_NormalZero = 0.0000000000001;
- private static double mathLineLineCross2D(double[,] line1, double[,] line2, double[] cross)
- {
- double[] vector1 = new double[2];
- vector1[0] = line1[0, 0] - line1[1, 0];
- vector1[1] = line1[0, 1] - line1[1, 1];
- mathNormalize2D(vector1, vector1);
- double[] vector2 = new double[2];
- vector2[0] = line2[0, 0] - line2[1, 0];
- vector2[1] = line2[0, 1] - line2[1, 1];
- mathNormalize2D(vector2, vector2);
- double product = mathCross2D(vector1, vector2);
- //if (fabs(product) < m_NormalZero*100000)
- if (Math.Abs(product) < m_NormalZero*100000)
- {
- cross[0] = 0.0;
- cross[1] = 0.0;
- return 0;
- }
- double[] vector3 = new double[2];
- vector3[0] = line1[0, 0] - line2[0, 0];
- vector3[1] = line1[0, 1] - line2[0, 1];
- double H = mathCross2D(vector2, vector3);
- cross[0] = line1[0, 0] + vector1[0]*(H/product);
- cross[1] = line1[0, 1] + vector1[1]*(H/product);
- return 1;
- }
- private static double mathCross2D(double[] vector1, double[] vector2)
- {
- return vector1[0]*vector2[1] - vector1[1]*vector2[0];
- }
- private static bool mathNormalize2D(double[] vector, double[] vectorn)
- {
- return mathNormalize(vector[0], vector[1], ref vectorn[0], ref vectorn[1]);
- }
- private static bool mathNormalize(double dx, double dy, ref double dxn, ref double dyn)
- {
- dxn = 0;
- dyn = 0;
- double Length = mathLength(dx, dy);
- if (Length < m_NormalZero)
- return false;
- dxn = dx/Length;
- dyn = dy/Length;
- return true;
- }
- }
- }
|