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