/// <summary>
        /// Turn information
        /// </summary>
        /// <param name="entry"></param>
        /// <param name="finalPath"></param>
        /// <param name="leftBound"></param>
        /// <param name="rightBound"></param>
        public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
        {
            if (entry.NextPartition != null)
            {
                double distance = entry.NextPartition.Length;

                // get left bound
                rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
                                                            entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90());

                // get right bound
                leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
                                                           entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90());

                ArbiterWaypoint current = entry.NextPartition.Final;
                while (current.NextPartition != null && distance < 50)
                {
                    distance += current.NextPartition.Length;

                    LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
                                                                     current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90());
                    rightBound.Add(rtTemp[rtTemp.Count - 1]);

                    LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
                                                                     current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90());
                    leftBound.Add(ltTemp[ltTemp.Count - 1]);

                    current = current.NextPartition.Final;
                }

                finalPath = entry.Lane.LanePath(entry, 50.0);
            }
            else
            {
                Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL);
                finalPath = new LinePath(new Coordinates[] { entry.Position, final });
                LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0);
                LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0);
                leftBound  = new LineList(lB);
                rightBound = new LineList(rB);
            }
        }
        /// <summary>
        /// Generates the bounding waypoints of the acceptable U-Turn area given Rndf Hazards and specified exit and entry waypoints
        /// </summary>
        /// <returns></returns>
        public static Polygon uTurnBounds(Coordinates exit, ArbiterSegment segment)
        {
            // initialize the bounding box
            List <Coordinates> boundingBox = new List <Coordinates>();

            // put in coords for every available lane
            foreach (ArbiterLane al in segment.Lanes.Values)
            {
                PointOnPath?pop = null;

                if (!al.IsInside(exit))
                {
                    ArbiterWaypoint aw = al.GetClosestWaypoint(exit, 10.0);
                    if (aw != null)
                    {
                        pop = al.PartitionPath.GetClosest(aw.Position);
                    }
                }
                else
                {
                    pop = al.PartitionPath.GetClosest(exit);
                }

                if (pop != null)
                {
                    ArbiterLanePartition alp    = al.GetClosestPartition(exit);
                    Coordinates          vector = alp.Vector().Normalize(15);
                    Coordinates          back   = pop.Value.pt - vector;
                    vector = vector.Normalize(30);
                    boundingBox.AddRange(InflatePartition(back, vector, alp.Lane.Width));
                }
            }

            // return the box
            return(GeneralToolkit.JarvisMarch(boundingBox));
        }
Exemplo n.º 3
0
        public static Polygon PartitionPolygon(ArbiterLanePartition alp)
        {
            if (alp.Initial.PreviousPartition != null &&
                alp.Final.NextPartition != null &&
                alp.Length < 30.0 && alp.Length > 4.0)
            {
                // get partition turn direction
                ArbiterTurnDirection pTD = PartitionTurnDirection(alp);

                // check if angles of previous and next are such that not straight through
                if (pTD != ArbiterTurnDirection.Straight)
                {
                    // get partition poly
                    ArbiterInterconnect tmpAi = alp.ToInterconnect;
                    tmpAi.TurnDirection = pTD;
                    GenerateInterconnectPolygon(tmpAi);
                    Polygon pPoly = tmpAi.TurnPolygon;

                    // here is default partition polygon
                    LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0);
                    LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0);
                    alprb.Reverse();
                    List <Coordinates> alpdefaultPoly = alplb;
                    alpdefaultPoly.AddRange(alprb);

                    // get full poly
                    pPoly.AddRange(alpdefaultPoly);
                    pPoly = Polygon.GrahamScan(pPoly);

                    return(pPoly);
                }
            }
            else if (alp.Length >= 30)
            {
                Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width);

                if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15)
                {
                    // initial portion
                    Coordinates i1   = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0);
                    Coordinates i2   = alp.Initial.Position;
                    Coordinates i3   = i2 + alp.Vector().Normalize(15.0);
                    LinePath    il12 = new LinePath(new Coordinates[] { i1, i2 });
                    LinePath    il23 = new LinePath(new Coordinates[] { i2, i3 });
                    LinePath    il13 = new LinePath(new Coordinates[] { i1, i3 });
                    Coordinates iCC  = il13.GetClosestPoint(i2).Location;
                    if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
                    {
                        il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    else
                    {
                        il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
                    iCCP = il13.AdvancePoint(iCCP, -10.0);
                    il13 = il13.SubPath(iCCP, 20.0);
                    Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width);
                    iBase.Add(il13[1]);
                    Polygon iP = Polygon.GrahamScan(iBase);
                    pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
                }

                if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15)
                {
                    // initial portion
                    Coordinates i1   = alp.Final.Position - alp.Vector().Normalize(15.0);
                    Coordinates i2   = alp.Final.Position;
                    Coordinates i3   = i2 + alp.Final.NextPartition.Vector().Normalize(15.0);
                    LinePath    il12 = new LinePath(new Coordinates[] { i1, i2 });
                    LinePath    il23 = new LinePath(new Coordinates[] { i2, i3 });
                    LinePath    il13 = new LinePath(new Coordinates[] { i1, i3 });
                    Coordinates iCC  = il13.GetClosestPoint(i2).Location;
                    if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
                    {
                        il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    else
                    {
                        il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
                    }
                    LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
                    iCCP = il13.AdvancePoint(iCCP, -10.0);
                    il13 = il13.SubPath(iCCP, 20.0);
                    Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width);
                    iBase.Add(il13[0]);
                    Polygon iP = Polygon.GrahamScan(iBase);
                    pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
                }

                return(pBase);
            }

            // fall out
            return(null);
        }