public Polygon DefaultPoly()
        {
            ArbiterInterconnect ai = this;

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

            List <Coordinates> polyPoints = new List <Coordinates>();
            LinePath           lp         = ai.InterconnectPath.ShiftLateral(width / 2.0);
            LinePath           rp         = ai.InterconnectPath.ShiftLateral(-width / 2.0);

            polyPoints.AddRange(lp);
            polyPoints.AddRange(rp);
            return(Polygon.GrahamScan(polyPoints));
        }
        /// <summary>
        /// Gets edge going to input node from this exit
        /// </summary>
        /// <param name="node"></param>
        /// <returns></returns>
        public ArbiterInterconnect GetEdge(INavigableNode node)
        {
            foreach (NavigableEdge ne in exit.OutgoingConnections)
            {
                if (ne is ArbiterInterconnect)
                {
                    ArbiterInterconnect ai = (ArbiterInterconnect)ne;

                    if (ai.FinalGeneric.Equals(node))
                    {
                        return(ai);
                    }
                }
            }

            return(null);
        }
        /// <summary>
        /// Get interconnect closest to our position and orientation
        /// </summary>
        /// <param name="point"></param>
        /// <param name="heading"></param>
        /// <returns></returns>
        public ArbiterInterconnect ClosestInterconnect(Coordinates point, Coordinates heading)
        {
            ArbiterInterconnect closest = null;
            double best = Double.MaxValue;

            foreach (ArbiterInterconnect ai in this.ArbiterInterconnects.Values)
            {
                // get closest
                double distance = ai.InterconnectPath.GetPoint(ai.InterconnectPath.GetClosestPoint(point)).DistanceTo(point);

                // get heading of interconnect approximately
                Coordinates interHeading = ai.FinalGeneric.Position - ai.InitialGeneric.Position;

                if (distance < best && Math.Abs(interHeading.ToDegrees() - heading.ToDegrees()) < 45.0)
                {
                    best    = distance;
                    closest = ai;
                }
            }

            return(closest);
        }
Пример #4
0
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="initial"></param>
        /// <param name="next"></param>
        /// <param name="final"></param>
        public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
        {
            Components = new SupraLaneComponentList();
            Components.Add(initial);
            Components.Add(next);
            Components.Add(final);
            this.Initial      = initial;
            this.Final        = final;
            this.Interconnect = next;

            if (!this.Initial.Equals(this.Final))
            {
                this.supraWaypoints = new List <ArbiterWaypoint>();
                this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
                this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
                this.SetDefaultLanePath();
            }
            else
            {
                this.supraWaypoints = this.Initial.WaypointList;
                this.supraPath      = this.Initial.LanePath();
                this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
            }
        }
Пример #5
0
        /// <summary>
        /// Generates interconnects into the road network
        /// </summary>
        /// <param name="arn"></param>
        /// <returns></returns>
        public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn)
        {
            // list of all exit entries in the xy rndf
            List <SimpleExitEntry> sees = new List <SimpleExitEntry>();

            // zones
            if (xyRndf.Zones != null)
            {
                // loop over zones
                foreach (SimpleZone sz in xyRndf.Zones)
                {
                    // add all ee's
                    sees.AddRange(sz.Perimeter.ExitEntries);
                }
            }

            // segments
            if (xyRndf.Segments != null)
            {
                // loop over segments
                foreach (SimpleSegment ss in xyRndf.Segments)
                {
                    // lanes
                    foreach (SimpleLane sl in ss.Lanes)
                    {
                        // add all ee's
                        sees.AddRange(sl.ExitEntries);
                    }
                }
            }

            // loop over ee's and create interconnects
            foreach (SimpleExitEntry see in sees)
            {
                IArbiterWaypoint    initial = arn.LegacyWaypointLookup[see.ExitId];
                IArbiterWaypoint    final   = arn.LegacyWaypointLookup[see.EntryId];
                ArbiterInterconnect ai      = new ArbiterInterconnect(initial, final);
                arn.ArbiterInterconnects.Add(ai.InterconnectId, ai);
                arn.DisplayObjects.Add(ai);

                if (initial is ITraversableWaypoint)
                {
                    ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial;

                    initialWaypoint.IsExit = true;

                    if (initialWaypoint.Exits == null)
                    {
                        initialWaypoint.Exits = new List <ArbiterInterconnect>();
                    }

                    initialWaypoint.Exits.Add(ai);
                }
                else
                {
                    throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint");
                }

                if (final is ITraversableWaypoint)
                {
                    ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final;

                    finalWaypoint.IsEntry = true;

                    if (finalWaypoint.Entries == null)
                    {
                        finalWaypoint.Entries = new List <ArbiterInterconnect>();
                    }

                    finalWaypoint.Entries.Add(ai);
                }
                else
                {
                    throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint");
                }

                // set the turn direction
                this.SetTurnDirection(ai);

                // interconnectp olygon stuff
                this.GenerateInterconnectPolygon(ai);
                if (ai.TurnPolygon.IsComplex)
                {
                    Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString());
                    ai.TurnPolygon = ai.DefaultPoly();
                }
            }

            return(arn);
        }
