C# Polygon.DoesIntersect方法代码示例

        private List<Obstacle> GetPerimeterObstacles()
        {
            // transform the polygon to relative coordinates
            AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer();

            Polygon relPerimeter = zonePerimeter.Transform(absTransform);
            LinePath relRecommendedPath = recommendedPath.Transform(absTransform);

            if (relPerimeter.IsCounterClockwise) {
                relPerimeter = relPerimeter.Reverse();
            }

            // create a polygon for ourselves and see if we intersect any perimeters
            Polygon vehiclePoly = new Polygon();
            vehiclePoly.Add(new Coordinates(-TahoeParams.RL, -(TahoeParams.T/2.0)));
            vehiclePoly.Add(new Coordinates(TahoeParams.FL, -(TahoeParams.T/2.0)));
            vehiclePoly.Add(new Coordinates(TahoeParams.FL, TahoeParams.T/2.0));
            vehiclePoly.Add(new Coordinates(-TahoeParams.RL, TahoeParams.T/2.0));
            // inflate by about 2 m
            vehiclePoly = vehiclePoly.Inflate(2);

            // test if we intersect any of the perimeter points
            List<Obstacle> perimeterObstacles = new List<Obstacle>();
            List<OperationalObstacle> operationalObstacles = new List<OperationalObstacle>();
            List<LineSegment> segments = new List<LineSegment>();
            foreach (LineSegment ls1 in relPerimeter.GetSegmentEnumerator()) {
                segments.Clear();
                if (ls1.Length > 15) {
                    // split into multiple segment
                    double targetLength = 10;
                    int numSegments = (int)Math.Round(ls1.Length/targetLength);
                    double splitLength = ls1.Length/numSegments;

                    Coordinates pt = ls1.P0;
                    for (int i = 0; i < numSegments; i++) {
                        Coordinates endPoint = pt + ls1.Vector.Normalize(splitLength);
                        LineSegment seg = new LineSegment(pt, endPoint);
                        segments.Add(seg);
                        pt = endPoint;
                    }
                }
                else {
                    segments.Add(ls1);
                }

                foreach (LineSegment ls in segments) {
                    bool pathTooClose = false;

                    foreach (Coordinates pt in relRecommendedPath) {
                        Coordinates closest = ls.ClosestPoint(pt);
                        if (closest.DistanceTo(pt) < 1)
                            pathTooClose = true;
                    }

                    if (!vehiclePoly.DoesIntersect(ls) && !pathTooClose) {
                        Obstacle obs = CreatePerimeterObstacle(ls);
                        perimeterObstacles.Add(obs);

                        OperationalObstacle uiobs = new OperationalObstacle();
                        uiobs.age = obs.age;
                        uiobs.heading = 0;
                        uiobs.headingValid = false;
                        uiobs.ignored = false;
                        uiobs.obstacleClass = obs.obstacleClass;
                        uiobs.poly = obs.obstaclePolygon;

                        operationalObstacles.Add(uiobs);
                    }
                }
            }

            Services.UIService.PushObstacles(operationalObstacles.ToArray(), curTimestamp, "perimeter obstacles", true);

            return perimeterObstacles;
        }

        /// <summary>
        /// Does the actuall splitting of each item. split needs to apply
        /// splitHelp to each individual item so easier to have a helper method.
        /// </summary>
        /// <param name="field">Shape of new Field</param>
        /// <param name="item">Specific item it is splitting</param>
        /// <returns>List of Polygons</returns>
        private List<Polygon> splitHelp(Polygon field, Polygon item)
        {
            List<Polygon> sub = new List<Polygon>();
            Polygon itemMini = mainField.CreateThickItem(item, -0.00001);
            for (int i = 0; i < item.Count; i++)
            {
                Vector2[] interV = new Vector2[0];

                bool A = field.IsInside(itemMini[i]);
                DEASL.Core.Mathematics.Shapes.LineSegment current = new DEASL.Core.Mathematics.Shapes.LineSegment();
                DEASL.Core.Mathematics.Shapes.LineSegment currentMini =
                    new DEASL.Core.Mathematics.Shapes.LineSegment(itemMini[(i + 1) % item.Count], itemMini[i]);
                bool B = field.DoesIntersect(currentMini);
                if (A)
                {
                    addCorrect(sub, new Vector2[] {item[i]}, field);
                }
                bool interDifferent = false;
                if (B)
                {
                    current = new DEASL.Core.Mathematics.Shapes.LineSegment(item[(i + 1) % item.Count], item[i]);
                    field.Intersect(current, out interV);
                }
                if (B && A)
                {
                    foreach (Vector2 v in interV)
                    {
                        interDifferent |= item[i].DistanceTo(v) < 0.0001 ||
                            item[(i + 1) % item.Count].DistanceTo(v) < 0.0001;
                    }
                }
                if (B && ! interDifferent)
                {
                    if (interV.Length > 1 && !current.UnitVector.ApproxEquals(new
                        DEASL.Core.Mathematics.Shapes.LineSegment(
                        interV[interV.Length - 1],interV[0]).UnitVector, 0.00001))
                    {
                        interV = reverse(interV);
                    }
                    addCorrect(sub, interV, field);
                }
            }
            return sub;
        }

        /// <summary>
        /// Adds the vectors in the correct order and to the correct polygon in addTo. Called from addCorrect.
        /// </summary>
        /// <param name="addTo">List of polygons that are collecting the split parts</param>
        /// <param name="addFrom">List of all vectors to add to polygon</param>
        /// <param name="field">Field shape</param>
        private void addCorrectHelp(List<Polygon> addTo, Vector2 addFrom, Polygon field)
        {
            bool addNew = true;
            for (int i = 0; i < addTo.Count; i++)
            {
                List<Polygon> tempPL = addTo.Select(p => new Polygon(p.points)).ToList();
                addNew = false;

                tempPL[i].Add(addFrom);
                if (tempPL[i].Count == 2)
                {
                    Vector2 v = tempPL[i][1] - tempPL[i][0];
                    DEASL.Core.Mathematics.Shapes.LineSegment line = new DEASL.Core.Mathematics.Shapes.
                        LineSegment(tempPL[i][1] - 0.00001 * v, tempPL[i][0] + 0.00001 * v);
                    if (field.IsInside(line))
                    {
                        addTo[i].Add(addFrom);
                        break;
                    }
                }
                else
                {
                    tempPL[i] = mainField.CreateThickItem(tempPL[i], -0.00001);
                    bool noIntersect = true;
                    for(int j = 0; j < tempPL[i].Count; j++)
                    {
                        DEASL.Core.Mathematics.Shapes.LineSegment line = new DEASL.Core.Mathematics.Shapes.
                        LineSegment(tempPL[i][j], tempPL[i][(j + 1) % tempPL[i].Count]);
                        if (field.DoesIntersect(line))
                        {
                            noIntersect = false;
                            break;
                        }
                    }
                    if (noIntersect && field.IsInside(tempPL[i]))
                    {
                        addTo[i].Add(addFrom);
                        break;
                    }
                }
                addNew = true;
            }
            if (addNew)
            {
                addTo.Add(new Polygon(new Vector2[] { addFrom }));
            }
        }

 /// <summary>
 /// Helper function to triangulate.
 /// </summary>
 /// <param name="poly">Polygon to breakdown</param>
 /// <param name="tris">List of triangles</param>
 /// <returns>List of triangles</returns>
 private List<Polygon> triangulateHelper(Polygon poly, List<Polygon> tris)
 {
     Polygon p = new Polygon(poly);
     if (p.Count <= 3)
     {
         tris.Add(p);
         return tris;
     }
     else
     {
         for (int i = 0; i < p.Count; i++)
         {
             Vector2 v = p[i+2] - p[i];
             DEASL.Core.Mathematics.Shapes.LineSegment ls = new DEASL.Core.Mathematics.Shapes.LineSegment(
                 p[i]+0.00001*v, p[i + 2]-0.00001*v);
             if(!p.DoesIntersect(ls) && p.IsInside(ls))
             {
                 tris.Add(new Polygon(new Vector2[] {p[i], p[i+1], p[i+2]}));
                 p.RemoveAt(i+1);
                 break;
             }
         }
         return triangulateHelper(p, tris);
     }
 }