凸多边形Minkowski求和算法

using System;
using System.Collections;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Web;

namespace RT.MvcWeb.Utility
{
    public class MinkowskiSum
    {
        public class Point2D
        {
            public string name { get; set; }
            public double x { get; set; }
            public double y { get; set; }
            public double deg { get; set; }

            public Point2D()
            {
                name = string.Empty;
                x = 0;
                y = 0;
                deg = 0;
            }

            public Point2D(string strName)
            {
                name = strName;
                x = 0;
                y = 0;
                deg = 0;
            }

            public Point2D(string strName, double dX, double dY)
            {
                name = strName;
                x = dX;
                y = dY;
                deg = 0;
            }

            public Point2D(string strName, double dX, double dY, double dDeg)
            {
                name = strName;
                x = dX;
                y = dY;
                deg = dDeg;
            }

            public bool Equal(Point2D b)
            {
                return (x == b.x && y == b.y);
            }

            public void Offset(double dX, double dY)
            {
                x += dX;
                y += dY;
            }

            public Point ToPoint()
            {
                return new Point((int)x, (int)y);
            }

            public double Angle()
            {
                return Math.Atan2(y, x);
            }

            public bool InLine(Line2D line)
            {
                return InLine(line.pointA, line.pointB);
            }

            public bool InLineX(Line2D line)
            {
                return InLineX(line.pointA, line.pointB);
            }

            public bool InLineR(Line2D line)
            {
                return InLineR(line.pointA, line.pointB);
            }

            /// <summary>
            /// 判断点在线段p(1,2)内
            /// </summary>
            /// <param name="p1"></param>
            /// <param name="p2"></param>
            /// <returns></returns>
            public bool InLine(Point2D p1, Point2D p2)
            {
                if (p1.x == p2.x)
                {
                    if (this.x == p1.x)
                        return ((this.y - p1.y) * (this.y - p2.y) <= 0);
                    else
                        return false;
                }
                else if (p1.y == p2.y)
                {
                    if (this.y == p1.y)
                        return ((this.x - p1.x) * (this.x - p2.x) <= 0);
                    else
                        return false;
                }
                else
                {
                    double xc = (this.y - p2.y) * (p1.x - p2.x) / (p1.y - p2.y) + p2.x;
                    if (xc == this.x)
                        return ((this.x - p1.x) * (this.x - p2.x) <= 0);
                    else
                        return false;
                }
            }

            /// <summary>
            /// 判断点在线条经过p1,p2的线上
            /// </summary>
            /// <param name="p1"></param>
            /// <param name="p2"></param>
            /// <returns></returns>
            public bool InLineX(Point2D p1, Point2D p2)
            {
                if (p1.x == p2.x)
                    return (this.x == p1.x);
                else if (p1.y == p2.y)
                    return (this.y == p1.y);
                else
                {
                    double xc = (this.y - p2.y) * (p1.x - p2.x) / (p1.y - p2.y) + p2.x;
                    return (xc == this.x);
                }
            }

            /// <summary>
            /// 判断点在线条p(1,2)的右侧延长线上
            /// </summary>
            /// <param name="p1"></param>
            /// <param name="p2"></param>
            /// <returns></returns>
            public bool InLineR(Point2D p1, Point2D p2)
            {
                if (p1.x == p2.x)
                {
                    if (this.x == p1.x)
                        return (p2.y > p1.y && y > p2.y || p2.y < p1.y && y < p2.y);
                }
                else if (p1.y == p2.y)
                {
                    if (this.y == p1.y)
                        return (p2.x > p1.x && x > p2.x && p2.x < p1.x && x < p2.x);
                }
                else
                {
                    double xc = (this.y - p2.y) * (p1.x - p2.x) / (p1.y - p2.y) + p2.x;
                    if (xc == this.x)
                        return (p2.x > p1.x && x > p2.x && p2.x < p1.x && x < p2.x);
                }
                return false;
            }

            public override string ToString()
            {
                return string.Format("{0}({1}*{2})+{3}", name, x, y, deg);
            }
        }

        /// <summary>
        /// 有向线段(a->b)
        /// </summary>
        public class Line2D
        {
            /// <summary>
            /// 两线相交的模式
            /// </summary>
            public enum enumCrossWay
            {
                enumNone ,
                /// <summary>
                /// 两线相交第一点
                /// </summary>
                enumCross1 ,
                /// <summary>
                /// 两线相交于中间自由点
                /// </summary>
                enumCrossIn ,
                /// <summary>
                /// 两线相交于第二点
                /// </summary>
                enumCross2 ,
                /// <summary>
                /// 两线平行相交于第一点
                /// </summary>
                enumMix1 ,
                /// <summary>
                /// 两线平行相交于中间自由点
                /// </summary>
                enumMixIn ,
                /// <summary>
                /// 两线平行相交于第二点
                /// </summary>
                enumMix2 ,
                /// <summary>
                /// 两线平行相交于延长线上的任意一点
                /// </summary>
                enumMixOut
            }

            public Point2D pointA { get; set; }
            public Point2D pointB { get; set; }
            public double deg { get; set; }

            public Line2D(Point2D a, Point2D b)
            {
                pointA = a;
                pointB = b;
                deg = Math.Atan2(pointB.y - pointA.y, pointB.x - pointA.x);
            }

            public Line2D(Polygon2D p, int nIndex)
            {
                pointA = p[nIndex % p.Count];
                pointB = p[(nIndex + 1) % p.Count];
                deg = Math.Atan2(pointB.y - pointA.y, pointB.x - pointA.x);
            }

            public Line2D(Polygon2D p, int nIndex , Point2D pointStart)
            {
                pointA = pointStart;
                pointB = p[(nIndex + 1) % p.Count];
                deg = Math.Atan2(pointB.y - pointA.y, pointB.x - pointA.x);
            }

