C# Polygon.IsInside方法代码示例

        public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
        {
            Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);
            vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
            List<Coordinates> pointsOutside = new List<Coordinates>();
            ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition);

            foreach(Coordinates c in vehiclePoly)
            {
                if (!p.IsInside(c))
                    pointsOutside.Add(c);
            }

            foreach (Coordinates m in pointsOutside)
            {
                foreach (Coordinates n in pointsOutside)
                {
                    if(!m.Equals(n))
                    {
                        if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
                            GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
                        {
                            return false;
                        }
                    }
                }
            }

            return true;
        }

        /// <summary>
        /// Gets all the incoming points to the intersection on each lane
        /// </summary>
        /// <param name="p"></param>
        /// <returns></returns>
        private Dictionary<ArbiterLane, LinePath.PointOnPath> DetermineIncoming(Polygon p)
        {
            Dictionary<ArbiterLane, LinePath.PointOnPath> incomingPoints =
                new Dictionary<ArbiterLane,LinePath.PointOnPath>();

            foreach (ArbiterInterconnect ai in arn.ArbiterInterconnects.Values)
            {
                if (ai.InitialGeneric is ArbiterWaypoint && !incomingPoints.ContainsKey(((ArbiterWaypoint)ai.InitialGeneric).Lane))
                {
                    if (p.IsInside(ai.InitialGeneric.Position))
                    {
                        ArbiterLane al = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
                        incomingPoints.Add(al, al.GetClosestPoint(ai.InitialGeneric.Position));
                    }
                }

                if (ai.FinalGeneric is ArbiterWaypoint && !incomingPoints.ContainsKey(((ArbiterWaypoint)ai.FinalGeneric).Lane))
                {
                    if (p.IsInside(ai.FinalGeneric.Position))
                    {
                        ArbiterLane al = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
                        incomingPoints.Add(al, al.GetClosestPoint(ai.FinalGeneric.Position));
                    }
                }
            }

            foreach (ArbiterSafetyZone asz in arn.ArbiterSafetyZones)
            {
                if (p.IsInside(asz.lane.LanePath().GetPoint(asz.safetyZoneEnd)))
                {
                    if (!incomingPoints.ContainsKey(asz.lane))
                        incomingPoints.Add(asz.lane, asz.safetyZoneEnd);
                }
            }

            foreach (IArbiterWaypoint iaw in arn.ArbiterWaypoints.Values)
            {
                if (iaw is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)iaw;

                    if (!incomingPoints.ContainsKey(aw.Lane) && p.IsInside(aw.Position))
                    {
                        incomingPoints.Add(aw.Lane, aw.Lane.GetClosestPoint(aw.Position));
                    }
                }
            }

            return incomingPoints;
        }

        public bool VehicleExistsInsidePolygon(VehicleAgent va, Polygon p, VehicleState ourState)
        {
            for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
            {
                Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
                if (p.IsInside(c))
                    return true;
            }

            return false;
        }

