C# Circle.ToPolygon方法代码示例

        /// <summary>
        /// Populates mapping of areas to vehicles
        /// </summary>
        public void PopulateAreaVehicles()
        {
            TacticalDirector.VehicleAreas = new Dictionary<IVehicleArea,List<VehicleAgent>>();
            VehicleState vs = CoreCommon.Communications.GetVehicleState();

            Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
            Polygon conv = circ.ToPolygon(32);

            Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
            Polygon conv1 = circ1.ToPolygon(32);

            Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
            Polygon conv2 = circ2.ToPolygon(32);

            foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
            {
                #region Standard Areas

                List<AreaProbability> AreaProbabilities = new List<AreaProbability>();

                #region Add up probablities

                for (int i = 0; i < va.StateMonitor.Observed.closestPartitions.Length; i++)
                {
                    SceneEstimatorClusterPartition secp = va.StateMonitor.Observed.closestPartitions[i];

                    if (CoreCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
                    {
                        IVehicleArea iva = CoreCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];

                        bool found = false;
                        for (int j = 0; j < AreaProbabilities.Count; j++)
                        {
                            AreaProbability ap = AreaProbabilities[j];

                            if (ap.Key.Equals(iva))
                            {
                                ap.Value = ap.Value + secp.probability;
                                found = true;
                            }
                        }

                        if (!found)
                        {
                            AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
                        }
                    }
                    else
                    {
                        ArbiterOutput.Output("Core Intelligence thread caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map");
                    }
                }

                #endregion

                #region Assign

                if (AreaProbabilities.Count > 0)
                {
                    double rP = 0.0;
                    foreach (AreaProbability ap in AreaProbabilities)
                    {
                        if (ap.Key is ArbiterLane)
                            rP += ap.Value;
                    }

                    if (rP > 0.1)
                    {
                        foreach (AreaProbability ap in AreaProbabilities)
                        {
                            if (ap.Key is ArbiterLane)
                            {
                                // get lane
                                ArbiterLane al = (ArbiterLane)ap.Key;

                                // probability says in area
                                double vP = ap.Value / rP;
                                if (vP > 0.3)
                                {
                                    #region Check if obstacle enough in area

                                    bool ok = false;
                                    if (ap.Key is ArbiterLane)
                                    {
                                        Coordinates closest = va.ClosestPointToLine(al.LanePath(), vs).Value;

                                        // dist to closest
                                        double distanceToClosest = vs.Front.DistanceTo(closest);

                                        // get our dist to closest
                                        if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value))))
                                        {
                                            if (al.LanePolygon != null)
                                                ok = this.VehicleExistsInsidePolygon(va, al.LanePolygon, vs);
                                            else
                                                ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
                                        }
//.........这里部分代码省略.........

        public static bool TestBlockage(IList<Polygon> obstaclePolygons, LinePath leftBound, LinePath rightBound, double expandDist, double trackWidth)
        {
            try
            {
                Circle robotCircle = new Circle(expandDist, Vector2.Zero);
                Polygon robotPoly = robotCircle.ToPolygon(24);

                List<BlockageData> obstacles = new List<BlockageData>(obstaclePolygons.Count);

                foreach (Polygon poly in obstaclePolygons)
                {
                    Polygon convolvedPoly = null;
                    try
                    {
                        convolvedPoly = Polygon.ConvexMinkowskiConvolution(robotPoly, poly);
                    }
                    catch (Exception)
                    {
                        // minkowski convolution failed, just expand that shit with the gaheezy inflate method
                        convolvedPoly = poly.Inflate(expandDist);
                    }

                    // add the entry to the obstacle collection
                    BlockageData data = new BlockageData(convolvedPoly);
                    obstacles.Add(data);
                }

                // shrink in the lanes by a half robot width
                leftBound = leftBound.ShiftLateral(-trackWidth / 2.0);
                rightBound = rightBound.ShiftLateral(-trackWidth / 2.0);

                Queue<BlockageData> testQueue = new Queue<BlockageData>();

                foreach (BlockageData obs in obstacles)
                {
                    if (obs.convolvedPolygon.DoesIntersect(leftBound))
                    {
                        // check if this hits the right bound
                        if (obs.convolvedPolygon.DoesIntersect(rightBound))
                        {
                            // this extends across the entire lane, the segment is blocked
                            return true;
                        }
                        else
                        {
                            testQueue.Enqueue(obs);
                            obs.searchMark = true;
                        }
                    }
                }

                while (testQueue.Count > 0)
                {
                    BlockageData obs = testQueue.Dequeue();

                    foreach (BlockageData neighbor in obstacles)
                    {
                        if (!neighbor.searchMark && Polygon.TestConvexIntersection(obs.convolvedPolygon, neighbor.convolvedPolygon))
                        {
                            if (neighbor.convolvedPolygon.DoesIntersect(rightBound))
                                return true;

                            testQueue.Enqueue(neighbor);
                            neighbor.searchMark = true;
                        }
                    }
                }

                return false;
            }
            catch (Exception) { }

            return false;
        }

//.........这里部分代码省略.........
                //    obs.obstacleClass = split.Key.obstacleClass;

                //    // don't bother doing a split operation on these clusters -- they have already been split
                //    obs.obstaclePolygon = Polygon.GrahamScan(split.Value);

                //    obstacles.Add(obs);
                //  }
                //}

                // handle the unclaimed points
                IList<Polygon> polygons = WrapAndSplit(unclaimed_points, split_area_threshold, split_length_threshold);

                foreach (Polygon poly in polygons) {
                    // create a new obstacle
                    Obstacle obs = new Obstacle();
                    obs.age = 1;
                    obs.obstacleClass = targetClass;

                    obs.obstaclePolygon = poly;

                    obstacles.Add(obs);
                }

            }

            // test all old obstacles and see if they intersect any new obstacles
            // project the previous static obstacles to the current time frame

            if (processedObstacles != null) {
                try {
                    // get the relative transform
                    List<Obstacle> carryOvers = new List<Obstacle>();
                    Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
                    Polygon mergePolygon = mergeCircle.ToPolygon(24);

                    RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, clusters.timestamp);
                    foreach (Obstacle prevObs in processedObstacles.obstacles) {
                        if (prevObs.obstacleClass == ObstacleClass.StaticLarge || prevObs.obstacleClass == ObstacleClass.StaticSmall) {
                            prevObs.obstaclePolygon = prevObs.obstaclePolygon.Transform(transform);
                            prevObs.age++;

                            if (prevObs.age < 20) {
                                Coordinates centroid = prevObs.obstaclePolygon.GetCentroid();
                                double dist = GetObstacleDistance(prevObs.obstaclePolygon);
                                double angle = centroid.ArcTan;
                                if (dist < 30 && dist > 6 && Math.Abs(centroid.Y) < 15 && Math.Abs(angle) < Math.PI/2.0) {
                                    try {
                                        prevObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, prevObs.obstaclePolygon);
                                        if (!TestIntersection(prevObs.mergePolygon, obstacles)) {
                                            bool dropObstacle = false;
                                            for (int i = 0; i < prevObs.obstaclePolygon.Count; i++) {
                                                Coordinates pt = prevObs.obstaclePolygon[i];
                                                // iterate over all tracked cluster

                                                if (vehicleBox.IsInside(pt)) {
                                                    dropObstacle = true;
                                                }
                                                else if (useOccupancyGrid && externalUseOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free) {
                                                  dropObstacle = true;
                                                }
                                                else if (trackedObstacles != null) {
                                                    foreach (Obstacle trackedObs in trackedObstacles) {
                                                        if (trackedObs.obstacleClass == ObstacleClass.DynamicCarlike) {
                                                            Polygon testPoly = trackedObs.extrudedPolygon ?? trackedObs.mergePolygon;

                                                            if (testPoly != null && testPoly.BoundingCircle.IsInside(pt) && testPoly.IsInside(pt)) {

        private List<Obstacle> ProcessTrackedClusters(SceneEstimatorTrackedClusterCollection clusters, Rect vehicleBox)
        {
            List<Obstacle> obstacles = new List<Obstacle>(clusters.clusters.Length);

            // get the list of previous id's
            SortedList<int, Obstacle> previousID;
            if (processedObstacles != null) {
                previousID = new SortedList<int, Obstacle>(processedObstacles.obstacles.Count);
                foreach (Obstacle obs in processedObstacles.obstacles) {
                    if (obs != null && obs.trackID != -1 && !previousID.ContainsKey(obs.trackID)) {
                        previousID.Add(obs.trackID, obs);
                    }
                }
            }
            else {
                previousID = new SortedList<int, Obstacle>();
            }

            List<Coordinates> goodPoints = new List<Coordinates>(1500);

            Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
            Polygon mergePolygon = mergeCircle.ToPolygon(24);

            foreach (SceneEstimatorTrackedCluster cluster in clusters.clusters) {
                // ignore deleted targets
                if (cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_DELETED || cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL || cluster.relativePoints == null || cluster.relativePoints.Length < 3)
                    continue;

                Obstacle obs = new Obstacle();

                obs.trackID = cluster.id;
                obs.speed = cluster.speed;
                obs.speedValid = cluster.speedValid;
                obs.occuluded = cluster.statusFlag != SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;

                // update the age
                Obstacle prevTrack = null;
                previousID.TryGetValue(cluster.id, out prevTrack);

                goodPoints.Clear();

                int numOccupancyDeleted = 0;
                foreach (Coordinates pt in cluster.relativePoints) {
                    if (!vehicleBox.IsInside(pt)) {
                        if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free) {
                            occupancyDeletedCount++;
                            numOccupancyDeleted++;
                        }
                        else {
                            goodPoints.Add(pt);
                        }
                    }
                }

                if (goodPoints.Count < 3) {
                    continue;
                }

                IList<Polygon> polys;
                if (obs.occuluded && numOccupancyDeleted > 0) {
                    polys = WrapAndSplit(goodPoints, 1, 2.5);
                }
                else {
                    polys = new Polygon[] { Polygon.GrahamScan(goodPoints) };
                }

                obs.absoluteHeadingValid = cluster.headingValid;
                obs.absoluteHeading = cluster.absoluteHeading;

                // set the obstacle polygon for calculate obstacle distance
                Polygon obsPoly = Polygon.GrahamScan(goodPoints);
                double targetDistance = GetObstacleDistance(obsPoly);

                ObstacleClass impliedClass = ObstacleClass.DynamicUnknown;
                switch (cluster.targetClass) {
                    case SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE:
                        if (cluster.isStopped) {
                            impliedClass = ObstacleClass.DynamicStopped;
                        }
                        else {
                            impliedClass = ObstacleClass.DynamicCarlike;
                        }
                        break;

                    case SceneEstimatorTargetClass.TARGET_CLASS_NOTCARLIKE:
                        impliedClass = ObstacleClass.DynamicNotCarlike;
                        break;

                    case SceneEstimatorTargetClass.TARGET_CLASS_UNKNOWN:
                        impliedClass = ObstacleClass.DynamicUnknown;
                        break;
                }

                if (prevTrack == null) {
                    obs.age = 1;
                    // we haven't seen this track before, determine what the implied class is
                    if (targetDistance < target_class_ignore_dist) {
                        impliedClass = ObstacleClass.DynamicUnknown;
                    }
                }
//.........这里部分代码省略.........

 private static Polygon MakeConvPolygon(double spacing)
 {
     Circle c = new Circle(TahoeParams.T/2.0 + spacing, Coordinates.Zero);
     return c.ToPolygon(24);
 }

        private Obstacle CreatePerimeterObstacle(LineSegment ls)
        {
            // perimeter points go clockwise, so shift this segment left to make it go outward
            LineSegment shifted = ls.ShiftLateral(1);

            // create a polygon for the obstacle
            Polygon obstaclePoly = new Polygon();
            obstaclePoly.Add(ls.P0);
            obstaclePoly.Add(ls.P1);
            obstaclePoly.Add(shifted.P1);
            obstaclePoly.Add(shifted.P0);

            Obstacle obs = new Obstacle();
            obs.obstaclePolygon = obstaclePoly;

            Circle tahoeCircle = new Circle(TahoeParams.T/2.0+0.1, Coordinates.Zero);
            Polygon tahoePoly = tahoeCircle.ToPolygon(24);
            obs.cspacePolygon = Polygon.ConvexMinkowskiConvolution(tahoePoly, obstaclePoly);

            obs.obstacleClass = ObstacleClass.StaticLarge;
            obs.desSpacing = 0.5;
            obs.minSpacing = 0.1;
            obs.age = 1;
            obs.trackID = -1;

            return obs;
        }

        public string PartitionIdString()
        {
            string final = "";

            #region Standard Areas

            List<AreaProbability> AreaProbabilities = new List<AreaProbability>();

            Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
            Polygon conv = circ.ToPolygon(32);

            Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
            Polygon conv1 = circ1.ToPolygon(32);

            Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
            Polygon conv2 = circ2.ToPolygon(32);

            for (int i = 0; i < this.trackedCluster.closestPartitions.Length; i++)
            {
                SceneEstimatorClusterPartition secp = this.trackedCluster.closestPartitions[i];

                if (RemoraCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
                {
                    IVehicleArea iva = RemoraCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];

                    bool found = false;
                    for (int j = 0; j < AreaProbabilities.Count; j++)
                    {
                        AreaProbability ap = AreaProbabilities[j];

                        if (ap.Key.Equals(iva))
                        {
                            ap.Value = ap.Value + secp.probability;
                            found = true;
                        }
                    }

                    if (!found)
                    {
                        AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
                    }
                }
                else
                {
                    RemoraOutput.WriteLine("Remora caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map", OutputType.Remora);
                }
            }

            if (AreaProbabilities.Count > 0)
            {
                double rP = 0.0;
                foreach (AreaProbability ap in AreaProbabilities)
                {
                    if (ap.Key is ArbiterLane)
                        rP += ap.Value;
                }

                if (rP > 0.1)
                {
                    foreach (AreaProbability ap in AreaProbabilities)
                    {
                        if (ap.Key is ArbiterLane)
                        {
                            // proabbility says in area
                            double vP = ap.Value / rP;
                            if (vP > 0.3)
                            {
                                #region Check if obstacle enough in area

                                bool ok = false;
                                if (ap.Key is ArbiterLane)
                                {
                                    VehicleState vs = RemoraCommon.Communicator.GetVehicleState();

                                    ArbiterLane al = (ArbiterLane)ap.Key;
                                    Coordinates closest = this.ClosestPointToLine(al.LanePath(), vs).Value;

                                    // dist to closest
                                    double distanceToClosest = vs.Front.DistanceTo(closest);

                                    // get our dist to closest
                                    if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(RemoraCommon.Communicator.GetVehicleSpeed().Value))))
                                    {
                                        if (al.LanePolygon != null)
                                            ok = this.VehicleExistsInsidePolygon(al.LanePolygon, vs);
                                        else
                                            ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
                                    }
                                    else if (distanceToClosest <= 30.0)
                                    {
                                        if (al.LanePolygon != null)
                                        {
                                            if (!this.trackedCluster.isStopped && this.VehicleAllInsidePolygon(al.LanePolygon, vs))
                                                ok = true;
                                            else
                                            {
                                                if (this.trackedCluster.isStopped)
                                                {
                                                    bool isInSafety = false;
                                                    foreach (ArbiterIntersection ai in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
//.........这里部分代码省略.........