            public double GetAngle()
            {
                return Math.Atan2(pointB.y - pointA.y, pointB.x - pointA.x);
            }

            /// <summary>
            /// 求线段b的拐向
            /// </summary>
            /// <param name="b"></param>
            /// <returns>0~pi:左拐,pi~2pi:右拐</returns>
            public double GetOffCourse(Line2D b)
            {
                double dAngle2 = b.GetAngle();
                double dAngle1 = this.GetAngle();
                return (dAngle2 - dAngle1 + Math.PI * 2) % (Math.PI * 2);
            }

            public override string ToString()
            {
                return string.Format("{0}->{1}", pointA, pointB);
            }

            #region 两线段相交测试

            /// <summary>
            /// 搜索与多边形相交点所在的线段
            /// </summary>
            /// <param name="p"></param>
            /// <param name="nStartIndex"></param>
            /// <param name="enumWay"></param>
            /// <param name="pointCross"></param>
            /// <returns></returns>
            public int CrossSearch(Polygon2D p, int nStartIndex, out enumCrossWay enumWay, out Point2D pointCross)
            {
                for (int i = nStartIndex; i < p.Count; i++)
                {
                    Line2D b = new Line2D(p, i);
                    enumWay = CrossTest(b, out pointCross);
                    if (enumWay != enumCrossWay.enumNone)
                    {
                        if (enumWay == enumCrossWay.enumCross1 ||
                            enumWay == enumCrossWay.enumCrossIn)
                        {
                            if (GetOffCourse(b) < Math.PI)
                            {
                                enumWay = enumCrossWay.enumNone;
                                continue;
                            }
                        }
                        if (enumWay == enumCrossWay.enumCross2)
                        {
                            //相交于该线的终点,即相当于下一条线的起点
                            enumWay = enumCrossWay.enumNone;
                        }
                        else if (enumWay == enumCrossWay.enumMixOut)
                        {
                            //Line2D b2 = new Line2D(p, i + 1);
                            //if (GetOffCourse(b2) < Math.PI)
                            //    enumWay = enumCrossWay.enumNone;

                            //相交于该线的终点,即相当于下一条线的起点
                            enumWay = enumCrossWay.enumNone;
                        }
                        else
                            return i;
                    }
                }
                enumWay = enumCrossWay.enumNone;
                pointCross = new Point2D();
                return -1;
            }

            /// <summary>
            /// 两线段相交测试
            /// </summary>
            /// <param name="b"></param>
            /// <param name="pointCross"></param>
            /// <returns></returns>
            public enumCrossWay CrossTest(Line2D b, out Point2D pointCross)
            {
                double degSub = Math.Abs(this.deg - b.deg);
                if (degSub == 0 || degSub == Math.PI)
                {
                    //平行线
                    if (pointB.InLine(b))
                    {
                        pointCross = new Point2D(pointB.name, pointB.x, pointB.y, pointB.deg);
                        if (pointB.Equal(b.pointB))
                            return enumCrossWay.enumMix2;
                        else if (GetOffCourse(b) == 0)
                        {
                            if (pointB.Equal(b.pointA))
                                return enumCrossWay.enumMix1;
                            else
                                return enumCrossWay.enumMixIn;
                        }
                        else
                        {
                            //反向平行(不考虑超越起点)
                            pointCross = new Point2D(b.pointB.name, b.pointB.x, b.pointB.y, b.pointB.deg);
                            return enumCrossWay.enumMixIn;
                        }
                    }
                    else if (pointB.InLineR(b))
                    {
                        pointCross = new Point2D(pointB.name, pointB.x, pointB.y, pointB.deg);
                        return enumCrossWay.enumMixOut;
                    }
                    else
                    {
                        //反向平行(不考虑超越起点)
                        pointCross = new Point2D();
                        return enumCrossWay.enumNone;
                    }
                }
                else
                {
                    //两线相交
                    double yCross = 0;
                    double xCross = 0;
                    if (pointA.x == pointB.x || b.pointA.x == b.pointB.x)
                    {
                        //处理竖线
                        if (pointA.x == pointB.x)
                        {
                            //当前线条为竖线
                            xCross = pointA.x;
                            yCross = (b.pointB.y - b.pointA.y) * (pointB.x - b.pointA.x) / (b.pointB.x - b.pointA.x) + b.pointA.y;
                        }
                        else
                        {
                            //另一线条为竖线
                            xCross = b.pointA.x;
                            yCross = (pointB.y - pointA.y) * (b.pointB.x - pointA.x) / (pointB.x - pointA.x) + pointA.y;
                        }
                    }
                    else
                    {
                        //无竖线相交
                        double ka = (pointB.y - pointA.y) / (pointB.x - pointA.x);
                        double kb = (b.pointB.y - b.pointA.y) / (b.pointB.x - b.pointA.x);
                        xCross = ((ka * pointB.x - kb * b.pointB.x) - (pointB.y - b.pointB.y)) / (ka - kb);
                        yCross = ka * (xCross - pointB.x) + pointB.y;
                    }
                    if ((yCross - b.pointA.y) * (yCross - b.pointB.y) <= 0 &&
                        (yCross - pointA.y) * (yCross - pointB.y) <= 0 &&
                        (xCross - b.pointA.x) * (xCross - b.pointB.x) <= 0 &&
                        (xCross - pointA.x) * (xCross - pointB.x) <= 0)
                    {
                        if (yCross == b.pointB.y && xCross == b.pointB.x)
                        {
                            pointCross = new Point2D(b.pointB.name, b.pointB.x, b.pointB.y, b.pointB.deg);
                            return enumCrossWay.enumCross2;
                        }
                        else if (yCross == b.pointA.y && xCross == b.pointA.x)
                        {
                            pointCross = new Point2D(b.pointA.name, b.pointA.x, b.pointA.y, b.pointA.deg);
                            return enumCrossWay.enumCross1;
                        }
                        else
                        {
                            if (yCross == pointB.y && xCross == pointB.x)
                            {
                                pointCross = new Point2D(pointB.name, pointB.x, pointB.y, pointB.deg);
                                return enumCrossWay.enumCrossIn;
                            }
                            else if (yCross == pointA.y && xCross == pointA.x)
                            {
                                pointCross = new Point2D(pointA.name, pointA.x, pointA.y, pointA.deg);
                                return enumCrossWay.enumCrossIn;
                            }
                            else
                            {
                                pointCross = new Point2D("*", xCross, yCross, 0);
                                return enumCrossWay.enumCrossIn;
                            }
                        }
                    }
                    pointCross = new Point2D();
                    return enumCrossWay.enumNone;
                }
            }

