/// <summary>
/// Constructor
/// </summary>
/// <param name="key"></param>
/// <param name="value"></param>
public AreaProbability(IVehicleArea key, double value)
{
this.Key = key;
this.Value = value;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="key"></param>
/// <param name="value"></param>
public AreaProbability(IVehicleArea key, double value)
{
this.Key = key;
this.Value = value;
}
/// <summary>
/// Populates mapping of areas to vehicles
/// </summary>
public void PopulateAreaVehicles()
{
TacticalDirector.VehicleAreas = new Dictionary <IVehicleArea, List <VehicleAgent> >();
VehicleState vs = CoreCommon.Communications.GetVehicleState();
Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
Polygon conv = circ.ToPolygon(32);
Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
Polygon conv1 = circ1.ToPolygon(32);
Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
Polygon conv2 = circ2.ToPolygon(32);
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
#region Standard Areas
List <AreaProbability> AreaProbabilities = new List <AreaProbability>();
#region Add up probablities
for (int i = 0; i < va.StateMonitor.Observed.closestPartitions.Length; i++)
{
SceneEstimatorClusterPartition secp = va.StateMonitor.Observed.closestPartitions[i];
if (CoreCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
{
IVehicleArea iva = CoreCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];
bool found = false;
for (int j = 0; j < AreaProbabilities.Count; j++)
{
AreaProbability ap = AreaProbabilities[j];
if (ap.Key.Equals(iva))
{
ap.Value = ap.Value + secp.probability;
found = true;
}
}
if (!found)
{
AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
}
}
else
{
ArbiterOutput.Output("Core Intelligence thread caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map");
}
}
#endregion
#region Assign
if (AreaProbabilities.Count > 0)
{
double rP = 0.0;
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
rP += ap.Value;
}
}
if (rP > 0.1)
{
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)ap.Key;
// probability says in area
double vP = ap.Value / rP;
if (vP > 0.3)
{
#region Check if obstacle enough in area
bool ok = false;
if (ap.Key is ArbiterLane)
{
Coordinates closest = va.ClosestPointToLine(al.LanePath(), vs).Value;
// dist to closest
double distanceToClosest = vs.Front.DistanceTo(closest);
// get our dist to closest
if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value))))
{
if (al.LanePolygon != null)
{
ok = this.VehicleExistsInsidePolygon(va, al.LanePolygon, vs);
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else if (distanceToClosest <= 30.0)
{
if (al.LanePolygon != null)
{
#warning Darpa is an asshole
if (!va.IsStopped && this.VehicleAllInsidePolygon(va, al.LanePolygon, vs))
{
ok = true;
}
else
{
if (va.IsStopped)
{
// check vehicle is in a safety zone
bool isInSafety = false;
try
{
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(va.ClosestPosition))
{
isInSafety = true;
}
}
foreach (ArbiterSafetyZone asz in al.SafetyZones)
{
if (asz.IsInSafety(va.ClosestPosition))
{
isInSafety = true;
}
}
}
catch (Exception) { }
if (isInSafety)
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv1))
{
ok = true;
}
}
else
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv))
{
ok = true;
}
}
}
else
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv2))
{
ok = true;
}
}
}
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else
{
ok = true;
}
}
#endregion
if (ok)
{
if (TacticalDirector.VehicleAreas.ContainsKey(ap.Key))
{
TacticalDirector.VehicleAreas[ap.Key].Add(va);
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(ap.Key, vas);
}
}
}
}
}
}
}
#endregion
#endregion
#region Interconnect Area Mappings
foreach (ArbiterInterconnect ai in CoreCommon.RoadNetwork.ArbiterInterconnects.Values)
{
if (ai.TurnPolygon.IsInside(va.StateMonitor.Observed.closestPoint))
{
if (TacticalDirector.VehicleAreas.ContainsKey(ai) && !TacticalDirector.VehicleAreas[ai].Contains(va))
{
TacticalDirector.VehicleAreas[ai].Add(va);
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(ai, vas);
}
// check if uturn
if (ai.TurnDirection == ArbiterTurnDirection.UTurn &&
ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
// get the lanes the uturn is a part of
ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
if (TacticalDirector.VehicleAreas.ContainsKey(initialLane))
{
if (!TacticalDirector.VehicleAreas[initialLane].Contains(va))
{
TacticalDirector.VehicleAreas[initialLane].Add(va);
}
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(initialLane, vas);
}
if (TacticalDirector.VehicleAreas.ContainsKey(targetLane))
{
if (!TacticalDirector.VehicleAreas[targetLane].Contains(va))
{
TacticalDirector.VehicleAreas[targetLane].Add(va);
}
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(targetLane, vas);
}
}
}
}
#endregion
}
}
public string PartitionIdString()
{
string final = "";
#region Standard Areas
List <AreaProbability> AreaProbabilities = new List <AreaProbability>();
Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
Polygon conv = circ.ToPolygon(32);
Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
Polygon conv1 = circ1.ToPolygon(32);
Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
Polygon conv2 = circ2.ToPolygon(32);
for (int i = 0; i < this.trackedCluster.closestPartitions.Length; i++)
{
SceneEstimatorClusterPartition secp = this.trackedCluster.closestPartitions[i];
if (RemoraCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
{
IVehicleArea iva = RemoraCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];
bool found = false;
for (int j = 0; j < AreaProbabilities.Count; j++)
{
AreaProbability ap = AreaProbabilities[j];
if (ap.Key.Equals(iva))
{
ap.Value = ap.Value + secp.probability;
found = true;
}
}
if (!found)
{
AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
}
}
else
{
RemoraOutput.WriteLine("Remora caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map", OutputType.Remora);
}
}
if (AreaProbabilities.Count > 0)
{
double rP = 0.0;
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
rP += ap.Value;
}
}
if (rP > 0.1)
{
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
// proabbility says in area
double vP = ap.Value / rP;
if (vP > 0.3)
{
#region Check if obstacle enough in area
bool ok = false;
if (ap.Key is ArbiterLane)
{
VehicleState vs = RemoraCommon.Communicator.GetVehicleState();
ArbiterLane al = (ArbiterLane)ap.Key;
Coordinates closest = this.ClosestPointToLine(al.LanePath(), vs).Value;
// dist to closest
double distanceToClosest = vs.Front.DistanceTo(closest);
// get our dist to closest
if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(RemoraCommon.Communicator.GetVehicleSpeed().Value))))
{
if (al.LanePolygon != null)
{
ok = this.VehicleExistsInsidePolygon(al.LanePolygon, vs);
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else if (distanceToClosest <= 30.0)
{
if (al.LanePolygon != null)
{
if (!this.trackedCluster.isStopped && this.VehicleAllInsidePolygon(al.LanePolygon, vs))
{
ok = true;
}
else
{
if (this.trackedCluster.isStopped)
{
bool isInSafety = false;
foreach (ArbiterIntersection ai in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint))
{
isInSafety = true;
}
}
foreach (ArbiterSafetyZone asz in al.SafetyZones)
{
if (asz.IsInSafety(this.trackedCluster.closestPoint))
{
isInSafety = true;
}
}
if (isInSafety)
{
if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv1))
{
ok = true;
}
}
else
{
if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv))
{
ok = true;
}
}
}
else
{
if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv2))
{
ok = true;
}
}
}
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else
{
ok = true;
}
if (ok)
{
final = final + ap.Key.ToString() + ": " + vP.ToString("F4") + "\n";
}
}
#endregion
}
}
}
}
}
#endregion
#region Interconnect Area Mappings
foreach (ArbiterInterconnect ai in RemoraCommon.RoadNetwork.ArbiterInterconnects.Values)
{
if (ai.TurnPolygon.IsInside(this.trackedCluster.closestPoint))
{
final = final + ai.ToString() + "\n";
if (ai.TurnDirection == ArbiterTurnDirection.UTurn && ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
// initial
ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
final = final + "UTurn2Lane: " + initialLane.ToString() + " & " + targetLane.ToString() + "\n";
}
}
}
#endregion
#region Intersections
foreach (ArbiterIntersection aInt in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (aInt.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint))
{
final = final + "Inter: " + aInt.ToString() + "\n";
}
}
#endregion
return(final);
}
public IntersectionInvolved(IVehicleArea area)
{
this.Exit = null;
this.Area = area;
this.TurnDirection = ArbiterTurnDirection.Unknown;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="exit"></param>
/// <param name="area"></param>
/// <param name="turnDirection"></param>
public IntersectionInvolved(ITraversableWaypoint exit, IVehicleArea area, ArbiterTurnDirection turnDirection)
{
this.Exit = exit;
this.Area = area;
this.TurnDirection = turnDirection;
}
public IntersectionInvolved(IVehicleArea area)
{
this.Exit = null;
this.Area = area;
this.TurnDirection = ArbiterTurnDirection.Unknown;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="exit"></param>
/// <param name="area"></param>
/// <param name="turnDirection"></param>
public IntersectionInvolved(ITraversableWaypoint exit, IVehicleArea area, ArbiterTurnDirection turnDirection)
{
this.Exit = exit;
this.Area = area;
this.TurnDirection = turnDirection;
}
