/// <summary>
/// Helps with parameterizations for lateral reasoning
/// </summary>
/// <param name="referenceLane"></param>
/// <param name="fqmLane"></param>
/// <param name="goal"></param>
/// <param name="vehicleFront"></param>
/// <returns></returns>
public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane,
Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va)
{
// get traveling parameterized list
List<TravelingParameters> tps = new List<TravelingParameters>();
// get distance to the goal
double goalDistance;
double goalSpeed;
this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance);
tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state));
// get next stop
ArbiterWaypoint stopPoint;
double stopSpeed;
double stopDistance;
StopType stopType;
this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List<ArbiterWaypoint>(),
out stopPoint, out stopSpeed, out stopDistance, out stopType);
this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { },
out stopSpeed, out stopDistance);
tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state));
// get vehicle
if (va != null)
{
VehicleAgent tmp = this.ForwardVehicle.CurrentVehicle;
double tmpDist = this.ForwardVehicle.currentDistance;
this.ForwardVehicle.CurrentVehicle = va;
this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition);
TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List<ArbiterWaypoint>());
tps.Add(tp);
this.ForwardVehicle.CurrentVehicle = tmp;
this.ForwardVehicle.currentDistance = tmpDist;
}
// parameterize
tps.Sort();
return tps[0];
}
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>
/// Updates this monitor
/// </summary>
public void Update(VehicleState ourState)
{
// make sure not our vehicle
if (!isOurs)
{
// get a list of vehicles possibly in the exit
List<VehicleAgent> stopVehicles = new List<VehicleAgent>();
// check all valid vehicles
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if any of the point inside the polygon
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
// get abs coord
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
// check if inside poly
if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
{
// add vehicle
stopVehicles.Add(va);
}
}
}
// check occluded
foreach (VehicleAgent va in TacticalDirector.OccludedVehicles.Values)
{
// check if any of the point inside the polygon
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
// get abs coord
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
// check if inside poly
if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
{
// add vehicle
ArbiterOutput.Output("Added occluded vehicle: " + va.ToString() + " to SEM: " + this.stoppedExit.ToString());
stopVehicles.Add(va);
}
}
}
// check if we already have a vehicle we are referencing
if (this.currentVehicle != null && !stopVehicles.Contains(this.currentVehicle))
{
// get the polygon of the current vehicle and inflate a tiny bit?
Polygon p = this.currentVehicle.GetAbsolutePolygon(ourState);
p = p.Inflate(1.0);
// Vehicle agent to switch to if exists
VehicleAgent newer = null;
double distance = Double.MaxValue;
// check for closest vehicle to exist in that polygon
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if vehicle is stopped
if (stopVehicles.Contains(va))
{
// get distance to stop
double stopLineDist;
va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out stopLineDist);
// check all vehicle points
foreach (Coordinates rel in va.StateMonitor.Observed.relativePoints)
{
Coordinates abs = va.TransformCoordAbs(rel, ourState);
double tmpDist = stopLineDist;
// check for the clsoest with points inside vehicle
if (p.IsInside(abs) && (newer == null || tmpDist < distance))
{
newer = va;
distance = tmpDist;
}
}
}
}
// check if we can switch vehicles
if (newer != null)
{
ArbiterOutput.Output("For StopLine: " + this.stoppedExit.ToString() + " switched vehicleId: " + this.currentVehicle.VehicleId.ToString() + " for vehicleId: " + newer.VehicleId.ToString());
this.currentVehicle = newer;
this.NotFoundPrevious = notFoundDefault;
}
else if(NotFoundPrevious == 0)
{
// set this as not having a vehicle
this.currentVehicle = null;
// set not found before
this.NotFoundPrevious = this.notFoundDefault;
// stop timers
this.timer.Stop();
this.timer.Reset();
}
else
{
// set not found before
this.NotFoundPrevious--;
ArbiterOutput.Output("current not found, not found previous: " + this.NotFoundPrevious.ToString() + ", at stopped exit: " + this.stoppedExit.Waypoint.ToString());
}
}
// update the current vehicle
else if (this.currentVehicle != null && stopVehicles.Contains(this.currentVehicle))
{
// udpate current
this.currentVehicle = stopVehicles[stopVehicles.IndexOf(this.currentVehicle)];
// set as found previous
this.NotFoundPrevious = this.notFoundDefault;
// check if need to update timer
if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
this.timer.Start();
}
// otherwise check if moving
else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
}
}
// otherwise if we have no vehicle and exist vehicles in stop area
else if (this.currentVehicle == null && stopVehicles.Count > 0)
{
// get closest vehicle to the stop
VehicleAgent toTrack = null;
double distance = Double.MaxValue;
// set as found previous
this.NotFoundPrevious = this.notFoundDefault;
// loop through
foreach (VehicleAgent va in stopVehicles)
{
// check if vehicle is stopped
if (va.IsStopped)
{
// distance of vehicle to stop line
double distanceToStop;
Coordinates closestToStop = va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out distanceToStop).Value;
// check if we don't have one or tmp closer than tracked
if (toTrack == null || distanceToStop < distance)
{
toTrack = va;
distance = distanceToStop;
}
}
}
// check if we have one to track
if (toTrack != null)
this.currentVehicle = toTrack;
}
}
}
/// <summary>
/// Populates mapping of vehicles we want to track given state
/// </summary>
/// <param name="vehicles"></param>
public void PopulateValidVehicles(SceneEstimatorTrackedClusterCollection vehicles)
{
TacticalDirector.NewVehicles = new List<VehicleAgent>();
TacticalDirector.OccludedVehicles = new Dictionary<int, VehicleAgent>();
if (TacticalDirector.ValidVehicles == null)
TacticalDirector.ValidVehicles = new Dictionary<int, VehicleAgent>();
List<int> toRemove = new List<int>();
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
bool found = false;
for (int i = 0; i < vehicles.clusters.Length; i++)
{
if (vehicles.clusters[i].id.Equals(va.VehicleId))
found = true;
}
if (!found)
toRemove.Add(va.VehicleId);
}
foreach (int r in toRemove)
TacticalDirector.ValidVehicles.Remove(r);
for(int i = 0; i < vehicles.clusters.Length; i++)
{
SceneEstimatorTrackedCluster ov = vehicles.clusters[i];
bool clusterStopped = ov.isStopped;
if (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE ||
(ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && !clusterStopped) ||
(ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && !clusterStopped))
{
if (ov.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE)
{
if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
TacticalDirector.ValidVehicles[ov.id].StateMonitor.Observed = ov;
else
{
VehicleAgent ovNew = new VehicleAgent(ov);
TacticalDirector.ValidVehicles.Add(ovNew.VehicleId, ovNew);
TacticalDirector.NewVehicles.Add(ovNew);
}
}
else
{
if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
TacticalDirector.ValidVehicles.Remove(ov.id);
}
}
else
{
if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
TacticalDirector.ValidVehicles.Remove(ov.id);
}
if ((ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && clusterStopped) ||
(ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && clusterStopped))
TacticalDirector.OccludedVehicles.Add(ov.id, new VehicleAgent(ov));
}
}
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>
/// Resets values held over time
/// </summary>
public void Reset()
{
if (this.CurrentVehicle != null)
this.CurrentVehicle.Reset();
this.CurrentVehicle = null;
this.currentDistance = Double.MaxValue;
}
/// <summary>
/// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public bool Occupied(ArbiterLane lane, VehicleState state)
{
// check stopwatch time for proper elapsed
if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10)
this.Reset();
if (TacticalDirector.VehicleAreas.ContainsKey(lane))
{
// vehicles in the lane
List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane];
// position of the center of our vehicle
Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);
// cutoff for allowing vehicles outside this range
double dCutOff = TahoeParams.VL * 1.5;
// loop through vehicles
foreach (VehicleAgent va in laneVehicles)
{
// vehicle high level distance
double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition));
// check less than distance cutoff
if (d < dCutOff)
{
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
this.CurrentVehicle = va;
this.Reset();
return true;
}
}
}
// now check the lateral sensor for being occupied
if (this.SideSickObstacleDetected(lane, state))
{
this.CurrentVehicle = new VehicleAgent(true, true);
this.Reset();
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
return true;
}
// none found
this.CurrentVehicle = null;
// if none found, timer not running start timer
if (!this.LateralClearStopwatch.IsRunning)
{
this.CurrentVehicle = new VehicleAgent(true, true);
this.Reset();
this.LateralClearStopwatch.Start();
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
return true;
}
// check enough time
else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
{
this.CurrentVehicle = null;
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
return false;
}
// not enough time
else
{
this.CurrentVehicle = new VehicleAgent(true, true);
double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
return true;
}
}
/// <summary>
/// Update the rear monitor with the closest vehicle in the rear
/// </summary>
/// <param name="state"></param>
public void Update(VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath();
// get our position
Coordinates f = state.Front - state.Heading.Normalize(TahoeParams.VL);
// get all vehicles associated with those components
List<VehicleAgent> vas = new List<VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
double frontDist = lane.DistanceBetween(frontPos, f);
if (frontDist >= 0 && frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
public void Update(VehicleState ourState)
{
if (TacticalDirector.VehicleAreas.ContainsKey(this.finalWaypoint.Perimeter.Zone))
{
VehicleAgent closest = null;
double tmpDist = Double.MaxValue;
foreach (VehicleAgent va in TacticalDirector.VehicleAreas[this.finalWaypoint.Perimeter.Zone])
{
if (this.entryPolygon.IsInside(va.ClosestPosition))
{
double tmp = va.ClosestPosition.DistanceTo(this.finalWaypoint.Position);
if (closest == null || tmp < tmpDist)
{
tmpDist = tmp;
closest = va;
}
}
}
if (closest != null && closest.IsStopped)
{
if (this.currentVehicle == null || !this.currentVehicle.Equals(closest))
{
this.currentVehicle = closest;
this.ResetTiming();
this.failedTimer.Start();
}
else
{
this.currentVehicle = closest;
}
}
else
{
this.currentVehicle = closest;
this.ResetTiming();
}
}
else
{
this.currentVehicle = null;
this.ResetTiming();
}
}
public void Update(UrbanChallenge.Common.Vehicle.VehicleState ourState)
{
// get lane
ArbiterLane lane = this.turnFinalWaypoint.Lane;
// get the possible vehicles that could be in this stop
if (TacticalDirector.VehicleAreas.ContainsKey(lane))
{
// get vehicles in the the lane of the exit
List<VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[lane];
// get the point we are looking from
LinePath.PointOnPath referencePoint = lane.LanePath().GetClosestPoint(this.turnFinalWaypoint.Position);
// tmp
VehicleAgent tmp = null;
double tmpDist = Double.MaxValue;
// check over all possible vehicles
foreach (VehicleAgent possible in possibleVehicles)
{
// get vehicle point on lane
LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);
// check if the vehicle is in front of the reference point
double vehicleDist = lane.LanePath().DistanceBetween(referencePoint, vehiclePoint);
if (vehicleDist >= 0 && vehicleDist < TahoeParams.VL * 2.0)
{
tmp = possible;
tmpDist = vehicleDist;
}
}
if (tmp != null)
{
if (this.currentVehicle == null || !this.currentVehicle.Equals(tmp))
{
this.timer.Stop();
this.timer.Reset();
this.currentVehicle = tmp;
}
else
this.currentVehicle = tmp;
if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
this.timer.Start();
}
else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
}
}
else
{
this.currentVehicle = null;
this.timer.Stop();
this.timer.Reset();
}
}
else
{
this.timer.Stop();
this.timer.Reset();
this.currentVehicle = null;
}
}
/// <summary>
/// Resets values held over time
/// </summary>
public void Reset()
{
this.CurrentVehicle = null;
this.currentDistance = 0.0;
this.ResetTiming();
}
/// <summary>
/// Gets closest vehicle on lane before either the exit or where we are clsoest
/// </summary>
/// <param name="ourState"></param>
public void Update(VehicleState ourState)
{
#region Exit Polygon
// check the exit polygon
if (exit != null)
{
// create the polygon
Polygon p = this.exitPolygon;
// closest vehicle
VehicleAgent closest = null;
double closestDist = Double.MinValue;
// check all inside the polygon
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if stopped
if (va.IsStopped)// && va.StateMonitor.Observed.speedValid)
{
// get distance to exit
double distToExit;
Coordinates c = va.ClosestPointTo(this.exit.Position, ourState, out distToExit).Value;
// check inside polygon
if (p.IsInside(c))
{
// check dist
if (closest == null || distToExit < closestDist)
{
closest = va;
closestDist = distToExit;
}
}
}
}
// check closest not null
if (closest != null)
{
// check vehicle switch
if (currentVehicle != closest)
{
this.ResetTiming();
}
// check if waiting timer not running
if (!this.waitingTimer.IsRunning)
{
this.ResetTiming();
this.waitingTimer.Start();
}
// set the new vehicle
this.currentVehicle = closest;
// return to stop update
return;
}
else
{
// check if we were waiting for something in this area
if (this.globalMonitor.PreviouslyWaiting.Contains(this))
{
// remove the previous waiting
this.globalMonitor.PreviouslyWaiting.Remove(this);
}
}
}
#endregion
#region Lane Vehicles
// get the possible vehicles that could be in this stop
if (TacticalDirector.VehicleAreas.ContainsKey(this.lane))
{
// get vehicles in the the lane of the exit
List<VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[this.lane];
// get the point we are looking from
LinePath.PointOnPath referencePoint;
// reference from exit if exists
if (exit != null)
{
LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(exit.Position);
referencePoint = lane.LanePath().AdvancePoint(pop, 3.0);
}
// otherwise look from where we are closest
else
referencePoint = lane.LanePath().GetClosestPoint(ourState.Front);
// tmp
VehicleAgent tmp = null;
double tmpDist = Double.MaxValue;
// check over all possible vehicles in the lane
foreach (VehicleAgent possible in possibleVehicles)
{
// get vehicle point on lane
LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);
// check if the vehicle is behind the reference point
double vehicleDist = lane.LanePath().DistanceBetween(vehiclePoint, referencePoint);
// check for closest
if (vehicleDist > 0 && (tmp == null || vehicleDist < tmpDist))
{
tmp = possible;
tmpDist = vehicleDist;
}
}
// set the closest
this.currentVehicle = tmp;
}
else
{
// set the closest
this.currentVehicle = null;
}
#endregion
// reset the timing if got this far
this.ResetTiming();
}
/// <summary>
/// Updates the montitor with the closest forward vehicle along the lane
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="final"></param>
public void Update(VehicleState vehicleState, ArbiterWaypoint final, LinePath fullPath)
{
this.VehiclesToIgnore = new List<int>();
if (TacticalDirector.VehicleAreas.ContainsKey(final.Lane))
{
List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[final.Lane];
this.CurrentVehicle = null;
double currentDistance = Double.MaxValue;
foreach (VehicleAgent va in laneVehicles)
{
double endDistance = final.Lane.DistanceBetween(final.Position, va.ClosestPosition);
if (endDistance > 0)
this.VehiclesToIgnore.Add(va.VehicleId);
double tmpDist = endDistance;
if (tmpDist > 0 && (this.CurrentVehicle == null || tmpDist < currentDistance))
{
this.CurrentVehicle = va;
currentDistance = tmpDist;
}
}
}
else
this.CurrentVehicle = null;
}
/// <summary>
/// Plans a lane change from the initial lane to the adjacent lane
/// </summary>
/// <param name="initial"></param>
/// <param name="adjacent"></param>
/// <param name="vehicleState"></param>
/// <param name="forwardVehicle"></param>
/// <returns></returns>
public Maneuver? PlanLaneChange(ArbiterLane initial, ArbiterWaypoint goal, LateralReasoning adjacent, VehicleState vehicleState, VehicleAgent forwardVehicle, LaneChangeInformation lci)
{
bool toLeft = initial.LaneOnLeft != null && adjacent.LateralLane.Equals(initial.LaneOnLeft);
LaneChangeParameters updatedLCP;
return this.LaneChangeManeuver(initial, toLeft, goal, vehicleState, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), lci, null, out updatedLCP);
}
/// <summary>
/// Update the monitor
/// </summary>
public void Update()
{
// flag if vehicle exists anymore
bool contains = false;
// get updated agent form tactical direction
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
if (this.intersectionVehicle.Equals(va))
{
Polygon vaP = va.GetAbsolutePolygon(CoreCommon.Communications.GetVehicleState());
if(this.globalMonitor.Intersection.IntersectionPolygon.IsInside(vaP))
{
this.intersectionVehicle = va;
contains = true;
}
}
}
if (contains)
{
if (this.intersectionVehicle.StateMonitor.Observed.speedValid && this.intersectionVehicle.IsStopped && !this.stopwatch.IsRunning)
{
this.ResetTimer();
this.stopwatch.Start();
}
else if(!this.intersectionVehicle.StateMonitor.Observed.speedValid ||
!this.intersectionVehicle.StateMonitor.Observed.isStopped)
{
this.ResetTimer();
}
}
else
{
this.intersectionVehicle = null;
this.ResetTimer();
}
}
/// <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;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="vehicle"></param>
public IntersectionVehicleMonitor(IntersectionMonitor global, VehicleAgent vehicle)
{
this.stopwatch = new Stopwatch();
this.intersectionVehicle = vehicle;
this.globalMonitor = global;
}
/// <summary>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(IFQMPlanable lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List<VehicleAgent> vas = new List<VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// set the vehicles to ignore
this.VehiclesToIgnore = new List<int>();
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// check relatively inside
if (lane.RelativelyInside(frontPos))
{
// distance to vehicle
double frontDist = lane.DistanceBetween(f, frontPos);
// check forward of us
if (frontDist > 0)
{
// add to ignorable
this.VehiclesToIgnore.Add(va.VehicleId);
// check for closest
if (frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