            #endregion
        }

        /// <summary>
        /// 多边形(点集以逆时针方式提供并存储,线段不得存在交叉)
        /// </summary>
        public class Polygon2D : IEnumerable
        {

            /// <summary>
            /// 逆时针的点序
            /// </summary>
            private List<Point2D> data;

            public Polygon2D(List<Point2D> A)
            {
                data = Copy(A);
            }

            public Polygon2D(List<Point2D> A, bool bAutoCenter, bool bRename)
            {
                data = Copy(A, bAutoCenter, bRename);
            }

            public List<Point2D> ToList()
            {
                return data;
            }

            #region 枚举相关代码

            public void Clear()
            {
                data.Clear();
            }

            public int Count
            {
                get
                {
                    return data.Count;
                }
            }

            public Point2D this[int nIndex]
            {
                get
                {
                    if (nIndex >= 0 && nIndex < data.Count)
                        return data[nIndex] as Point2D;
                    else
                        return null;
                }
                set
                {
                    if (nIndex >= 0 && nIndex < data.Count)
                        data[nIndex] = value;
                }
            }

            public IEnumerator GetEnumerator()
            {
                foreach (object obj in data) yield return (Point2D)obj;
            }

            #endregion

            #region 排序

            /// <summary>
            /// 将对象A,以左下角为起点复制至结果集
            /// </summary>
            /// <param name="A"></param>
            /// <returns></returns>
            public List<Point2D> Copy(List<Point2D> A)
            {
                return Copy(A, true, false);
            }

            public List<Point2D> Copy(List<Point2D> A, bool bAutoCenter, bool bRename)
            {
                int nMinIndex = GetMinIndex(A);
                bool bFirst = true;
                double dDegLast = 0;
                Point2D pointLast = null;
                List<Point2D> P = new List<Point2D>();
                for (int i = nMinIndex; i < nMinIndex + A.Count; i++)
                {
                    int nIndex = i % A.Count;
                    Point2D p = A[nIndex];
                    if (bFirst)
                    {
                        bFirst = false;
                        pointLast = A[(i - 1 + A.Count) % A.Count];
                    }

                    double dDeg = (Math.Atan2(p.y - pointLast.y, p.x - pointLast.x) + Math.PI * 2) % (Math.PI * 2);
                    P.Add(new Point2D(bRename ? (i + 1 - nMinIndex).ToString() : p.name, p.x, p.y, dDeg));

                    dDegLast = p.Angle();
                    pointLast = p;
                }

                if (bAutoCenter)
                    AutoCenter(ref P);
                return P;
            }

            /// <summary>
            /// 求Y最小,X最小的点(左下解)
            /// </summary>
            /// <param name="p"></param>
            /// <returns></returns>
            public int GetMinIndex(List<Point2D> p)
            {
                if (p.Count == 0)
                    return -1;
                else
                {
                    int nIndex = 0;
                    Point2D minPoint = p[0];

                    for (int i = 1; i < p.Count; i++)
                    {
                        if (p[i].y < minPoint.y || p[i].y == minPoint.y && p[i].x < minPoint.x)
                        {
                            nIndex = i;
                            minPoint = p[i];
                        }
                    }
                    return nIndex;
                }
            }

            #endregion

            #region 解析

