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