public void SelectPartition(Coordinates c)
{
if (this.Partition != null)
{
this.Partition.selected = SelectionType.NotSelected;
this.Partition = null;
}
this.tmpPolyCoords = new List <Coordinates>();
HitTestResult htr = this.Display.HitTest(c, this.PartitionFilter);
if (htr.Hit)
{
ArbiterLanePartition tmpPartition = (ArbiterLanePartition)htr.DisplayObject;
if (tmpPartition.Lane.RelativelyInside(c))
{
this.Partition = (ArbiterLanePartition)htr.DisplayObject;
this.Partition.selected = SelectionType.SingleSelected;
this.Mode = SparseToolboxMode.None;
this.ResetButtons();
if (this.Partition.SparsePolygon == null)
{
this.Partition.SetDefaultSparsePolygon();
}
}
}
this.Display.Invalidate();
}
public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
{
Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);
vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
List <Coordinates> pointsOutside = new List <Coordinates>();
ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition);
foreach (Coordinates c in vehiclePoly)
{
if (!p.IsInside(c))
{
pointsOutside.Add(c);
}
}
foreach (Coordinates m in pointsOutside)
{
foreach (Coordinates n in pointsOutside)
{
if (!m.Equals(n))
{
if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
{
return(false);
}
}
}
}
return(true);
}
/// <summary>
/// Gets all exits downstream from a point
/// </summary>
/// <param name="position"></param>
/// <param name="way"></param>
/// <param name="exits">We are looking for exits</param>
/// <returns></returns>
private List <DownstreamPointOfInterest> Downstream(Coordinates position, ArbiterWay way, bool exits, List <ArbiterWaypoint> ignorable)
{
List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>();
foreach (ArbiterLane al in way.Lanes.Values)
{
LinePath.PointOnPath pop = al.GetClosestPoint(position);
if (al.IsInside(position) || position.DistanceTo(al.LanePath().StartPoint.Location) < 1.0)
{
ArbiterLanePartition currentPartition = al.GetClosestPartition(position);
ArbiterWaypoint initial = currentPartition.Final;
double initialCost = position.DistanceTo(currentPartition.Final.Position) / way.Segment.SpeedLimits.MaximumSpeed;
do
{
if (((exits && currentPartition.Final.IsExit) || (!exits && currentPartition.Final.IsEntry)) && !ignorable.Contains(currentPartition.Final))
{
double timeCost = initialCost + this.TimeCostInLane(initial, currentPartition.Final);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.LanePath().DistanceBetween(pop, al.GetClosestPoint(currentPartition.Final.Position));
dpoi.IsExit = true;
dpoi.IsGoal = false;
dpoi.PointOfInterest = currentPartition.Final;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
currentPartition = currentPartition.Final.NextPartition;
}while(currentPartition != null);
}
}
return(waypoints);
}
public static ArbiterTurnDirection PartitionTurnDirection(ArbiterLanePartition alp)
{
ArbiterWaypoint initWp = alp.Initial;
ArbiterWaypoint finWp = alp.Final;
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 > 20.0)
{
return(ArbiterTurnDirection.Left);
}
else if (arcTan < -20.0)
{
return(ArbiterTurnDirection.Right);
}
else
{
return(ArbiterTurnDirection.Straight);
}
}
/// <summary>
/// Get a road plan while setting partition costs very high
/// </summary>
/// <param name="partition"></param>
/// <param name="goal"></param>
/// <param name="blockAdjacent"></param>
/// <param name="c"></param>
/// <returns></returns>
public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
{
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
RoadPlan rp = null;
if (!blockAdjacent)
{
NavigationBlockage nb = partition.Blockage;
NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
partition.Blockage = tmp;
rp = this.PlanNavigableArea(partition.Lane, c, goal, new List <ArbiterWaypoint>());
partition.Blockage = nb;
}
else
{
// save
List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> > savedBlockages = new List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> >();
// set
savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(partition, partition.Blockage));
// create new
NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
anewerBlockage.BlockageExists = true;
partition.Blockage = anewerBlockage;
foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
{
if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
{
savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(alp, alp.Blockage));
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
}
// plan
rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List <ArbiterWaypoint>());
// restore
foreach (KeyValuePair <ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
{
saved.Key.Blockage = saved.Value;
}
}
// restore
this.currentTimes = tmpCurrentTimes;
// return
return(rp);
}
/// <summary>
/// Try to plan the intersection heavily penalizing the interconnect
/// </summary>
/// <param name="iTraversableWaypoint"></param>
/// <param name="iArbiterWaypoint"></param>
/// <param name="arbiterInterconnect"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated)
{
ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric;
if (!blockAllRelated)
{
return(this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect));
}
else
{
Dictionary <IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary <IConnectAreaWaypoints, NavigationBlockage>();
if (entry.IsEntry)
{
foreach (ArbiterInterconnect ai in entry.Entries)
{
saved.Add(ai, ai.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
ai.Blockage = newerBlockage;
}
}
if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null)
{
ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition;
saved.Add(alp, alp.Blockage);
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
// plan
IntersectionPlan ip = this.PlanIntersection(exit, goal);
this.currentTimes = tmpCurrentTimes;
// reset the blockages
foreach (KeyValuePair <IConnectAreaWaypoints, NavigationBlockage> savedPair in saved)
{
savedPair.Key.Blockage = savedPair.Value;
}
// return plan
return(ip);
}
}
private static double FinalIntersectionAngle(ArbiterLanePartition alp1)
{
Coordinates iVec = alp1.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = alp1.Final.NextPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
return(arcTan);
}
/// <summary>
/// Gets adjacent lane partitions
/// </summary>
/// <param name="alp"></param>
/// <returns></returns>
private List <NavigableEdge> AdjacentPartitions(ArbiterLanePartition alp)
{
List <NavigableEdge> edges = new List <NavigableEdge>();
edges.Add(alp);
List <ArbiterLanePartition> pars = alp.NonLaneAdjacentPartitions;
foreach (ArbiterLanePartition alps in pars)
{
edges.Add(alps);
}
return(edges);
}
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));
}
private void SparseToolboxResetSparsePolygon_Click(object sender, EventArgs e)
{
this.ResetButtons();
if (this.Partition != null)
{
this.Partition.SetDefaultSparsePolygon();
}
if (this.Partition != null)
{
this.Partition.selected = SelectionType.NotSelected;
this.Partition = null;
}
this.Display.Invalidate();
}
/// <summary>
/// Gets route information for sending to remote listeners
/// </summary>
public List <RouteInformation> RouteInformation(Coordinates currentPosition)
{
List <RouteInformation> routes = new List <RouteInformation>();
foreach (KeyValuePair <ArbiterLaneId, LanePlan> lp in this.LanePlans)
{
List <Coordinates> route = new List <Coordinates>();
double time = lp.Value.laneWaypointOfInterest.TotalTime;
// get lane coords
ArbiterLanePartition current = lp.Value.laneWaypointOfInterest.PointOfInterest.Lane.GetClosestPartition(currentPosition);
if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
if (sisls.Lane.ClosestComponent(currentPosition) == SLComponentType.Initial)
{
LinePath p = sisls.Lane.LanePath(currentPosition, sisls.Lane.Interconnect.InitialGeneric.Position);
route.AddRange(p);
route.Add(sisls.Lane.Interconnect.FinalGeneric.Position);
current = ((ArbiterWaypoint)sisls.Lane.Interconnect.FinalGeneric).NextPartition;
}
}
while (current != null && current.Initial != lp.Value.laneWaypointOfInterest.PointOfInterest)
{
route.Add(current.Final.Position);
current = current.Final.NextPartition;
}
// get route coords
if (lp.Value.laneWaypointOfInterest.BestRoute != null)
{
foreach (INavigableNode inn in lp.Value.laneWaypointOfInterest.BestRoute)
{
route.Add(inn.Position);
}
}
RouteInformation ri = new RouteInformation(route, time, lp.Value.laneWaypointOfInterest.PointOfInterest.WaypointId.ToString());
routes.Add(ri);
}
routes.Sort();
return(routes);
}
/// <summary>
/// Distance to the next stop line
/// </summary>
/// <param name="simVehicleState"></param>
/// <returns></returns>
public double?DistanceToNextStop(SimVehicleState simVehicleState)
{
if (this.RoadNetwork == null)
{
return(null);
}
else
{
IAreaSubtypeId iasi = this.GetAreaId(simVehicleState);
if (iasi == null)
{
return(null);
}
else if (iasi is ArbiterPerimeterId)
{
return(null);
}
else
{
ArbiterLaneId ali = (ArbiterLaneId)iasi;
// get closest
ArbiterLanePartition alp = this.RoadNetwork.ArbiterSegments[ali.SegmentId].Lanes[ali].GetClosestPartition(simVehicleState.Position);
ArbiterWaypoint waypoint;
double distance;
RoadNetworkTools.NextStop(alp.Lane, alp, simVehicleState.Position, out waypoint, out distance);
if (waypoint == null)
{
return(null);
}
else
{
return(distance);
}
}
}
}
private static Polygon GenerateMiddlePathPolygon(LinePath initial, LinePath middle, LinePath final, ArbiterLane lane)
{
// wp's
ArbiterWaypoint w1 = new ArbiterWaypoint(initial[0], new ArbiterWaypointId(1, lane.LaneId));
ArbiterWaypoint w2 = new ArbiterWaypoint(initial[1], new ArbiterWaypointId(2, lane.LaneId));
ArbiterWaypoint w3 = new ArbiterWaypoint(final[0], new ArbiterWaypointId(3, lane.LaneId));
ArbiterWaypoint w4 = new ArbiterWaypoint(final[1], new ArbiterWaypointId(4, lane.LaneId));
// set lane
w1.Lane = lane;
w2.Lane = lane;
w3.Lane = lane;
w4.Lane = lane;
// alps
ArbiterLanePartition alp1 = new ArbiterLanePartition(new ArbiterLanePartitionId(w1.WaypointId, w2.WaypointId, lane.LaneId), w1, w2, lane.Way.Segment);
ArbiterLanePartition alp2 = new ArbiterLanePartition(new ArbiterLanePartitionId(w2.WaypointId, w3.WaypointId, lane.LaneId), w2, w3, lane.Way.Segment);
ArbiterLanePartition alp3 = new ArbiterLanePartition(new ArbiterLanePartitionId(w3.WaypointId, w4.WaypointId, lane.LaneId), w3, w4, lane.Way.Segment);
// set links
w1.NextPartition = alp1;
w2.NextPartition = alp2;
w3.NextPartition = alp3;
w4.PreviousPartition = alp3;
w3.PreviousPartition = alp2;
w2.PreviousPartition = alp1;
// get poly
ArbiterTurnDirection atd = PartitionTurnDirection(alp2);
if (atd != ArbiterTurnDirection.Straight)
{
ArbiterInterconnect ai = alp2.ToInterconnect;
ai.TurnDirection = atd;
GenerateInterconnectPolygon(ai);
return(ai.TurnPolygon);
}
return(null);
}
public bool VehiclePassableInPolygon(ArbiterLane al, Polygon p, VehicleState ourState, Polygon circ)
{
List <Coordinates> vhcCoords = new List <Coordinates>();
for (int i = 0; i < this.trackedCluster.relativePoints.Length; i++)
{
vhcCoords.Add(this.TransformCoordAbs(this.trackedCluster.relativePoints[i], ourState));
}
Polygon vehiclePoly = Polygon.GrahamScan(vhcCoords);
vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
ArbiterLanePartition alp = al.GetClosestPartition(this.trackedCluster.closestPoint);
List <Coordinates> pointsOutside = new List <Coordinates>();
foreach (Coordinates c in vehiclePoly)
{
if (!p.IsInside(c))
{
pointsOutside.Add(c);
}
}
foreach (Coordinates m in pointsOutside)
{
foreach (Coordinates n in pointsOutside)
{
if (!m.Equals(n))
{
if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
{
return(false);
}
}
}
}
return(true);
}
/// <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(VehicleState state, List <ArbiterLane> involved)
{
Coordinates exit = state.Front;
// initialize the bounding box
List <Coordinates> boundingBox = new List <Coordinates>();
// put in coords for every available lane
foreach (ArbiterLane al in involved)
{
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(new Polygon(Polygon.GrahamScan(boundingBox)));
}
/// <summary>
/// Add a costly blockage across the road
/// </summary>
/// <param name="partition"></param>
/// <param name="c"></param>
public void AddHarshBlockageAcrossSegment(ArbiterLanePartition partition, Coordinates c)
{
partition.Blockage.BlockageExists = true;
partition.Blockage.BlockageCoordinates = c;
partition.Blockage.SecondsSinceObserved = 0.0;
partition.Blockage.BlockageTimeCost = 3600;
partition.Blockage.BlockageLifetime = partition.Blockage.BlockageLifetime * 4.0;
foreach (ArbiterLanePartition alp in ((ArbiterLanePartition)partition).NonLaneAdjacentPartitions)
{
if (alp.IsInside(c))
{
alp.Blockage.BlockageExists = true;
alp.Blockage.BlockageCoordinates = c;
alp.Blockage.SecondsSinceObserved = 0.0;
alp.Blockage.BlockageTimeCost = 3600;
alp.Blockage.BlockageLifetime = alp.Blockage.BlockageLifetime * 4.0;
}
}
// reset navigation costs
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
}
/// <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));
}
/// <summary>
/// Gets the next stop
/// </summary>
/// <param name="al"></param>
/// <param name="alp"></param>
/// <param name="c"></param>
/// <returns></returns>
public static void NextStop(ArbiterLane al, ArbiterLanePartition alp, Coordinates c, out ArbiterWaypoint waypoint, out double distance)
{
ArbiterWaypoint current = alp.Final;
while (current != null)
{
if (current.IsStop)
{
distance = al.PartitionPath.DistanceBetween(
al.PartitionPath.GetClosest(c),
al.PartitionPath.GetClosest(current.Position));
waypoint = current;
return;
}
else
{
current = current.NextPartition != null ? current.NextPartition.Final : null;
}
}
waypoint = null;
distance = Double.MaxValue;
}
public override void Process(object param)
{
// inform the base that we're beginning processing
if (!BeginProcess())
{
return;
}
// check if we were given a parameter
if (param != null && param is StayInLaneBehavior)
{
HandleBehavior((StayInLaneBehavior)param);
}
if (reverseGear)
{
ProcessReverse();
return;
}
// figure out the planning distance
double planningDistance = GetPlanningDistance();
if (sparse && planningDistance > 10)
{
planningDistance = 10;
}
double?boundStartDistMin = null;
double?boundEndDistMax = null;
if (laneID != null && Services.RoadNetwork != null)
{
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy);
LinePath partitionPath = partition.UserPartitionPath;
LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy);
double remainingDist = planningDistance;
double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint);
remainingDist -= totalDist;
if (sparse)
{
// walk ahead and determine where sparsity ends
bool nonSparseFound = false;
while (remainingDist > 0)
{
// get the next partition
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type != PartitionType.Sparse)
{
nonSparseFound = true;
break;
}
else
{
double dist = partition.UserPartitionPath.PathLength;
totalDist += dist;
remainingDist -= dist;
}
}
if (nonSparseFound)
{
boundStartDistMin = totalDist;
}
}
else
{
// determine if there is a sparse segment upcoming
bool sparseFound = false;
while (remainingDist > 0)
{
partition = partition.Final.NextPartition;
if (partition == null)
{
break;
}
if (partition.Type == PartitionType.Sparse)
{
sparseFound = true;
break;
}
else
{
double dist = partition.Length;
totalDist += dist;
remainingDist -= dist;
}
}
if (sparseFound)
{
boundEndDistMax = totalDist;
sparse = true;
}
}
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance);
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight);
double avoidanceExtra = sparse ? 5 : 7.5;
LinePath centerLine, leftBound, rightBound;
if (boundEndDistMax != null || boundStartDistMin != null)
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound);
}
else
{
LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound);
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count);
// calculate time to collision of opposing obstacle if it exists
LinePath targetLine = centerLine;
double targetWeight = default_lane_alpha_w;
if (sparse)
{
targetWeight = 0.00000025;
}
else if (oncomingVehicleExists)
{
double shiftDist = -(TahoeParams.T + 1);
targetLine = centerLine.ShiftLateral(shiftDist);
targetWeight = 0.01;
}
else if (opposingLaneVehicleExists)
{
double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed));
Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp);
if (timeToCollision < 5)
{
// shift to the right
double shiftDist = -TahoeParams.T / 2.0;
targetLine = centerLine.ShiftLateral(shiftDist);
}
}
// set up the planning
AddTargetPath(targetLine, targetWeight);
if (!sparse || boundStartDistMin != null || boundEndDistMax != null)
{
AddLeftBound(leftBound, true);
if (!oncomingVehicleExists)
{
AddRightBound(rightBound, false);
}
}
double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance);
smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist);
maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint;
double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength;
extraDist = Math.Min(extraDist, 5);
if (extraDist > 0)
{
centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist));
}
avoidanceBasePath = centerLine;
Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
// get the overall max speed looking forward from our current point
settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay));
// get the max speed at the end point
settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance));
useAvoidancePath = false;
if (sparse)
{
// limit to 5 mph
laneWidthAtPathEnd = 20;
pathAngleCheckMax = 50;
pathAngleMax = 5 * Math.PI / 180.0;
settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352);
maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2);
//maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2);
LinePath leftEdge = RoadEdge.GetLeftEdgeLine();
LinePath rightEdge = RoadEdge.GetRightEdgeLine();
if (leftEdge != null)
{
leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false));
}
if (rightEdge != null)
{
rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false));
}
PlanningResult result = new PlanningResult();
ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint);
if (commandGenerator == null)
{
// we have a block, report dynamically infeasible
result = OnDynamicallyInfeasible(null, null);
}
else
{
result.steeringCommandGenerator = commandGenerator;
result.commandLabel = commandLabel;
}
Track(result, commandLabel);
return;
}
else if (oncomingVehicleExists)
{
laneWidthAtPathEnd = 10;
}
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed);
disablePathAngleCheck = false;
SmoothAndTrack(commandLabel, true);
}
public PathRoadModel GetPathRoadEstimate(SimVehicleState state)
{
PathRoadModel ret = new PathRoadModel(new List <PathRoadModel.LaneEstimate>(), (double)SimulatorClient.GetCurrentTimestamp);
ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position);
LinePath path;
if (IsPartitionSameDirection(closestPartition, state.Heading))
{
path = BuildPath(closestPartition, true, 10, 40);
}
else
{
path = BuildPath(closestPartition, false, 10, 40);
}
Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan) * Matrix3.Translation(-state.Position.X, -state.Position.Y);
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate center = new PathRoadModel.LaneEstimate(path, closestPartition.Lane.Width, closestPartition.PartitionId.ToString());
ret.laneEstimates.Add(center);
// get the lane to the left
if (closestPartition.Lane.LaneOnLeft != null)
{
ArbiterLanePartition partition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(partition))
{
if (IsPartitionSameDirection(partition, state.Heading))
{
path = BuildPath(partition, true, 10, 25);
}
else
{
path = BuildPath(partition, false, 10, 25);
}
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate left = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString());
ret.laneEstimates.Add(left);
}
}
if (closestPartition.Lane.LaneOnRight != null)
{
ArbiterLanePartition partition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(partition))
{
if (IsPartitionSameDirection(partition, state.Heading))
{
path = BuildPath(partition, true, 10, 25);
}
else
{
path = BuildPath(partition, false, 10, 25);
}
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate right = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString());
ret.laneEstimates.Add(right);
}
}
return(ret);
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Waiting At Intersection Exit
if (planningState is WaitingAtIntersectionExitState)
{
// state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// nullify turn reasoning
this.TurnReasoning = null;
#region Intersection Monitor Updates
// check correct intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) &&
(IntersectionTactical.IntersectionMonitor == null ||
!IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint)))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
waies.exitWaypoint,
CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
}
// update if exists
if (IntersectionTactical.IntersectionMonitor != null)
{
// update monitor
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
// print current
ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString());
}
#endregion
#region Desired Behavior
// get best option from previously saved
IConnectAreaWaypoints icaw = null;
if (waies.desired != null)
{
ArbiterInterconnect tmpInterconnect = waies.desired;
if (waies.desired.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric;
if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric))
{
icaw = init.NextPartition;
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = ip.BestOption;
waies.desired = icaw.ToInterconnect;
}
#endregion
#region Turn Feasibility Reasoning
// check uturn
if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn)
{
waies.turnTestState = TurnTestState.Completed;
}
// check already determined feasible
if (waies.turnTestState == TurnTestState.Unknown ||
waies.turnTestState == TurnTestState.Failed)
{
#region Determine Behavior to Accomplish Turn
// get default turn behavior
TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw);
// set saudi decorator
if (waies.saudi != SAUDILevel.None)
{
testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi));
}
// set to ignore all vehicles
testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
#endregion
#region Check Turn Feasible
// check if we have completed
CompletionReport turnCompletionReport;
bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport); //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true);
// if we have completed the test
if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic)
{
#region Can Complete
// check success
if (turnCompletionReport.Result == CompletionResult.Success)
{
// set completion state of the turn
waies.turnTestState = TurnTestState.Completed;
}
#endregion
#region No Saudi Level, Found Initial Blockage
// otherwise we cannot do the turn, check if saudi is still none
else if (waies.saudi == SAUDILevel.None)
{
// notify
ArbiterOutput.Output("Increased Saudi Level of Turn to L1");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Failed;
}
#endregion
#region Already at L1 Saudi
else if (waies.saudi == SAUDILevel.L1)
{
// notify
ArbiterOutput.Output("Turn with Saudi L1 Level failed");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString());
// set the interconnect as being blocked
CoreCommon.Navigation.AddInterconnectBlockage(waies.desired);
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
}
#endregion
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// notify
ArbiterOutput.Output("Had to fallout to using no turn bounds");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Completed;
waies.useTurnBounds = false;
}
#endregion
}
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Unknown;
waies.useTurnBounds = false;
}
#endregion
}
#endregion
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
#endregion
#region Entry Monitor Blocked
// checks the entry monitor vehicle for failure
if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null &&
IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed)
{
ArbiterOutput.Output("Entry area blocked");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired,
true);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95))
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final");
// set all the interconnects to the final as being blocked
if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry)
{
foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries)
{
CoreCommon.Navigation.AddInterconnectBlockage(toBlock);
}
}
// check if exists previous partition to block
if (waies.desired.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric;
if (finWaypoint.PreviousPartition != null)
{
CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false);
}
}
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
else
{
ArbiterOutput.Output("Entry area blocked, but no otehr valid route found");
}
}
#endregion
// check if can traverse
if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState))
{
#region If can traverse the intersection
// quick check not interconnect
if (!(icaw is ArbiterInterconnect))
{
icaw = icaw.ToInterconnect;
}
// get interconnect
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
// clear all old completion reports
CoreCommon.Communications.ClearCompletionReports();
// check if uturn
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
// go into turn
List <ArbiterLane> involvedLanes = new List <ArbiterLane>();
involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane);
involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane);
uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane,
IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes));
nextState.Interconnect = ai;
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
}
// otherwise need to wait
else
{
IState next = waies;
return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Stopping At Exit
else if (planningState is StoppingAtExitState)
{
// cast to exit stopping
StoppingAtExitState saes = (StoppingAtExitState)planningState;
saes.currentPosition = vehicleState.Front;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// if has an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
// update it
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
}
else
{
IntersectionTactical.IntersectionMonitor = null;
}
// otherwise update the stop parameters
saes.currentPosition = vehicleState.Front;
Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp));
}
#endregion
#region In uTurn
else if (planningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)planningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
{
// quick check
if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get the closest partition to the new lane
ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front);
// waypoints
ArbiterWaypoint partitionInitial = alpClose.Initial;
ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric;
// check initial past end
if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number)
{
// get waypoints inclusive
List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial);
bool found = false;
// loop through
foreach (ArbiterWaypoint aw in inclusive)
{
if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// set found
found = true;
}
}
}
// default check
else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
// check if the uturn is for a blockage
else if (uts.Interconnect == null)
{
// get final lane
ArbiterLane targetLane = uts.TargetLane;
// check has opposing
if (targetLane.Way.Segment.Lanes.Count > 1)
{
// check the final checkpoint is in our lane
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId))
{
// check that the final checkpoint is within the uturn polygon
if (uts.Polygon != null &&
uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position))
{
// remove the checkpoint
ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
}
}
// stay in target lane
IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true));
return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// otherwise continue uturn
else
{
// get polygon
Polygon p = uts.Polygon;
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon));
// check the type of uturn
if (!uts.singleLaneUturn)
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath();
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
else
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath().Clone();
endingPath = endingPath.ShiftLateral(-2.0); //uts.TargetLane.Width);
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound));
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
}
}
#endregion
#region In Turn
else if (planningState is TurnState)
{
// get state
TurnState ts = (TurnState)planningState;
// add bounds to observable
if (ts.LeftBound != null && ts.RightBound != null)
{
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound));
}
// create current turn reasoning
if (this.TurnReasoning == null)
{
this.TurnReasoning = new TurnReasoning(ts.Interconnect,
IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null);
}
// get primary maneuver
Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts);
// get secondary maneuver
Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan);
// return the manevuer
return(secondary.HasValue ? secondary.Value : primary);
}
#endregion
#region Itnersection Startup
else if (planningState is IntersectionStartupState)
{
// state and plan
IntersectionStartupState iss = (IntersectionStartupState)planningState;
IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan;
// initial path
LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front });
List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>();
// vehicle polygon forward of us
Polygon vehicleForward = vehicleState.ForwardPolygon;
// best waypoint
ITraversableWaypoint best = null;
double bestCost = Double.MaxValue;
// given feasible choose best, no feasible choose random
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
if (vehicleForward.IsInside(itw.Position))
{
feasibleEntries.Add(itw);
}
}
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
feasibleEntries.Add(itw);
}
}
}
// get best
foreach (ITraversableWaypoint itw in feasibleEntries)
{
if (isp.NodeTimeCosts.ContainsKey(itw))
{
KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]);
if (best == null || lookup.Value < bestCost)
{
best = lookup.Key;
bestCost = lookup.Value;
}
}
}
// get something going to this waypoint
ArbiterInterconnect interconnect = null;
if (best.IsEntry)
{
ArbiterInterconnect closest = null;
double closestDistance = double.MaxValue;
foreach (ArbiterInterconnect ai in best.Entries)
{
double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
if (closest == null || dist < closestDistance)
{
closest = ai;
closestDistance = dist;
}
}
interconnect = closest;
}
else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null)
{
interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect;
}
// get state
if (best is ArbiterWaypoint)
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString());
}
#endregion
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public StartupOffChuteState(ArbiterLanePartition final)
{
this.Final = final;
}
/// <summary>
/// What to do when mouse moves
/// </summary>
/// <param name="e"></param>
protected override void OnMouseMove(MouseEventArgs e)
{
#region Left
if (e.Button == MouseButtons.Left)
{
// dragging vehicle
if (this.selected != null && this.selected is CarDisplayObject && isDragging && this.temporaryCoordinate.HasValue)
{
// Get the offset.
Point point = e.Location;
// new coord
Coordinates newCoord = this.transform.GetWorldPoint(new PointF(point.X, point.Y));
// calc offse
Coordinates offset = newCoord - this.temporaryCoordinate.Value;
// moving object
CarDisplayObject cdo = (CarDisplayObject)this.selected;
// check we are not tracking
if (this.tracked != null && cdo.Equals(this.tracked))
{
this.tracked = null;
}
// move
cdo.InMove(this.temporaryCoordinate.Value, offset, this.transform);
// check snap pos or heading
if (cdo.SnapHeading || cdo.SnapPosition)
{
// filter for vehicles
DisplayObjectFilter partitionDof = delegate(IDisplayObject target)
{
// check if target is network object
if (target is ArbiterLanePartition)
{
return(true);
}
else
{
return(false);
}
};
// check to see if selected a partition
HitTestResult vhcHtr = this.HitTest(transform.GetWorldPoint(new PointF(e.X, e.Y)), partitionDof);
// check hit
if (vhcHtr.Hit)
{
// get partition
ArbiterLanePartition alp = (ArbiterLanePartition)vhcHtr.DisplayObject;
// heading
Coordinates heading = alp.Vector();
// position
Coordinates closest = alp.PartitionPath.GetPoint(alp.PartitionPath.GetClosestPoint(transform.GetWorldPoint(new PointF(e.X, e.Y))));
if (cdo.SnapPosition)
{
cdo.Position = closest;
}
if (cdo.SnapHeading)
{
cdo.Heading = heading;
}
}
}
}
else if (this.selected != null && this.selected is SimObstacle && isDragging && this.temporaryCoordinate.HasValue)
{
// Get the offset.
Point point = e.Location;
// new coord
Coordinates newCoord = this.transform.GetWorldPoint(new PointF(point.X, point.Y));
// calc offse
Coordinates offset = newCoord - this.temporaryCoordinate.Value;
// moving object
SimObstacle so = (SimObstacle)this.selected;
// move
so.InMove(this.temporaryCoordinate.Value, offset, this.transform);
}
// check if user is dragging
else if (isDragging)
{
// Get the offset.
Point point = (Point)controlTag;
// Calculate change in position
double deltaX = e.X - point.X;
double deltaY = e.Y - point.Y;
// Update the world
Coordinates tempCenter = WorldTransform.CenterPoint;
tempCenter.X -= deltaX / WorldTransform.Scale;
tempCenter.Y += deltaY / WorldTransform.Scale;
WorldTransform.CenterPoint = tempCenter;
// update control
controlTag = new Point(e.X, e.Y);
}
// redraw
this.Invalidate();
}
#endregion
#region Right
else if (e.Button == MouseButtons.Right)
{
if (this.selected != null && this.selected is SimObstacle && isDragging && this.temporaryCoordinate.HasValue)
{
// Get the offset.
Point point = e.Location;
// new coord
Coordinates newCoord = this.transform.GetWorldPoint(new PointF(point.X, point.Y));
// moving object
SimObstacle so = (SimObstacle)this.selected;
// calc new rel heading
Coordinates offset = newCoord - so.Position;
// calc degree diff
//double rotDiff = offset.ToDegrees() - this.temporaryCoordinate.Value.ToDegrees();
// new head
//Coordinates newHead = so.Heading.Rotate(rotDiff);
// set
so.Heading = offset;
this.Invalidate();
}
}
#endregion
base.OnMouseMove(e);
}
/// <summary>
/// Startup the vehicle from a certain position, pass the next state back,
/// and initialize the lane agent if possible
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IState Startup(VehicleState vehicleState, CarMode carMode)
{
// check car mode
if (carMode == CarMode.Run)
{
// check areas
ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);
if (az != null)
{
// special zone startup
return(new ZoneStartupState(az));
}
if (ain != null)
{
if (al != null)
{
// check lane stuff
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check dist to lane
if (dist < al.Width + 1.0)
{
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// Opposing lane
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// startup intersection state
return(new IntersectionStartupState(ain));
}
if (al != null)
{
// get a startup chute
ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);
// check if in a startup chute
if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
{
ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
return(new StartupOffChuteState(startupChute));
}
// not in a startup chute
else
{
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// opposing
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// fell out
ArbiterOutput.Output("Cannot find area to startup in");
return(CoreCommon.CorePlanningState);
}
else
{
return(CoreCommon.CorePlanningState);
}
}
private LinePath BuildPath(ArbiterLanePartition parition, bool forward, double distBackTarget, double distForwardTarget)
{
if (forward)
{
// iterate all the way backward until we have our distance
ArbiterLanePartition backPartition = parition;
double distBack = 0;
while (backPartition.Initial.PreviousPartition != null && distBack < distBackTarget)
{
backPartition = backPartition.Initial.PreviousPartition;
distBack += backPartition.Length;
}
LinePath path = new LinePath();
// insert the backmost point
while (backPartition != parition)
{
path.Add(backPartition.Initial.Position);
backPartition = backPartition.Final.NextPartition;
}
// add our initial position
path.Add(parition.Initial.Position);
// add our final position
path.Add(parition.Final.Position);
double distForward = 0;
while (parition.Final.NextPartition != null && distForward < distForwardTarget)
{
parition = parition.Final.NextPartition;
distForward += parition.Length;
path.Add(parition.Final.Position);
}
return(path);
}
else
{
// iterate all the way backward until we have our distance
ArbiterLanePartition backPartition = parition;
double distBack = 0;
while (backPartition.Final.NextPartition != null && distBack < distBackTarget)
{
backPartition = backPartition.Final.NextPartition;
distBack += backPartition.Length;
}
LinePath path = new LinePath();
// insert the backmost point
while (backPartition != parition)
{
path.Add(backPartition.Final.Position);
backPartition = backPartition.Initial.PreviousPartition;
}
// add our initial position
path.Add(parition.Final.Position);
// add our final position
path.Add(parition.Initial.Position);
double distForward = 0;
while (parition.Initial.PreviousPartition != null && distForward < distForwardTarget)
{
parition = parition.Initial.PreviousPartition;
distForward += parition.Length;
path.Add(parition.Initial.Position);
}
return(path);
}
}
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);
}
/// <summary>
/// Write partition informaton
/// </summary>
/// <param name="sw"></param>
private void WritePartitionInformation(StreamWriter sw)
{
// get list of all partitions that connect waypoints
List <IConnectAreaWaypoints> icaws = new List <IConnectAreaWaypoints>();
#region Populate partitions
// get lane partitions
foreach (ArbiterSegment asg in roadNetwork.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
foreach (ArbiterLanePartition alp in al.Partitions)
{
icaws.Add(alp);
}
}
}
// get interconnects
foreach (ArbiterInterconnect ai in roadNetwork.ArbiterInterconnects.Values)
{
icaws.Add(ai);
}
// zones (holy stuff what a hack)
foreach (ArbiterZone az in roadNetwork.ArbiterZones.Values)
{
icaws.Add(new SceneZonePartition(az));
}
#endregion
// notify
sw.WriteLine("NumberOfPartitions" + "\t" + icaws.Count.ToString());
string completionPercent = "";
#region Create Partitions in Road Graph
// write each
for (int i = 0; i < icaws.Count; i++)
{
IConnectAreaWaypoints icaw = icaws[i];
sw.WriteLine("Partition");
string id = "";
if (icaw is SceneZonePartition)
{
id = ("PartitionId" + "\t" + ((SceneZonePartition)icaw).Zone.ToString());
}
else
{
id = ("PartitionId" + "\t" + icaw.ConnectionId.ToString());
}
sw.WriteLine(id);
// notify
double percent = ((double)i) / ((double)icaws.Count) * 100.0;
string tmpP = percent.ToString("F0") + "% Complete";
if (tmpP != completionPercent)
{
completionPercent = tmpP;
EditorOutput.WriteLine(completionPercent);
}
#region Interconnect
if (icaw is ArbiterInterconnect)
{
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
sw.WriteLine("PartitionType" + "\t" + "Interconnect");
sw.WriteLine("Sparse" + "\t" + "False");
sw.WriteLine("FitType" + "\t" + "Line");
Coordinates c = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
sw.WriteLine("FitParameters" + "\t" + c.ArcTan.ToString("F6"));
sw.WriteLine("LeftBoundary" + "\t" + "None");
sw.WriteLine("RightBoundary" + "\t" + "None");
sw.WriteLine("NumberOfPoints" + "\t" + "2");
sw.WriteLine("Points");
sw.WriteLine(ai.InitialGeneric.ToString());
sw.WriteLine(ai.FinalGeneric.ToString());
sw.WriteLine("End_Points");
List <ArbiterWaypoint> aws = this.GetNearbyStops(ai);
sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
if (aws.Count != 0)
{
sw.WriteLine("NearbyStoplines");
foreach (ArbiterWaypoint aw in aws)
{
sw.WriteLine(aw.ToString());
}
sw.WriteLine("End_NearbyStoplines");
}
#region Adjacent
List <string> adjacentPartitions = new List <string>();
// add current
adjacentPartitions.Add(ai.ToString());
#region Initial
if (icaw.InitialGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;
// prev
if (aw.PreviousPartition != null)
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null)
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
if (!ais.Equals(ai))
{
adjacentPartitions.Add(ais.ToString());
}
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
if (!ais.Equals(ai))
{
adjacentPartitions.Add(ais.ToString());
}
}
}
}
else if (icaw.InitialGeneric is ArbiterPerimeterWaypoint)
{
adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.InitialGeneric).Perimeter.Zone)).ToString());
}
#endregion
#region Final
if (icaw.FinalGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;
// prev
if (aw.PreviousPartition != null)
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null)
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
adjacentPartitions.Add(ais.ToString());
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
if (!ais.Equals(ai))
{
adjacentPartitions.Add(ais.ToString());
}
}
}
}
else if (icaw.FinalGeneric is ArbiterPerimeterWaypoint)
{
adjacentPartitions.Add((new SceneZonePartition(((ArbiterPerimeterWaypoint)icaw.FinalGeneric).Perimeter.Zone)).ToString());
}
#endregion
sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
if (adjacentPartitions.Count != 0)
{
sw.WriteLine("LaneAdjacentPartitions");
foreach (string s in adjacentPartitions)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LaneAdjacentPartitions");
}
#endregion
sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");
List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(ai, icaws);
sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
if (nearby.Count != 0)
{
sw.WriteLine("NearbyPartitions");
foreach (IConnectAreaWaypoints tmp in nearby)
{
sw.WriteLine(tmp.ToString());
}
sw.WriteLine("End_NearbyPartitions");
}
sw.WriteLine("End_Partition");
}
#endregion
#region Zone
else if (icaw is SceneZonePartition)
{
SceneZonePartition szp = (SceneZonePartition)icaw;
sw.WriteLine("PartitionType" + "\t" + "Zone");
sw.WriteLine("Sparse" + "\t" + "False");
sw.WriteLine("FitType" + "\t" + "Polygon");
string count = szp.Zone.Perimeter.PerimeterPoints.Count.ToString();
string wps = "";
foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
{
wps = wps + "\t" + apw.Position.X.ToString("f6") + "\t" + apw.Position.Y.ToString("f6");
}
sw.WriteLine("FitParameters" + "\t" + count + wps);
sw.WriteLine("LeftBoundary" + "\t" + "None");
sw.WriteLine("RightBoundary" + "\t" + "None");
sw.WriteLine("NumberOfPoints" + "\t" + szp.Zone.Perimeter.PerimeterPoints.Count.ToString());
sw.WriteLine("Points");
foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
{
sw.WriteLine(apw.WaypointId.ToString());
}
sw.WriteLine("End_Points");
List <ArbiterWaypoint> aws = this.GetNearbyStops(szp);
sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
if (aws.Count != 0)
{
sw.WriteLine("NearbyStoplines");
foreach (ArbiterWaypoint aw in aws)
{
sw.WriteLine(aw.ToString());
}
sw.WriteLine("End_NearbyStoplines");
}
#region Adjacent
List <string> adjacentStrings = new List <string>();
// add current
adjacentStrings.Add(szp.ToString());
foreach (ArbiterPerimeterWaypoint apw in szp.Zone.Perimeter.PerimeterPoints.Values)
{
if (apw.IsExit)
{
foreach (ArbiterInterconnect ai in apw.Exits)
{
adjacentStrings.Add(ai.ToString());
}
}
if (apw.IsEntry)
{
foreach (ArbiterInterconnect ais in apw.Entries)
{
adjacentStrings.Add(ais.ToString());
}
}
}
sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentStrings.Count.ToString());
if (adjacentStrings.Count != 0)
{
sw.WriteLine("LaneAdjacentPartitions");
foreach (string s in adjacentStrings)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LaneAdjacentPartitions");
}
#endregion
sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + "0");
sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + "0");
List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(szp, icaws);
sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
if (nearby.Count != 0)
{
sw.WriteLine("NearbyPartitions");
foreach (IConnectAreaWaypoints tmp in nearby)
{
sw.WriteLine(tmp.ToString());
}
sw.WriteLine("End_NearbyPartitions");
}
sw.WriteLine("End_Partition");
}
#endregion
#region Lane
else if (icaw is ArbiterLanePartition)
{
ArbiterLanePartition alp = (ArbiterLanePartition)icaw;
sw.WriteLine("PartitionType" + "\t" + "Lane");
string sparseString = alp.Type == PartitionType.Sparse ? "True" : "False";
sw.WriteLine("Sparse" + "\t" + sparseString);
if (alp.Type != PartitionType.Sparse) //alp.UserPartitions.Count <= 1)
{
sw.WriteLine("FitType" + "\t" + "Line");
sw.WriteLine("FitParameters" + "\t" + alp.Vector().ArcTan.ToString("F6"));
}
else
{
sw.WriteLine("FitType" + "\t" + "Polygon");
/*List<Coordinates> polyCoords = new List<Coordinates>();
* polyCoords.Add(alp.Initial.Position);
* polyCoords.AddRange(alp.NotInitialPathCoords());
* LinePath lpr = (new LinePath(polyCoords)).ShiftLateral(-TahoeParams.VL * 3.0);
* LinePath lpl = (new LinePath(polyCoords)).ShiftLateral(TahoeParams.VL * 3.0);
* List<Coordinates> finalCoords = new List<Coordinates>(polyCoords.ToArray());
* finalCoords.AddRange(lpr);
* finalCoords.AddRange(lpl);
* Polygon p = Polygon.GrahamScan(finalCoords);*/
if (alp.SparsePolygon == null)
{
alp.SetDefaultSparsePolygon();
}
string coordinateString = "";
foreach (Coordinates c in alp.SparsePolygon)
{
coordinateString = coordinateString + "\t" + c.X.ToString("F6") + "\t" + c.Y.ToString("F6");
}
sw.WriteLine("FitParameters" + "\t" + alp.SparsePolygon.Count.ToString() + coordinateString);
}
sw.WriteLine("LaneWidth" + "\t" + alp.Lane.Width.ToString("F6"));
sw.WriteLine("LeftBoundary" + "\t" + alp.Lane.BoundaryLeft.ToString());
sw.WriteLine("RightBoundary" + "\t" + alp.Lane.BoundaryRight.ToString());
sw.WriteLine("NumberOfPoints" + "\t" + "2");
sw.WriteLine("Points");
sw.WriteLine(alp.InitialGeneric.ToString());
sw.WriteLine(alp.FinalGeneric.ToString());
sw.WriteLine("End_Points");
List <ArbiterWaypoint> aws = this.GetNearbyStops(alp);
sw.WriteLine("NumberOfNearbyStoplines" + "\t" + aws.Count);
if (aws.Count != 0)
{
sw.WriteLine("NearbyStoplines");
foreach (ArbiterWaypoint aw in aws)
{
sw.WriteLine(aw.ToString());
}
sw.WriteLine("End_NearbyStoplines");
}
#region Adjacent
List <string> adjacentPartitions = new List <string>();
// add current
adjacentPartitions.Add(alp.ToString());
#region Initial
if (icaw.InitialGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.InitialGeneric;
// prev
if (aw.PreviousPartition != null)
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null && !aw.NextPartition.Equals(alp))
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
adjacentPartitions.Add(ais.ToString());
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
adjacentPartitions.Add(ais.ToString());
}
}
}
#endregion
#region Final
if (icaw.FinalGeneric is ArbiterWaypoint)
{
// wp
ArbiterWaypoint aw = (ArbiterWaypoint)icaw.FinalGeneric;
// prev
if (aw.PreviousPartition != null && !aw.PreviousPartition.Equals(alp))
{
adjacentPartitions.Add(aw.PreviousPartition.ToString());
}
// next
if (aw.NextPartition != null)
{
adjacentPartitions.Add(aw.NextPartition.ToString());
}
// exits
if (aw.IsExit)
{
foreach (ArbiterInterconnect ais in aw.Exits)
{
adjacentPartitions.Add(ais.ToString());
}
}
if (aw.IsEntry)
{
foreach (ArbiterInterconnect ais in aw.Entries)
{
adjacentPartitions.Add(ais.ToString());
}
}
}
#endregion
sw.WriteLine("NumberOfLaneAdjacentPartitions" + "\t" + adjacentPartitions.Count.ToString());
if (adjacentPartitions.Count != 0)
{
sw.WriteLine("LaneAdjacentPartitions");
foreach (string s in adjacentPartitions)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LaneAdjacentPartitions");
}
#endregion
List <string> leftAlps = new List <string>();
List <string> rightAlps = new List <string>();
foreach (ArbiterLanePartition tmpAlp in alp.NonLaneAdjacentPartitions)
{
if (tmpAlp.Lane.Equals(alp.Lane.LaneOnLeft))
{
leftAlps.Add(tmpAlp.ToString());
}
else
{
rightAlps.Add(tmpAlp.ToString());
}
}
sw.WriteLine("NumberOfLeftLaneAdjacentPartitions" + "\t" + leftAlps.Count.ToString());
if (leftAlps.Count != 0)
{
sw.WriteLine("LeftLaneAdjacentPartitions");
foreach (string s in leftAlps)
{
sw.WriteLine(s);
}
sw.WriteLine("End_LeftLaneAdjacentPartitions");
}
sw.WriteLine("NumberOfRightLaneAdjacentPartitions" + "\t" + rightAlps.Count.ToString());
if (rightAlps.Count != 0)
{
sw.WriteLine("RightLaneAdjacentPartitions");
foreach (string s in rightAlps)
{
sw.WriteLine(s);
}
sw.WriteLine("End_RightLaneAdjacentPartitions");
}
List <IConnectAreaWaypoints> nearby = this.GetNearbyPartitions(alp, icaws);
sw.WriteLine("NumberOfNearbyPartitions" + "\t" + nearby.Count.ToString());
if (nearby.Count != 0)
{
sw.WriteLine("NearbyPartitions");
foreach (IConnectAreaWaypoints tmp in nearby)
{
sw.WriteLine(tmp.ToString());
}
sw.WriteLine("End_NearbyPartitions");
}
sw.WriteLine("End_Partition");
}
#endregion
}
#endregion
}
private bool IsPartitionSameDirection(ArbiterLanePartition partition, Coordinates heading)
{
return(partition.Vector().Normalize().Dot(heading) > 0);
}