            /// <summary>
            /// 按1,2;3,4...格式解析成坐标序列
            /// </summary>
            /// <param name="strText"></param>
            /// <returns></returns>
            public static List<Point2D> Parse(string strText)
            {
                List<Point2D> list = new List<Point2D>();
                string[] strRows = strText.Split(new char[] { ';' }, StringSplitOptions.RemoveEmptyEntries);
                int nIndex = 0;
                foreach (string strRow in strRows)
                {
                    string[] strCols = strRow.Split(new char[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
                    if (strCols.Length == 2)
                    {
                        nIndex++;
                        Point2D p = new Point2D(nIndex.ToString(), double.Parse(strCols[0]), double.Parse(strCols[1]));
                        list.Add(p);
                    }
                }
                return list;
            }

            #endregion

            #region 计算点是否在多边形内

            public bool InPolygon(Point2D p)
            {
                //相交类型:
                //0:中间相交
                //1:上端点相交
                //2:下端点相交
                int nCrossFlag = 0;

                int nCount = 0;
                for (int i = 0; i < data.Count; i++)
                {
                    Point2D p1 = data[i];
                    Point2D p2 = data[(i + 1) % data.Count];

                    if (p.InLine(p1, p2))
                        return true;
                    else if (p1.y != p2.y)
                    {
                        //水平线同时与点不相交,该线可以忽略处理
                        if (!(p1.y < p.y && p2.y < p.y || p1.y > p.y && p2.y > p.y))
                        {
                            double x = (p.y - p2.y) * (p1.x - p2.x) / (p1.y - p2.y) + p2.x;
                            if (x >= p.x)
                            {
                                //处理与线条的端点相交
                                if (p.y == p1.y || p.y == p2.y)
                                {
                                    int nCrossFlagCur = 0;
                                    if (p.y == p1.y && p1.y > p2.y || p.y == p2.y && p2.y > p1.y)
                                    {
                                        //上端点相交
                                        nCrossFlagCur = 1;
                                    }
                                    else if (p.y == p1.y && p1.y < p2.y || p.y == p2.y && p2.y < p1.y)
                                    {
                                        //下端点相交
                                        nCrossFlagCur = 2;
                                    }
                                    if (nCrossFlag == 0)
                                        nCrossFlag = nCrossFlagCur;
                                    else
                                    {
                                        if (nCrossFlag != nCrossFlagCur)
                                            nCount++;
                                        nCrossFlag = 0;
                                    }
                                }
                                else
                                {
                                    nCount++;
                                }
                            }
                        }
                    }
                }

                return nCount % 2 == 1;
            }
            #endregion

            #region 计算线的偏离方向

            /// <summary>
            /// 求指定一点,线的偏离方向
            /// </summary>
            /// <param name="nIndex"></param>
            /// <param name="bForward">向前或向后</param>
            /// <returns>0~pi:左拐,pi~2pi:右拐</returns>
            public double GetOffCourse(int nIndex)
            {
                return GetOffCourse(nIndex, true);
            }

            public double GetOffCourse(int nIndex, bool bForward)
            {
                Point2D pCur = data[nIndex];
                int nPreId = (bForward ? (nIndex - 1 + data.Count) : (nIndex + 1)) % data.Count;
                Point2D pPre = data[nPreId];
                int nNextId = (bForward ? (nIndex + 1) : (nIndex - 1 + data.Count)) % data.Count;
                Point2D pNext = data[nNextId];

                double dAngle1 = Math.Atan2(pCur.y - pPre.y, pCur.x - pPre.x);
                double dAngle2 = Math.Atan2(pNext.y - pCur.y, pNext.x - pCur.x);

                return (dAngle2 - dAngle1 + Math.PI * 2) % (Math.PI * 2);
            }

            public double GetOffCourse(Point2D pCur, Point2D pPre, Point2D pNext)
            {
                double dAngle1 = Math.Atan2(pCur.y - pPre.y, pCur.x - pPre.x);
                double dAngle2 = Math.Atan2(pNext.y - pCur.y, pNext.x - pCur.x);

                return (dAngle2 - dAngle1 + Math.PI * 2) % (Math.PI * 2);
            }

            #endregion

            #region 输出一个凸多边形

            public List<Point2D> Pop()
            {
                List<Point2D> listOut = new List<Point2D>();
                if (data.Count == 0)
                {
                    return new List<Point2D>();
                }
                int nStartPos = 0;
                double dOC0 = GetOffCourse(nStartPos);
                if (dOC0 > Math.PI)
                {
                    for (int i = nStartPos; i < data.Count - 1; i++)
                    {
                        double dOCi = GetOffCourse(i + 1);
                        if (dOCi < Math.PI)
                            nStartPos = i;
                            break;
                    }
                }
                else
                {
                    for (int i = data.Count - 1; i > 0; i--)
                    {
                        double dOCi = GetOffCourse(i);
                        if (dOCi > Math.PI)
                        {
                            nStartPos = i;
                            break;
                        }
                    }
                }
                int nStopPos = nStartPos;
                for(int i = 1;i < data.Count;i ++)
                {
                    double dOCi = GetOffCourse(i);
                    if(dOCi > Math.PI)
                    {
                        nStopPos = i;
                        break;
                    }
                }

                double dOCs = GetOffCourse(nStopPos);
                if(nStopPos == nStartPos && dOCs < Math.PI)
                {
                    foreach (Point2D point in data) listOut.Add(new Point2D(point.name, point.x, point.y, point.deg));
                    data = new List<Point2D>();
                    return listOut;
                }
                else
                {
                    //求(nStopPos - 1 , nStopPos)射线相交的线段,以线段的终点为该闭合线
                    if(nStopPos <= nStartPos)
                        nStopPos += data.Count;

                    Line2D lineRef = new Line2D (data[(nStopPos - 1 + data.Count) % data.Count] , data[nStopPos % data.Count]);

                    while(nStartPos < nStopPos)
                    {
                        if ((nStopPos - nStartPos) % data.Count == 0)
                            nStartPos++;
                        else
                        {
                            Line2D lineMid = new Line2D(data[nStopPos % data.Count], data[nStartPos % data.Count]);
                            if (lineRef.GetOffCourse(lineMid) < Math.PI)
                                break;
                            else
                                nStartPos++;
                        }
                    }
                    lineRef = new Line2D(data[nStartPos % data.Count], data[(nStartPos + 1) % data.Count]);
                    while (nStartPos < nStopPos)
                    {
                        Line2D lineMid = new Line2D(data[nStopPos % data.Count], data[nStartPos % data.Count]);
                        if (lineMid.GetOffCourse(lineRef) < Math.PI)
                            break;
                        else
                            nStopPos--;
                    }
                    return Pop(nStartPos, nStopPos);
                }
            }

            /// <summary>
            /// 将指定区间的点组成一个多边形返回,同时将除首尾两点以外的点移除
            /// </summary>
            /// <param name="nStart"></param>
            /// <param name="nStop"></param>
            /// <returns></returns>
            public List<Point2D> Pop(int nStart, int nStop)
            {
                List<Point2D> list = new List<Point2D>();
                Line2D lineRef = new Line2D(data[nStop % data.Count], data[nStart % data.Count]);
                for (int i = nStart; i <= nStop; i++)
                {
                    if (i == nStart)
                    {
                        //判断起点位置夹角是还为180度
                        Line2D lineNext = new Line2D(data[nStart % data.Count], data[(nStart + 1) % data.Count]);
                        if (lineRef.GetOffCourse(lineNext) == 0)
                            continue;
                    }
                    else if (i == nStop)
                    {
                        //判断终点位置夹角是还为180度
                        Line2D linePre = new Line2D(data[(nStop - 1 + data.Count) % data.Count], data[nStop % data.Count]);
                        if (linePre.GetOffCourse(lineRef) == 0)
                            continue;
                    }
                    int nIndex = i % data.Count;
                    list.Add(new Point2D(data[nIndex].name, data[nIndex].x, data[nIndex].y, data[nIndex].deg));
                }

                Line2D linePre2 = new Line2D(data[(nStop + 1) % data.Count], data[nStop % data.Count]);
                Line2D lineNext2 = new Line2D(data[nStart % data.Count], data[(nStart - 1 + data.Count) % data.Count]);
                //判断起点位置夹角是还为180度
                if (lineRef.GetOffCourse(lineNext2) == 0)
                {
                    nStart = nStart == 0 ? (nStart - 1 + data.Count) : (nStart - 1);
                }
                //判断终点位置夹角是还为180度
                if (linePre2.GetOffCourse(lineRef) == 0)
                {
                    nStop++;
                }

                int nCount = data.Count;
                nStart %= data.Count;
                nStop %= data.Count;
                if (nStop > nStart)
                {
                    for (int i = nStop - 1; i > nStart; i--)
                    {
                        int nIndex = i % nCount;
                        data.RemoveAt(nIndex);
                    }
                }
                else
                {
                    for (int i = nCount - 1; i > nStart; i--)
                    {
                        data.RemoveAt(i);
                    }
                    for (int i = nStop - 1; i >= 0; i--)
                    {
                        data.RemoveAt(i);
                    }
                }
                return list;
            }

            #endregion

            /// <summary>
            /// 拆分多边形为若干个凸多边形
            /// </summary>
            /// <returns></returns>
            public List<Polygon2D> Split()
            {
                List<Polygon2D> ps = new List<Polygon2D>();
                List<Point2D> listResult = Pop();
                while (listResult.Count > 0)
                {
                    ps.Add(new Polygon2D(listResult, false, false));
                    listResult = Pop();
                }
                if (data.Count > 0)
                {
                    ps.Add(this);
                    throw new Exception("Polygon Split Error");
                }
                return ps;
            }

            #region 居中操作

            /// <summary>
            /// 获取多边形的中心点(求坐标的均值)
            /// </summary>
            /// <param name="p"></param>
            /// <returns></returns>
            public Point2D GetCenter(List<Point2D> p)
            {
                if (p.Count > 0)
                {
                    double dXSum = 0;
                    double dYSum = 0;
                    foreach (Point2D point in p)
                    {
                        dXSum += point.x;
                        dYSum += point.y;
                    }

                    return new Point2D("", dXSum / p.Count, dYSum / p.Count);
                }
                else
                    return new Point2D("", 0, 0);
            }

            /// <summary>
            /// 自动以中心点为原点
            /// </summary>
            /// <param name="p"></param>
            public void AutoCenter(ref List<Point2D> p)
            {
                Point2D center = GetCenter(p);
                Move(ref p, center);
            }

            /// <summary>
            /// 偏移一个坐标(坐标-偏移值)
            /// </summary>
            /// <param name="p"></param>
            /// <param name="offset"></param>
            public void Move(ref List<Point2D> p, Point2D offset)
            {
                foreach (Point2D point in p)
                {
                    point.x -= offset.x;
                    point.y -= offset.y;
                }
            }

            /// <summary>
            /// 获取多边形的中心点(求坐标的均值)
            /// </summary>
            /// <param name="p"></param>
            /// <returns></returns>
            public Point2D GetCenter()
            {
                if (data.Count > 0)
                {
                    double dXSum = 0;
                    double dYSum = 0;
                    foreach (Point2D point in data)
                    {
                        dXSum += point.x;
                        dYSum += point.y;
                    }

                    return new Point2D("", dXSum / data.Count, dYSum / data.Count);
                }
                else
                    return new Point2D("", 0, 0);
            }

            /// <summary>
            /// 自动以中心点为原点
            /// </summary>
            /// <param name="p"></param>
            public void AutoCenter()
            {
                Point2D center = GetCenter();
                Move(center);
            }

            /// <summary>
            /// 偏移一个坐标(坐标-偏移值)
            /// </summary>
            /// <param name="p"></param>
            /// <param name="offset"></param>
            public void Move(Point2D offset)
            {
                foreach (Point2D point in data)
                {
                    point.x -= offset.x;
                    point.y -= offset.y;
                }
            }

            #endregion

            #region 输出多边形

            /// <summary>
            /// 输出Minkowski和
            /// </summary>
            /// <returns></returns>
            public override string ToString()
            {
                StringBuilder sb = new StringBuilder();
                bool bFirst = true;
                foreach (Point2D p in data)
                {
                    if (bFirst)
                    {
                        bFirst = false;
                        sb.AppendFormat("{0},{1}", p.x, p.y);
                    }
                    else
                        sb.AppendFormat(";{0},{1}", p.x, p.y);
                }

                return sb.ToString();
            }

            /// <summary>
            /// 画一个多边形
            /// </summary>
            /// <param name="g"></param>
            /// <param name="org">原点</param>
            /// <param name="pen">画笔</param>
            /// <param name="nTimes">放大倍数</param>
            public void DrawBox(Graphics g, System.Drawing.Point org, Pen pen, int nTimes)
            {
                DrawBox(g, org, pen, nTimes, false);
            }

            public void DrawBox(Graphics g, System.Drawing.Point org, Pen pen, int nTimes , bool bWithPoint)
            {
                for (int i = 0; i < data.Count; i++)
                {
                    System.Drawing.Point ps = new System.Drawing.Point(org.X + (int)(data[i].x * nTimes), org.Y - (int)(data[i].y * nTimes));
                    System.Drawing.Point pe = new System.Drawing.Point(org.X + (int)(data[(i + 1) % data.Count].x * nTimes), org.Y - (int)(data[(i + 1) % data.Count].y * nTimes));
                    g.DrawLine(pen, ps, pe);
                    g.DrawString(data[i].name, SystemFonts.DefaultFont, Brushes.Black, ps);
                    if (bWithPoint)
                    {
                        DrawPointStatus(g, org, data[i], nTimes);
                    }
                }
            }

            /// <summary>
            /// 画点在多边形的位置状态(绿色在内,红色在外)
            /// </summary>
            /// <param name="g"></param>
            /// <param name="p"></param>
            public void DrawPointStatus(Graphics g, System.Drawing.Point org, Point2D p, int nTimes)
            {
                Point2D pp = new Point2D("", p.x, p.y);
                Pen pen = Pens.Black;
                Brush brush = Brushes.Black;
                if (!InPolygon(pp))
                {
                    pen = Pens.Red;
                    brush = Brushes.Red;
                }
                int nSize = 6;

                Point pointStart = new Point((int)(org.X + p.x * nTimes - nSize / 2), (int)(org.Y - p.y * nTimes - nSize / 2));

                g.DrawArc(pen, new Rectangle(pointStart, new Size(nSize, nSize)), 0, 360);
                g.FillPie(brush, new Rectangle(pointStart, new Size(nSize, nSize)), 0, 360);
            }

            #endregion
        }

        private Polygon2D P;
        private Polygon2D Q;
        private Polygon2D R;

        public Polygon2D SumResult
        {
            get
            {
                return R;
            }
        }

        public MinkowskiSum(List<Point2D> A, List<Point2D> B)
        {
            P = new Polygon2D(A,false,true);
            Q = new Polygon2D(B,false,true);
        }

        public MinkowskiSum(List<Point2D> A, List<Point2D> B, bool bBCenter)
        {
            P = new Polygon2D(A, false, true);
            Q = new Polygon2D(B, bBCenter, true);
        }

        /// <summary>
        /// 求Minkowski和
        /// </summary>
        /// <returns></returns>
        public bool MakeSum()
        {
            List<Point2D> r;
            if (SimpleSum(P, Q, out r))
            {
                R = new Polygon2D (r, false , false);
                return true;
            }
            else
                return false;
        }

        /// <summary>
        /// 分解P后再依次与Q求Minkowski和,然后再合并(目前算法不合理,用的是求Minkowski和)
        /// </summary>
        /// <returns></returns>
        public bool MakeSumSplit()
        {
            Polygon2D PCopy = new Polygon2D(P.ToList(), false, false);
            List<Polygon2D> ps = PCopy.Split();

            Polygon2D QCopy = new Polygon2D(Q.ToList(), false, false);
            List<Polygon2D> qs = QCopy.Split();

            Polygon2D pSum = null;
            foreach (Polygon2D p in ps)
            {
                Polygon2D r;
                if (SimpleSum(p, Q, out r))
                {
                    if (pSum == null)
                        pSum = r;
                    else
                    {
                        Polygon2D[] P2 = new Polygon2D[]{
                            pSum ,
                            r
                        };
                        Polygon2D r2;
                        Combine(P2, out r2);
                        pSum = r2;
                    }
                }
            }
            R = pSum;
            return true;
        }

        /// <summary>
        /// 求两凸变形的Minkowski和
        /// </summary>
        /// <param name="P"></param>
        /// <param name="Q"></param>
        /// <param name="R"></param>
        /// <returns></returns>
        public bool SimpleSum(List<Point2D> P, List<Point2D> Q, out List<Point2D> R)
        {
            return SimpleSum(new Polygon2D(P, false, false), new Polygon2D(Q, false, false), out R);
        }

        public bool SimpleSum(Polygon2D P, Polygon2D Q, out Polygon2D R)
        {
            List<Point2D> r;
            bool bRet = SimpleSum(P, Q, out r);
            R = new Polygon2D(r, false, false);
            return bRet;
        }

        public bool SimpleSum(Polygon2D P, Polygon2D Q, out List<Point2D> R)
        {
            R = new List<Point2D>();
            if (P.Count >= 3 && Q.Count >= 3)
            {
                int pi = 0;
                int qi = 0;
                while (pi < P.Count || qi < Q.Count)
                {
                    int pii = pi % P.Count;
                    int qii = qi % Q.Count;

                    R.Add(new Point2D(string.Format("{0}-{1}", P[pii].name, Q[qii].name), P[pii].x + Q[qii].x, P[pii].y + Q[qii].y));
                    if (pi == P.Count && qi < Q.Count)
                        qi++;
                    else if (pi < P.Count && qi == Q.Count)
                        pi++;
                    else if (P[(pi + 1)%P.Count].deg < Q[(qi + 1)%Q.Count].deg)
                        pi++;
                    else if (P[(pi + 1)%P.Count].deg > Q[(qi + 1)%Q.Count].deg)
                        qi++;
                    else
                    {
                        pi++;
                        qi++;
                    }
                }
                //R.Add(new Point2D(string.Format("{0}-{1}", P[pi].name, Q[qi].name), P[pi].x + Q[qi].x, P[pi].y + Q[qi].y));
                R = new Polygon2D(R, false, false).ToList();
                return true;
            }
            else
                return false;
        }

        public bool Combine(Polygon2D[] P, out Polygon2D R)
        {
            List<Point2D> r;
            bool bRet = Combine(P, out r);
            R = new Polygon2D(r, false, false);
            return bRet;
        }

        /// <summary>
        /// 合同并两多边形
        /// </summary>
        /// <param name="P"></param>
        /// <param name="R"></param>
        /// <returns></returns>
        public bool Combine(Polygon2D[] P, out List<Point2D> R)
        {
            R = new List<Point2D>();
            if (P.Length != 2)
            {
                return false;
            }
            if (P[0].Count >= 3 && P[1].Count >= 3)
            {
                //设定开始计算的多边形
                int pIndex = (P[0][0].y < P[1][0].y || P[0][0].y == P[1][0].y && P[0][0].x <= P[1][0].x) ? 0 : 1;
                int pIndexCopy = pIndex;
                int[] nPos = new int[] { 0, 0 };
                int[] nStartOffset = new int[] { 0, 0 };
                Point2D pointCross = P[pIndex][nPos[pIndex]];
                Point2D pointLast = null;
                bool bFirstCross = true;
                while (nPos[pIndex] < P[pIndex].Count + nStartOffset[pIndex])
                {
                    if (pointLast == null || pointLast != null && !pointCross.Equal(pointLast))
                    {
                        R.Add(pointCross);
                    }
                    else
                    {
                        Console.WriteLine(pointCross);
                    }

                    pointLast = pointCross;

                    Line2D.enumCrossWay enumWay;
                    Line2D lineA = new Line2D(pointCross, P[pIndex][(nPos[pIndex] + 1) % P[pIndex].Count]);
                    int nNextIndex = lineA.CrossSearch(P[1 - pIndex], nPos[1 - pIndex], out enumWay, out pointCross);
                    if (nNextIndex < 0)
                    {
                        nPos[pIndex]++;
                        pointCross = P[pIndex][nPos[pIndex] % P[pIndex].Count];
                    }
                    else
                    {
                        if (bFirstCross)
                        {
                            //解决第一次相交的点之前的线段被忽略计算
                            bFirstCross = false;
                            nStartOffset[1 - pIndex] = nNextIndex == 0 ? 0 : (nNextIndex + 1);
                        }
                        if (enumWay == Line2D.enumCrossWay.enumCross1 ||
                        enumWay == Line2D.enumCrossWay.enumCrossIn)
                        {
                            //相交于起点或中间点(左拐的情况已经在CrossSearch中规避了)
                            pIndex = 1 - pIndex;
                            nPos[pIndex] = nNextIndex;
                        }
                        else if (enumWay == Line2D.enumCrossWay.enumCross2)
                        {
                            //相交于终点(相当于下一线段的起点,CrossSearch中规避了)
                        }
                        else if (enumWay == Line2D.enumCrossWay.enumMixIn)
                        {
                            Line2D lineB = new Line2D(P[1 - pIndex], nNextIndex + 1);
                            if (lineA.GetOffCourse(lineB) > Math.PI)
                            {
                                //另一条线右拐
                                nPos[pIndex]++;
                                pIndex = 1 - pIndex;
                                nPos[pIndex] = nNextIndex + 1;
                                pointCross = P[pIndex][nPos[pIndex]];
                            }
                            else
                            {
                                nPos[pIndex]++;
                            }
                        }
                        else if (enumWay == Line2D.enumCrossWay.enumMix1)
                        {
                            Line2D lineB = new Line2D(P[pIndex], (nPos[pIndex] + 1) % P[pIndex].Count);
                            if (lineA.GetOffCourse(lineB) > Math.PI)
                            {
                                //当前线段右拐
                                nPos[pIndex]++;
                            }
                            else
                            {
                                nPos[pIndex]++;
                                pIndex = 1 - pIndex;
                                nPos[pIndex] = nNextIndex + 1;
                                pointCross = P[pIndex][nPos[pIndex]];
                            }
                        }
                        else if (enumWay == Line2D.enumCrossWay.enumMix2)
                        {
                            nPos[pIndex]++;
                            nPos[1 - pIndex] = nNextIndex + 1;
                        }
                        else if (enumWay == Line2D.enumCrossWay.enumMixOut)
                        {
                            //左拐的情况已经在CrossSearch中规避了
                            pIndex = 1 - pIndex;
                            nPos[pIndex] = nNextIndex + 1;
                        }
                    }
                }
                return true;

            }
            else if (P[0].Count >= 3)
            {
                R = P[0].ToList();
                return true;
            }
            else if (P[1].Count >= 3)
            {
                R = P[1].ToList();
                return true;
            }
            else
            {
                R = new List<Point2D>();
                return false;
            }
        }

        #region 图形输出

        public Image DrawSplit(System.Drawing.Point org, Size size)
        {
            Bitmap bm = new Bitmap(size.Width, size.Height);
            Graphics g = Graphics.FromImage(bm);
            g.FillRectangle(Brushes.White, new Rectangle(new Point(0, 0), size));
            DrawCoordinate(g, size, org);

            DrawSplit(g, org, size , 100);
            return bm;

        }

        public void DrawSplit(Graphics g, System.Drawing.Point org, Size size, int nTimes)
        {
            Q.DrawBox(g, org, Pens.Blue, nTimes);

            Polygon2D PCopy = new Polygon2D(P.ToList(), false, false);
            List<Polygon2D> ps = PCopy.Split();

            Polygon2D QCopy = new Polygon2D(Q.ToList(), false, false);
            List<Polygon2D> qs = QCopy.Split();

            Polygon2D pSum = null;
            foreach (Polygon2D p in ps)
            {
                p.DrawBox(g, org, Pens.LightGreen, nTimes);

                foreach (Polygon2D q in qs)
                {
                    q.DrawBox(g, org, Pens.Blue, nTimes);
                    Polygon2D r;
                    if (SimpleSum(p, q, out r))
                    {
                        r.DrawBox(g, org, Pens.Red, nTimes);
                        if (pSum == null)
                            pSum = r;
                        else
                        {
                            Polygon2D[] P2 = new Polygon2D[]{
                                    pSum ,
                                    r
                                };
                            Polygon2D r2;
                            Combine(P2, out r2);
                            pSum = r2;
                        }
                    }
                }
            }
            pSum.DrawBox(g, org, Pens.Blue, nTimes, true);
        }

        /// <summary>
        /// 将三个多边形画到画面上
        /// </summary>
        /// <param name="org">画布原点</param>
        /// <param name="size">画布尺寸</param>
        /// <returns></returns>
        public Image DrawImage(System.Drawing.Point org, Size size)
        {
            Bitmap bm = new Bitmap(size.Width, size.Height);
            Graphics g = Graphics.FromImage(bm);
            g.FillRectangle(Brushes.White, new Rectangle(new Point(0, 0), size));

            DrawCoordinate(g, size , org);

            int nTimes = 60;
            P.DrawBox(g, org, Pens.LightGreen, nTimes);
            Q.DrawBox(g, org, Pens.Blue, nTimes);
            R.DrawBox(g, org, Pens.Red, nTimes);
            
            DrawSplit(g, org, size, nTimes);
            
            return bm;
        }

        /// <summary>
        /// 画坐标线
        /// </summary>
        /// <param name="g"></param>
        /// <param name="size">画布尺寸</param>
        /// <param name="org">原点</param>
        public void DrawCoordinate(Graphics g, Size size, System.Drawing.Point org)
        {
            g.DrawLine(Pens.DarkSlateGray, new System.Drawing.Point(0, org.Y), new System.Drawing.Point(size.Width, org.Y));
            g.DrawLine(Pens.DarkSlateGray, new System.Drawing.Point(org.X, 0), new System.Drawing.Point(org.X, size.Height));

            Pen pen = new Pen(Color.DarkSlateGray, 3);
            for (int i = 1; i < size.Width / 2 / 10; i++)
            {
                bool bTen = i % 10 == 0;
                bool bFive = i % 5 == 0;
                int nHeight = bTen ? 20 : (bFive ? 10 : 5);
                g.DrawLine(pen, new System.Drawing.Point(org.X + i * 10, org.Y), new System.Drawing.Point(org.X + i * 10, org.Y - nHeight));
                g.DrawLine(pen, new System.Drawing.Point(org.X - i * 10, org.Y), new System.Drawing.Point(org.X - i * 10, org.Y - nHeight));
            }
            for (int i = 1; i < size.Height / 2 / 10; i++)
            {
                bool bTen = i % 10 == 0;
                bool bFive = i % 5 == 0;
                int nHeight = bTen ? 20 : (bFive ? 10 : 5);
                g.DrawLine(pen, new System.Drawing.Point(org.X, org.Y + i * 10), new System.Drawing.Point(org.X + nHeight, org.Y + i * 10));
                g.DrawLine(pen, new System.Drawing.Point(org.X, org.Y - i * 10), new System.Drawing.Point(org.X + nHeight, org.Y - i * 10));
            }
        }

        #endregion

        #region 调用输出
        /// <summary>
        /// 直接输出图形
        /// </summary>
        /// <param name="p">逆序点集</param>
        /// <param name="q">逆序点集</param>
        /// <returns></returns>
        public static Image MinkImage(string p, string q)
        {
            List<MinkowskiSum.Point2D> pp = new List<MinkowskiSum.Point2D>();

            //string strText1 = "2,2;0,2;-2,1;-2,-2;2,-2";
            string strText1 = "2,2;1,2;1,0;0,0;0,2;-2,1;-3,-3;1,-2;1,-1;2,-1";
            //string strText1 = "2,2;1,2;1,0;0,0;0,2;-2,1;-2,-2;2,-1";
            pp = MinkowskiSum.Polygon2D.Parse(p ?? strText1);

            List<MinkowskiSum.Point2D> qq = new List<MinkowskiSum.Point2D>();

            //string strText2 = "-0.5,1;-0.5,-0.5;0.5,-0.5";
            //string strText2 = "0.5,1;0.5,-0.5;1.5,-0.5";
            string strText2 = "1.5,1;1,0;0.5,1;0.5,-0.5;1.5,-0.5";
            qq = MinkowskiSum.Polygon2D.Parse(q ?? strText2);

            //参数true表示对qq进行中心原点化
            MinkowskiSum mink = new MinkowskiSum(pp, qq, true);

            Size size = new Size(800, 800);
            Point point = new Point(size.Width / 2, size.Height / 2);
            return mink.DrawSplit(point, size);
        }

        /// <summary>
        /// 直接输出结果点集(格式:x1,y1;x2,y2...)
        /// </summary>
        /// <param name="p">逆序点集</param>
        /// <param name="q">逆序点集</param>
        /// <param name="strResult">结果点集(格式:x1,y1;x2,y2...)</param>
        /// <returns></returns>
        public static bool MinkResult(string p, string q, out string strResult)
        {
            //string strText1 = "2,2;0,2;-2,1;-2,-2;2,-2";
            string strText1 = "2,2;1,2;1,0;0,0;0,2;-2,1;-3,-3;1,-2;1,-1;2,-1";
            //string strText1 = "2,2;1,2;1,0;0,0;0,2;-2,1;-2,-2;2,-1";

            List<MinkowskiSum.Point2D> pp = new List<MinkowskiSum.Point2D>();
            pp = MinkowskiSum.Polygon2D.Parse(p ?? strText1);
            
            //string strText2 = "-0.5,1;-0.5,-0.5;0.5,-0.5";
            //string strText2 = "0.5,1;0.5,-0.5;1.5,-0.5";
            string strText2 = "1.5,1;1,0;0.5,1;0.5,-0.5;1.5,-0.5";

            List<MinkowskiSum.Point2D> qq = new List<MinkowskiSum.Point2D>();
            qq = MinkowskiSum.Polygon2D.Parse(q ?? strText2);

            MinkowskiSum mink = new MinkowskiSum(pp, qq);
            bool bRet = mink.MakeSumSplit();
            strResult = bRet ? mink.SumResult.ToString() : "";
            return bRet;
        }
        #endregion
    }
}

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值