C# Line.Intersect方法代码示例

        public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
        {
            List<Coordinates> polyPoints = new List<Coordinates>();
            try
            {
                // width
                double width = 3.0;
                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }
                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
                    width = width < aw.Lane.Width ? aw.Lane.Width : width;
                }

                if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
                    ai.TurnDirection == ArbiterTurnDirection.Straight ||
                    !(ai.InitialGeneric is ArbiterWaypoint) ||
                    !(ai.FinalGeneric is ArbiterWaypoint))
                {
                    LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
                    LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
                    polyPoints.AddRange(lp);
                    polyPoints.AddRange(rp);
                    ai.TurnPolygon = Polygon.GrahamScan(polyPoints);

                    if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
                    {
                        List<Coordinates> updatedPts = new List<Coordinates>();
                        LinePath interTmp = ai.InterconnectPath.Clone();
                        Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
                        interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
                        interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
                        lp = interTmp.ShiftLateral(TahoeParams.VL);
                        rp = interTmp.ShiftLateral(-TahoeParams.VL);
                        updatedPts.AddRange(lp);
                        updatedPts.AddRange(rp);
                        ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
                    }
                }
                else
                {
                    // polygon points
                    List<Coordinates> interPoints = new List<Coordinates>();

                    // waypoint
                    ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
                    ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;

                    // left and right path
                    LinePath leftPath = new LinePath();
                    LinePath rightPath = new LinePath();

                    // some initial points
                    LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
                    LinePath il = initialPath.ShiftLateral(width / 2.0);
                    LinePath ir = initialPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(il[1]);
                    rightPath.Add(ir[1]);

                    // some final points
                    LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
                    LinePath fl = finalPath.ShiftLateral(width / 2.0);
                    LinePath fr = finalPath.ShiftLateral(-width / 2.0);
                    leftPath.Add(fl[0]);
                    rightPath.Add(fr[0]);

                    // initial and final paths
                    Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
                    Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);

                    // get where the paths intersect and vector to normal path
                    Coordinates c;
                    iPath.Intersect(fPath, out c);
                    Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
                    Coordinates center = c + vector.Normalize((vector.Length / 2.0));

                    // get width expansion
                    Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
                    double iRot = -iVec.ArcTan;
                    Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    double fDeg = fVec.ToDegrees();
                    double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
                    double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;

                    // get inner point (small scale)
                    Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);

                    // get outer
                    Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);

                    if (ai.TurnDirection == ArbiterTurnDirection.Right)
                    {
                        rightPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = rightPath;
                        leftPath.Reverse();
//.........这里部分代码省略.........

        /// <summary>
        /// Check for all waypoints who have exit interconnects that overlaps input and no stop
        /// </summary>
        /// <param name="exits"></param>
        /// <param name="ai"></param>
        /// <returns></returns>
        private List<IntersectionInvolved> nonStopOverlaps(IEnumerable<ITraversableWaypoint> exits, ArbiterInterconnect ai)
        {
            // list of exits that have an interconnect which overlaps the interconnect input
            List<IntersectionInvolved> nonStopOverlapWaypoints = new List<IntersectionInvolved>();

            // get line of the interconnect
            Line aiLine = new Line(ai.InitialGeneric.Position, ai.FinalGeneric.Position);

            // loop over all exits
            foreach (ITraversableWaypoint exit in exits)
            {
                // make sure not our exit and the exit is not a stop and if exit and other are both waypoints then ways not the same
                if (!exit.Equals(ai.InitialGeneric) && !exit.IsStop &&
                    ((!(ai.InitialGeneric is ArbiterWaypoint) || !(exit is ArbiterWaypoint))
                    || !((ArbiterWaypoint)ai.InitialGeneric).Lane.Way.Equals(((ArbiterWaypoint)exit).Lane.Way)))
                {
                    // get all interconnects of the exit
                    foreach (ArbiterInterconnect tmp in exit.Exits)
                    {
                        // check relative priority that these are equal or lesser priority
                        if (ai.ComparePriority(tmp) != -1)
                        {
                            // simple check if the interconnect's final is same as input final
                            if (tmp.FinalGeneric.Equals(ai.FinalGeneric))
                            {
                                // check not already added
                                if (!nonStopOverlapWaypoints.Contains(new IntersectionInvolved(((ITraversableWaypoint)tmp.FinalGeneric).VehicleArea)))
                                {
                                    // add exit
                                    nonStopOverlapWaypoints.Add(new IntersectionInvolved(exit, exit.VehicleArea, tmp.TurnDirection));
                                }
                            }
                            // otherwise check overlap of interconnects
                            else
                            {
                                // get line of tmp interconnect
                                Line tmpLine = new Line(tmp.InitialGeneric.Position, tmp.FinalGeneric.Position);

                                // position of cross
                                Coordinates intersectionPoint;

                                // check intersection
                                bool intersects = aiLine.Intersect(tmpLine, out intersectionPoint) && ai.IsInside(intersectionPoint);
                                if (intersects)
                                {
                                    // check not already added
                                    if (!nonStopOverlapWaypoints.Contains(new IntersectionInvolved(((ITraversableWaypoint)tmp.FinalGeneric).VehicleArea)))
                                    {
                                        // add exit
                                        nonStopOverlapWaypoints.Add(new IntersectionInvolved(exit, exit.VehicleArea, tmp.TurnDirection));
                                    }
                                }
                            }
                        }
                    }
                }
            }

            return nonStopOverlapWaypoints;
        }

        protected void GetIntersectionPullPath(LinePath startingPath, LinePath endingPath, Polygon intersectionPolygon, bool addStartingPoint, bool addEndingPoint, LinePath targetPath, ref double pullWeight)
        {
            double angle = Math.Acos(startingPath.EndSegment.UnitVector.Dot(endingPath.GetSegment(0).UnitVector));

            // get the centroid of the intersection
            Coordinates centroid;

            // check if the angle is great than an threshold
            if (angle > 10*Math.PI/180.0) {
                // intersect the two lines formed by the starting and ending lanes
                Line startingLaneLine = new Line(startingPath[startingPath.Count-2], startingPath[startingPath.Count-1]);
                Line endingLaneLine = new Line(endingPath[1], endingPath[0]);

                // intersect them stuff and see if the point of intersection is between the two lines
                Coordinates K;
                if (!startingLaneLine.Intersect(endingLaneLine, out centroid, out K) || K.X <= 0 || K.Y <= 0)
                    return;
            }
            else {
                // if there is no intersection polygon, there isn't much we can do
                if (intersectionPolygon == null || intersectionPolygon.Count < 3) {
                    return;
                }

                centroid = intersectionPolygon.GetCentroid();
            }

            // calculate the pull weighting dependent on angle of intersection
            // angle 0 -> 0 weighting
            // angle 45 -> 0.00025 weighting
            // angle 90 -> 0.001 weighting
            pullWeight = Math.Pow(angle/(Math.PI/2), 2)*0.001;

            // get the relative transform from the behavior timestamp to the current timestamp
            RelativeTransform transform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
            centroid = transform.TransformPoint(centroid);

            if (addStartingPoint) {
                targetPath.Add(startingPath.EndPoint.Location);
            }
            // add the line from exit -> centroid (assuming that exit is already in the target path)
            targetPath.Add(centroid);
            if (addEndingPoint) {
                // add the line from centroid -> entrance
                targetPath.Add(endingPath[0]);
            }

            Services.UIService.PushLineList(targetPath, curTimestamp, "intersection path", true);
            Services.Dataset.ItemAs<double>("intersection weight").Add(pullWeight, curTimestamp);
        }