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