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