//.........这里部分代码省略.........
            {
                // list of replacements
                List<LinePath> toReplace = new List<LinePath>();

                // loop through outer
                foreach (LinePath edge in polyEdges)
                {
                    // flag to replace
                    bool replace = false;

                    // make sure this goes to a valid edge section
                    LinePath.PointOnPath closest = edge.GetClosestPoint(inner);
                    if (!closest.Equals(edge.StartPoint) && !closest.Equals(edge.EndPoint) &&
                        !(closest.Location.DistanceTo(edge.StartPoint.Location)  < 0.5) &&
                        !(closest.Location.DistanceTo(edge.EndPoint.Location)  < 0.5))
                    {
                        // create seg (extend a bit)
                        Coordinates expansion = closest.Location - inner;
                        LinePath seg = new LinePath(new Coordinates[] { inner, closest.Location + expansion.Normalize(1.0) });

                        // set flag
                        replace = true;

                        // loop through other edges
                        foreach (LinePath otherEdge in other)
                        {
                            double x1 = seg[0].X;
                            double y1 = seg[0].Y;
                            double x2 = seg[1].X;
                            double y2 = seg[1].Y;
                            double x3 = otherEdge[0].X;
                            double y3 = otherEdge[0].Y;
                            double x4 = otherEdge[1].X;
                            double y4 = otherEdge[1].Y;

                            // get if inside both
                            double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
                            double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));

                            if (0.01 < ua && ua < 0.99 && 0 < ub && ub < 1)
                            {
                                replace = false;
                            }
                        }
                    }

                    // check if should replace
                    if (replace)
                    {
                        // add analyzed to adjacent
                        toReplace.Add(edge);
                    }
                }

                // loop through replacements
                foreach (LinePath ll in toReplace)
                {
                    LinePath[] tmpArrayPoly = new LinePath[polyEdges.Count];
                    polyEdges.CopyTo(tmpArrayPoly);
                    List<LinePath> tmpPoly = new List<LinePath>(tmpArrayPoly);

                    // get index of edge
                    int index = tmpPoly.IndexOf(ll);

                    // remove
                    tmpPoly.RemoveAt(index);

                    // add correctly to outer
                    tmpPoly.Insert(index, new LinePath(new Coordinates[] { ll[0], inner }));
                    tmpPoly.Insert(index + 1, new LinePath(new Coordinates[] { inner, ll[1] }));

                    // poly
                    Polygon temp = new Polygon();
                    foreach(LinePath lpTemp in tmpPoly)
                        temp.Add(lpTemp[1]);
                    temp.Inflate(0.5);

                    // make sure none of original outside
                    bool ok = true;
                    foreach (LinePath lp in other)
                    {
                        if (!temp.IsInside(lp[1]) && !temp.Contains(lp[1]))
                            ok = false;
                    }

                    // set if created ok
                    if (ok)
                        polyEdges = tmpPoly;
                }
            }

            // create final
            List<Coordinates> finalPoly = new List<Coordinates>();
            foreach (LinePath outerEdge in polyEdges)
                finalPoly.Add(outerEdge[1]);
            interPoly = new Polygon(finalPoly);

            aInt.IntersectionPolygon = interPoly;
            aInt.Center = interPoly.GetCentroid();
        }

        /// <summary>
        /// Creates intersection polygon from input quare polygon
        /// </summary>
        /// <param name="square"></param>
        /// <returns></returns>
        private Polygon CreateIntersectionPolygon(Polygon square)
        {
            // coordinates we hold inside of the square represented by p0 - p3
            List<Coordinates> cSq = new List<Coordinates>();

            // check waypoints
            foreach (IArbiterWaypoint iaw in arn.ArbiterWaypoints.Values)
            {
                if (square.IsInside(iaw.Position) && !cSq.Contains(iaw.Position))
                {
                    cSq.Add(iaw.Position);
                }
            }

            // check safety areas
            foreach (ArbiterSafetyZone asz in arn.ArbiterSafetyZones)
            {
                if (square.IsInside(asz.lane.LanePath().GetPoint(asz.safetyZoneEnd)) && !asz.isExit && !cSq.Contains(asz.lane.LanePath().GetPoint(asz.safetyZoneEnd)))
                {
                    cSq.Add(asz.lane.LanePath().GetPoint(asz.safetyZoneEnd));
                }
            }

            // check wrapping helpers
            foreach (Coordinates c in this.WrappingHelpers)
            {
                // check if the coordinate is inside the square
                if (square.IsInside(c) && !cSq.Contains(c))
                {
                    cSq.Add(c);
                }
            }

            // check num of pts
            if (cSq.Count >= 3)
            {
                // wrap the polygon
                Polygon interPoly = Polygon.GrahamScan(cSq);

                // return
                return interPoly;
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// gets entries within polygon
        /// </summary>
        /// <param name="p"></param>
        /// <returns></returns>
        private Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> IntersectionEntries(Polygon p)
        {
            Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> entries = new Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint>();

            foreach (IArbiterWaypoint iaw in arn.ArbiterWaypoints.Values)
            {
                if (iaw is ITraversableWaypoint)
                {
                    ITraversableWaypoint aw = (ITraversableWaypoint)iaw;

                    if (aw.IsEntry && p.IsInside(aw.Position))
                    {
                        entries.Add(aw.AreaSubtypeWaypointId, aw);
                    }
                }
            }

            return entries;
        }

        /// <summary>
        /// Gets exits within polygon
        /// </summary>
        /// <param name="p"></param>
        /// <returns></returns>
        private Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> IntersectionExits(Polygon p)
        {
            Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> aws = new Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint>();

            foreach (IArbiterWaypoint iaw in arn.ArbiterWaypoints.Values)
            {
                if (iaw is ITraversableWaypoint)
                {
                    ITraversableWaypoint itw = (ITraversableWaypoint)iaw;

                    if (itw.IsExit && p.IsInside(iaw.Position))
                    {
                        aws.Add(iaw.AreaSubtypeWaypointId, itw);
                    }
                }
            }

            return aws;
        }

        /// <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 }));
            }
        }

        private double CheckFeelerDist(bool reverse, List<Obstacle> obstacles)
        {
            // create a "feeler box" of 3 m in front of the vehicle

            AbsolutePose pose = Services.StateProvider.GetAbsolutePose();

            Polygon feelerPoly = new Polygon(4);
            if (!reverse) {
                feelerPoly.Add(new Coordinates(TahoeParams.FL, (TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(TahoeParams.FL, -(TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(TahoeParams.FL + 2, -(TahoeParams.T/2.0 + 0.25)));
                feelerPoly.Add(new Coordinates(TahoeParams.FL + 2, (TahoeParams.T/2.0 + 0.25)));
            }
            else {
                feelerPoly.Add(new Coordinates(-TahoeParams.RL, -(TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(-TahoeParams.RL, (TahoeParams.T/2.0 + 0.5)));
                feelerPoly.Add(new Coordinates(-TahoeParams.RL - 2, (TahoeParams.T/2.0 + 0.25)));
                feelerPoly.Add(new Coordinates(-TahoeParams.RL - 2, -(TahoeParams.T/2.0 + 0.25)));
            }

            Matrix3 transform = Matrix3.Translation(pose.xy.X, pose.xy.Y)*Matrix3.Rotation(pose.heading);
            feelerPoly = feelerPoly.Transform(transform);

            double minDist = double.MaxValue;

            foreach (Obstacle obs in obstacles) {
                foreach (Coordinates pt in obs.AvoidancePolygon) {
                    if (feelerPoly.IsInside(pt)) {
                        double dist = pose.xy.DistanceTo(pt);
                        if (dist < minDist) {
                            minDist = dist;
                        }
                    }
                }
            }

            if (reverse) {
                return minDist - TahoeParams.RL;
            }
            else {
                return minDist - TahoeParams.FL;
            }
        }

        public void PolygonIsInsideReturnsTrueForPointOutside()
        {
            PointGeo p1 = new PointGeo(15, 10, 0);
            PointGeo p2 = new PointGeo(15, -10, 0);
            PointGeo p3 = new PointGeo(-15, -10, 0);
            PointGeo p4 = new PointGeo(-15, 10, 0);

            PointGeo pTest = new PointGeo(-20, 15, 0);

            Polygon<PointGeo> target = new Polygon<PointGeo>(new IPointGeo[] { p1, p2, p3, p4 });

            Assert.False(target.IsInside(pTest));
        }

 /// <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);
     }
 }

        public bool VehiclePassableInPolygon(ArbiterLane al, Polygon p, VehicleState ourState, Polygon circ)
        {
            List<Coordinates> vhcCoords = new List<Coordinates>();
            for (int i = 0; i < this.trackedCluster.relativePoints.Length; i++)
                vhcCoords.Add(this.TransformCoordAbs(this.trackedCluster.relativePoints[i], ourState));
            Polygon vehiclePoly = Polygon.GrahamScan(vhcCoords);
            vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
            ArbiterLanePartition alp = al.GetClosestPartition(this.trackedCluster.closestPoint);
            List<Coordinates> pointsOutside = new List<Coordinates>();

            foreach (Coordinates c in vehiclePoly)
            {
                if (!p.IsInside(c))
                    pointsOutside.Add(c);
            }

            foreach (Coordinates m in pointsOutside)
            {
                foreach (Coordinates n in pointsOutside)
                {
                    if (!m.Equals(n))
                    {
                        if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
                            GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
                        {
                            return false;
                        }
                    }
                }
            }

            return true;
        }

        public bool VehicleExistsInsidePolygon(Polygon p, VehicleState ourState)
        {
            for (int i = 0; i < this.trackedCluster.relativePoints.Length; i++)
            {
                Coordinates c = this.TransformCoordAbs(this.trackedCluster.relativePoints[i], ourState);
                if (p.IsInside(c))
                    return true;
            }

            return false;
        }

 // returns true if the line segment is not completely inside of the perimeter or if it touches one of the obstacles
 private static bool ExpansionViolatesConstraints(LineSegment ls, Polygon pcPerimeter, List<Polygon> pcObstacles)
 {
     if (!pcPerimeter.IsInside(ls)) return true;
       for (int i = 0; i < pcObstacles.Count; i++)
       {
     if (pcObstacles[i].DoesIntersect(ls))
     {
       return true;
     }
       }
       return false;
 }