public bool VehicleExistsInsidePolygon(VehicleAgent va, Polygon p, VehicleState ourState)
{
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
if (p.IsInside(c))
return true;
}
return false;
}
/// <summary>
/// ALl polygons of all obstacles and stopped vehicles
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public static List<Polygon> AllObstacleAndStoppedVehiclePolygons(VehicleState vehicleState, bool failedVehiclesPersistentlyOnly)
{
// all polygons to include
List<Polygon> allPolys = new List<Polygon>();
// get list of all point collections
List<Coordinates[]> includePolys = new List<Coordinates[]>();
// get all stopped vehicle polys
SceneEstimatorTrackedClusterCollection setcc = CoreCommon.Communications.GetObservedVehicles();
for (int i = 0; i < setcc.clusters.Length; i++)
{
// check failed vehicle
if (failedVehiclesPersistentlyOnly && TacticalDirector.ValidVehicles.ContainsKey(setcc.clusters[i].id))
{
VehicleAgent va = TacticalDirector.ValidVehicles[setcc.clusters[i].id];
if (va.QueuingState.Queuing == QueuingState.Failed)
{
if (setcc.clusters[i].relativePoints.Length > 2)
includePolys.Add(setcc.clusters[i].relativePoints);
}
}
// check stopped
else if (setcc.clusters[i].relativePoints.Length > 2 && setcc.clusters[i].isStopped)
includePolys.Add(setcc.clusters[i].relativePoints);
}
// get all static polygons
SceneEstimatorUntrackedClusterCollection seucc = CoreCommon.Communications.GetObservedObstacles();
for (int i = 0; i < seucc.clusters.Length; i++)
{
if (seucc.clusters[i].points.Length > 2)
includePolys.Add(seucc.clusters[i].points);
}
// loop through point collections
VehicleAgent tmp = new VehicleAgent(-1);
foreach (Coordinates[] points in includePolys)
{
List<Coordinates> polyPoints = new List<Coordinates>();
for (int j = 0; j < points.Length; j++)
{
polyPoints.Add(tmp.TransformCoordAbs(points[j], vehicleState));
}
allPolys.Add(Polygon.GrahamScan(polyPoints));
}
return allPolys;
}