Пример #6
0
        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();
                        leftPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                    else
                    {
                        leftPath.Insert(1, innerPoint);
                        ai.InnerCoordinates = leftPath;
                        rightPath.Reverse();
                        rightPath.Insert(1, outerPoint);
                        Polygon p = new Polygon(leftPath.ToArray());
                        p.AddRange(rightPath.ToArray());
                        ai.TurnPolygon = p;
                    }
                }
            }
            catch (Exception e)
            {
                Console.WriteLine("error generating turn polygon: " + ai.ToString());
                ai.TurnPolygon = ai.DefaultPoly();
            }
        }
Пример #7
0
        public void SetTurnDirection(ArbiterInterconnect ai)
        {
            #region Turn Direction

            if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
            {
                ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
                ArbiterWaypoint finWp  = (ArbiterWaypoint)ai.FinalGeneric;

                // check not uturn
                if (!initWp.Lane.Way.Segment.Equals(finWp.Lane.Way.Segment) || initWp.Lane.Way.Equals(finWp.Lane.Way))
                {
                    Coordinates iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);

                    double iRot = -iVec.ArcTan;

                    Coordinates fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.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;

                    if (arcTan > 45.0)
                    {
                        ai.TurnDirection = ArbiterTurnDirection.Left;
                    }
                    else if (arcTan < -45.0)
                    {
                        ai.TurnDirection = ArbiterTurnDirection.Right;
                    }
                    else
                    {
                        ai.TurnDirection = ArbiterTurnDirection.Straight;
                    }
                }
                else
                {
                    Coordinates iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);

                    double iRot = -iVec.ArcTan;

                    Coordinates fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.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;

                    if (arcTan > 45.0 && arcTan < 135.0)
                    {
                        ai.TurnDirection = ArbiterTurnDirection.Left;
                    }
                    else if (arcTan < -45.0 && arcTan > -135.0)
                    {
                        ai.TurnDirection = ArbiterTurnDirection.Right;
                    }
                    else if (Math.Abs(arcTan) < 45.0)
                    {
                        ai.TurnDirection = ArbiterTurnDirection.Straight;
                    }
                    else
                    {
                        ai.TurnDirection = ArbiterTurnDirection.UTurn;
                    }
                }
            }
            else
            {
                Coordinates iVec = new Coordinates();
                double      iRot = 0.0;
                Coordinates fVec = new Coordinates();
                double      fDeg = 0.0;

                if (ai.InitialGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric;
                    iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0);

                    iRot = -iVec.ArcTan;
                }
                else if (ai.InitialGeneric is ArbiterPerimeterWaypoint)
                {
                    ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.InitialGeneric;
                    Coordinates centerPoly       = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
                    iVec = apw.Position - centerPoly;
                    iVec = iVec.Normalize(1.0);
                    iRot = -iVec.ArcTan;
                }

                if (ai.FinalGeneric is ArbiterWaypoint)
                {
                    ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric;
                    fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0);

                    fVec = fVec.Rotate(iRot);
                    fDeg = fVec.ToDegrees();
                }
                else if (ai.FinalGeneric is ArbiterPerimeterWaypoint)
                {
                    ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
                    Coordinates centerPoly       = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center;
                    fVec = centerPoly - apw.Position;
                    fVec = fVec.Normalize(1.0);
                    fVec = fVec.Rotate(iRot);
                    fDeg = fVec.ToDegrees();
                }

                double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;

                if (arcTan > 45.0)
                {
                    ai.TurnDirection = ArbiterTurnDirection.Left;
                }
                else if (arcTan < -45.0)
                {
                    ai.TurnDirection = ArbiterTurnDirection.Right;
                }
                else
                {
                    ai.TurnDirection = ArbiterTurnDirection.Straight;
                }
            }

            #endregion
        }
        /// <summary>
        /// Generates the default time cost for the edge
        /// </summary>
        /// <returns></returns>
        public double CalculateDefaultTimeCost()
        {
            // add extra costs of the end of this edge
            double cost = End.ExtraTimeCost;

            if (IsZone)
            {
                // zone returns justdistance of edge divided by the zone's minimum speed
                cost += this.standardEdgeDistance / Math.Max(NavigationPenalties.ZoneMinSpeedDefault, this.Zone.SpeedLimits.MinimumSpeed);
                return(cost);
            }
            else if (IsSegment)
            {
                // default cost of segment edge
                cost += this.standardEdgeDistance / this.Segment.SpeedLimits.MaximumSpeed;

                // time costs of adjacent
                foreach (IConnectAreaWaypoints icaw in this.Contained)
                {
                    if (!icaw.Equals(this))
                    {
                        // add changing lanes cost
                        cost += NavigationPenalties.ChangeLanes;

                        if (icaw is NavigableEdge)
                        {
                            cost += ((NavigableEdge)icaw).TimeCost();
                        }
                    }
                }

                // interconnect if stop
                if (Contained.Count > 0 && Contained[0] is ArbiterLanePartition)
                {
                    ArbiterLanePartition alp = (ArbiterLanePartition)Contained[0];

                    if (alp.Type == PartitionType.Sparse)
                    {
                        cost += this.standardEdgeDistance * 2.0;
                    }

                    if (alp.Initial.IsStop)
                    {
                        ArbiterInterconnect ai = alp.ToInterconnect;
                        cost += ai.ExtraCost - NavigationPenalties.Interconnect;
                    }
                }

                // cost of partition by default
                return(cost);
            }
            else
            {
                // default cost
                cost += this.standardEdgeDistance / this.defaultSpeed;

                // interconnect
                if (Contained.Count > 0 && Contained[0] is ArbiterInterconnect)
                {
                    ArbiterInterconnect ai = (ArbiterInterconnect)Contained[0];
                    cost = this.standardEdgeDistance / ai.MaximumDefaultSpeed + ai.ExtraCost;
                }

                // return
                return(cost);
            }
        }
        public int ComparePriority(ArbiterInterconnect other)
        {
            if (this.initialWaypoint is ITraversableWaypoint && ((ITraversableWaypoint)initialWaypoint).IsStop)
            {
                if (other.InitialGeneric is ITraversableWaypoint && !((ITraversableWaypoint)other.InitialGeneric).IsStop)
                {
                    return(1);
                }
            }
            else
            {
                if (other.InitialGeneric is ITraversableWaypoint && ((ITraversableWaypoint)other.InitialGeneric).IsStop)
                {
                    return(-1);
                }
            }

            if (this.TurnDirection == ArbiterTurnDirection.Unknown || other.TurnDirection == ArbiterTurnDirection.Unknown)
            {
                return(0);
            }

            if (this.TurnDirection == ArbiterTurnDirection.Left)
            {
                if (other.TurnDirection == ArbiterTurnDirection.UTurn)
                {
                    return(-1);
                }
                if (other.TurnDirection == ArbiterTurnDirection.Left)
                {
                    return(0);
                }
                else
                {
                    return(1);
                }
            }
            else if (this.TurnDirection == ArbiterTurnDirection.Right)
            {
                if (other.TurnDirection == ArbiterTurnDirection.Right)
                {
                    return(0);
                }
                else if (other.TurnDirection == ArbiterTurnDirection.Straight)
                {
                    return(1);
                }
                else
                {
                    return(-1);
                }
            }
            else if (this.TurnDirection == ArbiterTurnDirection.Straight)
            {
                if (other.TurnDirection == ArbiterTurnDirection.Straight)
                {
                    return(0);
                }
                else
                {
                    return(-1);
                }
            }
            else if (this.TurnDirection == ArbiterTurnDirection.UTurn)
            {
                if (other.TurnDirection == ArbiterTurnDirection.UTurn)
                {
                    return(0);
                }
                else
                {
                    return(1);
                }
            }

            else
            {
                return(1);
            }
        }