private static Polygon GenerateSimplePolygon(LinePath path, double width)
{
// here is default partition polygon
LinePath alplb = path.ShiftLateral(-width / 2.0);
LinePath alprb = path.ShiftLateral(width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
return(new Polygon(alpdefaultPoly));
}
private static Polygon GenerateSimplePartitionPolygon(ArbiterLanePartition alp, LinePath path, double width)
{
// here is default partition polygon
LinePath alplb = path.ShiftLateral(-width / 2.0);
LinePath alprb = path.ShiftLateral(width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
foreach (ArbiterUserWaypoint auw in alp.UserWaypoints)
{
alpdefaultPoly.Add(auw.Position);
}
return(Polygon.GrahamScan(alpdefaultPoly));
}
public static Polygon DefaultLanePolygon(ArbiterLane al)
{
// fist get the right boundary of the lane
LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0);
LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0);
rb.Reverse();
List <Coordinates> defaultPoly = lb;
defaultPoly.AddRange(rb);
// start the polygon
Polygon poly = new Polygon(defaultPoly);
return(poly);
}
public static Polygon LanePolygon(ArbiterLane al)
{
// fist get the right boundary of the lane
LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0);
LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0);
rb.Reverse();
List <Coordinates> defaultPoly = lb;
defaultPoly.AddRange(rb);
// start the polygon
Polygon poly = new Polygon(defaultPoly);
// loop through partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
//if (alp.Initial.PreviousPartition != null && alp.Final.NextPartition != null)
//{
// get the good polygon
Polygon pPoly = PartitionPolygon(alp);
// check not null
if (pPoly != null)
{
poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, pPoly }));
}
//}
}
// circles for intersections of partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
if (alp.Final.NextPartition != null)
{
double interAngle = Math.Abs(FinalIntersectionAngle(alp));
if (interAngle > 15)
{
Circle connect = new Circle((alp.Lane.Width / 2.0) + (interAngle / 15.0 * 0.5), alp.Final.Position);
poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, connect.ToPolygon(16) }));
}
}
}
// return the polygon
return(poly);
}
/// <summary>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(ArbiterLane lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath().Clone();
p.Reverse();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List <VehicleAgent> vas = new List <VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
{
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// gets distance from other vehicle to us along the lane
double frontDist = lane.DistanceBetween(frontPos, f);
if (frontDist >= 0 && frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <summary>
/// Parameters to follow the forward vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters Follow(ArbiterLane lane, VehicleState state)
{
// travelling parameters
TravelingParameters tp = new TravelingParameters();
// ignorable obstacles
List <int> ignoreVehicle = new List <int>();
ignoreVehicle.Add(CurrentVehicle.VehicleId);
// get control parameters
ForwardVehicleTrackingControl fvtc = GetControl(lane, state);
// init params
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.Decorators = TurnDecorators.NoDecorators;
// flag to ignore forward vehicle
bool ignoreForward = false;
// reversed lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Following Control
#region Immediate Stop
// need to stop immediately
if (fvtc.vFollowing == 0.0)
{
// don't ignore forward
ignoreForward = false;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(0.0);
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Distance Stop
// stop at distance
else if (fvtc.vFollowing < 0.7 &&
CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 &&
fvtc.xSeparation > fvtc.xAbsMin)
{
// ignore forward vehicle
ignoreForward = true;
// speed command
SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood);
tp.UsingSpeed = false;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Normal Following
// else normal
else
{
// ignore the forward vehicle as we are tracking properly
ignoreForward = true;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing);
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#endregion
// check ignore
if (ignoreForward)
{
List <int> ignorable = new List <int>();
ignorable.Add(this.CurrentVehicle.VehicleId);
tp.VehiclesToIgnore = ignorable;
}
else
{
tp.VehiclesToIgnore = new List <int>();
}
// return parameterization
return(tp);
}
public void Render(IGraphics g, WorldTransform wt)
{
LocalLaneModel laneModel = this.laneModel;
if (laneModel == null || laneModel.LanePath == null || laneModel.LanePath.Count <= 1)
{
return;
}
// generate the left and right bounds
LinePath leftBound = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0);
LinePath leftBound2 = null;
if (laneModel.WidthVariance > 0.01)
{
leftBound2 = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0 + Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
}
LinePath rightBound = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0);
LinePath rightBound2 = null;
if (laneModel.WidthVariance > 0.01)
{
rightBound2 = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0 - Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
}
// generate a polygon of the confidence region of the center line
Polygon centerConfLeft = null;
Polygon centerConfRight = null;
Polygon centerConfFull = null;
if (sigma > 0 && laneModel.LaneYVariance != null && laneModel.LaneYVariance.Length == laneModel.LanePath.Count)
{
float varianceThreshold = 9f * 9f;
if (laneModel.LaneYVariance[0] > varianceThreshold)
{
varianceThreshold = 600;
}
int numPoints;
for (numPoints = 0; numPoints < laneModel.LaneYVariance.Length; numPoints++)
{
if (laneModel.LaneYVariance[numPoints] > varianceThreshold)
{
break;
}
}
double[] leftDist = new double[numPoints];
double[] rightDist = new double[numPoints];
double prevDist = 0;
for (int i = 0; i < numPoints; i++)
{
double dist = laneModel.LaneYVariance[i];
if (dist > 0.01)
{
dist = Math.Sqrt(dist) * sigma;
leftDist[i] = dist;
rightDist[i] = -dist;
prevDist = dist;
}
else
{
leftDist[i] = prevDist;
rightDist[i] = -prevDist;
}
}
// get the subset of the lane model we're interested in
LinePath subset = laneModel.LanePath.SubPath(0, numPoints - 1);
LinePath left = subset.ShiftLateral(leftDist);
LinePath right = subset.ShiftLateral(rightDist);
Coordinates midTop = (left[left.Count - 1] + right[right.Count - 1]) / 2.0;
Coordinates midBottom = (left[0] + right[0]) / 2.0;
centerConfLeft = new Polygon(numPoints + 2);
centerConfLeft.Add(midTop);
centerConfLeft.Add(midBottom);
centerConfLeft.AddRange(left);
centerConfRight = new Polygon(numPoints + 2);
centerConfRight.Add(midTop);
centerConfRight.Add(midBottom);
centerConfRight.AddRange(right);
centerConfFull = new Polygon(numPoints * 2);
right.Reverse();
centerConfFull.AddRange(right);
centerConfFull.AddRange(left);
}
// draw the shits
IPen pen = g.CreatePen();
pen.Width = 1.0f / wt.Scale;
pen.Color = color;
// first draw the confidence polygon
if (centerConfFull != null)
{
g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfLeft));
g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfRight));
pen.Color = Color.FromArgb(30, color);
g.DrawPolygon(pen, Utility.ToPointF(centerConfFull));
}
// next draw the center line
pen.Color = color;
//g.DrawLines(pen, Utility.ToPointF(laneModel.LanePath));
//// next draw the left lane confidence bound
//if (leftBound2 != null) {
// pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
// g.DrawLines(pen, Utility.ToPointF(leftBound2));
//}
//// draw the right lane confidence bound
//if (rightBound2 != null) {
// pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
// g.DrawLines(pen, Utility.ToPointF(rightBound2));
//}
// draw the left bound
pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Solid;
g.DrawLines(pen, Utility.ToPointF(leftBound));
// draw the right bound
g.DrawLines(pen, Utility.ToPointF(rightBound));
// draw the model confidence
string labelString = laneModel.Probability.ToString("F3");
SizeF stringSize = g.MeasureString(labelString, labelFont);
stringSize.Width /= wt.Scale;
stringSize.Height /= wt.Scale;
RectangleF rect = new RectangleF(Utility.ToPointF(laneModel.LanePath[0]), stringSize);
float inflateValue = 4 / wt.Scale;
rect.X -= inflateValue;
rect.Y -= inflateValue;
g.FillRectangle(Color.FromArgb(127, Color.White), rect);
g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(laneModel.LanePath[0]));
prevLeftBound = leftBound;
prevRightBound = rightBound;
}
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();
}
}
/// <summary>
/// Generate the traveling parameterization for the desired behaivor
/// </summary>
/// <param name="lane"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStop"></param>
/// <param name="navStopType"></param>
/// <param name="state"></param>
/// <returns></returns>
private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state)
{
// get min dist
double distanceCutOff = CoreCommon.OperationalStopDistance;
// turn direction default
List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// create new params
TravelingParameters tp = new TravelingParameters();
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
// get lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Distance Cutoff
// check if distance is less than cutoff
if (navStopDistance < distanceCutOff)
{
// default behavior
tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance);
Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List <int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
// stopping so not using speed param
usingSpeed = false;
IState nextState = CoreCommon.CorePlanningState;
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24));
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List <int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false));
// standard behavior is fine for maneuver
m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp);
}
#endregion
#endregion
#region Parameterize
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = navStopDistance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = navStopSpeed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.VehiclesToIgnore = new List <int>();
// return navigation params
return(tp);
#endregion
}
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);
}
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();
}
}
