/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public OpposingLateralReasoning(ArbiterLane lane, SideObstacleSide sos)
{
this.lane = lane;
this.ForwardMonitor = new OpposingForwardQuadrantMonitor();
this.LateralMonitor = new OpposingLateralQuadrantMonitor(sos);
this.RearMonitor = new OpposingRearQuadrantMonitor(lane, sos);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="available"></param>
/// <param name="feasible"></param>
/// <param name="initial"></param>
/// <param name="initialOncoming"></param>
/// <param name="target"></param>
/// <param name="targetOncoming"></param>
/// <param name="toLeft"></param>
/// <param name="behavior"></param>
/// <param name="nextState"></param>
/// <param name="decorators"></param>
/// <param name="parameters"></param>
/// <param name="departUppedBound"></param>
/// <param name="defaultReturnLowerBound"></param>
/// <param name="minimumReturnComplete"></param>
/// <param name="defaultReturnUpperBound"></param>
/// <param name="reason"></param>
public LaneChangeParameters(bool available, bool feasible, ArbiterLane initial, bool initialOncoming,
ArbiterLane target, bool targetOncoming, bool toLeft, Behavior behavior, double distanceToDepartUpperBound,
IState nextState, List <BehaviorDecorator> decorators, TravelingParameters parameters,
Coordinates departUppedBound, Coordinates defaultReturnLowerBound, Coordinates minimumReturnComplete,
Coordinates defaultReturnUpperBound, LaneChangeReason reason)
{
this.Available = available;
this.Feasible = feasible;
this.Initial = initial;
this.InitialOncoming = initialOncoming;
this.Target = target;
this.TargetOncoming = targetOncoming;
this.ToLeft = toLeft;
this.Behavior = behavior;
this.NextState = nextState;
this.Decorators = decorators;
this.Parameters = parameters;
this.DistanceToDepartUpperBound = distanceToDepartUpperBound;
this.DepartUpperBound = departUppedBound;
this.DefaultReturnLowerBound = defaultReturnLowerBound;
this.MinimumReturnComplete = minimumReturnComplete;
this.DefaultReturnUpperBound = defaultReturnUpperBound;
this.Reason = reason;
this.ForcedOpposing = false;
}
/// <summary>
/// Checks if hte opposing lane is clear to pass an opposing vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public bool ClearForDisabledVehiclePass(ArbiterLane lane, VehicleState state, double vUs, Coordinates minReturn)
{
// update the forward vehicle
this.ForwardVehicle.Update(lane, state);
// check if the rear vehicle exists and is moving along with us
if (this.ForwardVehicle.ShouldUseForwardTracker && this.ForwardVehicle.CurrentVehicle != null)
{
// distance from other to us
double currentDistance = lane.DistanceBetween(this.ForwardVehicle.CurrentVehicle.ClosestPosition, state.Front) - (2 * TahoeParams.VL);
double minChangeDist = lane.DistanceBetween(minReturn, state.Front);
// check if he's within min return dist
if (currentDistance > minChangeDist)
{
// params
double vOther = this.ForwardVehicle.CurrentVehicle.StateMonitor.Observed.speedValid ? this.ForwardVehicle.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed;
// get distance of envelope for him to slow to our speed
double xEnvelope = (Math.Pow(vUs, 2.0) - Math.Pow(vOther, 2.0)) / (2.0 * -0.5);
// check to see if vehicle is outside of the envelope to slow down for us after 3 seconds
double xSafe = currentDistance - minChangeDist - (xEnvelope + (vOther * 15.0));
return(xSafe > 0 ? true : false);
}
else
{
return(false);
}
}
else
{
return(true);
}
}
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>
/// Constructor
/// </summary>
/// <param name="lane"></param>
public StayInLaneState(ArbiterLane lane, Probability confidence, IState previous)
{
this.Lane = lane;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId, confidence);
this.IgnorableWaypoints = new List <IgnorableWaypoint>();
this.CheckPreviousState(previous);
}
/// <summary>
/// Check if a side sick blockng obstacle is detected
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
{
try
{
// get distance from vehicle to lane opp side
Coordinates vec = state.Front - state.Position;
vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();
SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
if (sobs != null && sobs.obstacles != null)
{
foreach (SideObstacle so in sobs.obstacles)
{
Coordinates cVec = state.Position + vec.Normalize(so.distance);
if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
{
return(true);
}
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("opposing side sick obstacle exception: " + ex.ToString());
}
return(false);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="leftLateral"></param>
/// <param name="rightLateral"></param>
public OpposingReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, ArbiterLane lane)
{
this.Lane = lane;
this.leftLateralReasoning = leftLateral;
this.rightLateralReasoning = rightLateral;
this.OpposingForwardMonitor = new OpposingForwardQuadrantMonitor();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="stopPoint"></param>
/// <param name="currentPosition"></param>
/// <param name="timeStamp"></param>
public StoppingState(ArbiterLane lane, Coordinates stopPoint, Coordinates currentPosition, double timeStamp)
{
this.lane = lane;
this.stopPoint = stopPoint;
this.currentPosition = currentPosition;
this.timeStamp = timeStamp;
this.internalState = new InternalState(lane.LaneId, lane.LaneId);
}
/// <summary>
/// Plans the forward maneuver and secondary maneuver
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver ForwardManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, RoadPlan roadPlan,
List <ITacticalBlockage> blockages)
{
// get primary maneuver
Maneuver primary = this.OpposingForwardMonitor.PrimaryManeuver(arbiterLane, closestGood, vehicleState, blockages);
// return primary for now
return(primary);
}
/// <summary>
/// Determines proper speed commands given we want to stop at a certain waypoint
/// </summary>
/// <param name="waypoint"></param>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="enCovariance"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance,
out double stopSpeed, out double stopDistance)
{
// get dist to waypoint
stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance;
// speed tools
stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24);
}
/// <summary>
/// Updates evidence
/// </summary>
/// <param name="observations"></param>
public void UpdateEvidence(List <AreaEstimate> observations)
{
// dictionary mapping lanes to values
Dictionary <ArbiterLane, double> areaEstimates = new Dictionary <ArbiterLane, double>();
// loop over area possibilities
foreach (AreaEstimate ae in observations)
{
// do nothing for now and return first for nav tests
if (ae.AreaType == StateAreaType.Lane)
{
// get lane
ArbiterLane al = this.GetLane(ae.AreaId);
// assign
if (areaEstimates.ContainsKey(al))
{
// update
areaEstimates[al] = areaEstimates[al] + ae.Probability;
}
else
{
// add new
areaEstimates.Add(al, ae.Probability);
}
}
}
// output
if (this.InternalState.Count > 0)
{
foreach (KeyValuePair <ArbiterLane, double> kvp in areaEstimates)
{
if (kvp.Key.LaneId.Equals(this.InternalState[this.InternalState.Count - 1].Initial))
{
CoreCommon.CurrentInformation.LAPosteriorProbInitial = kvp.Value.ToString("F6");
}
if (kvp.Key.LaneId.Equals(this.InternalState[this.InternalState.Count - 1].Target))
{
CoreCommon.CurrentInformation.LAPosteriorProbTarget = kvp.Value.ToString("F6");
}
}
}
if (this.FilteredEstimate.Count > 0)
{
Probability p = this.FilteredEstimate[this.FilteredEstimate.Count - 1];
CoreCommon.CurrentInformation.LAProbabilityCorrect = p.T.ToString("F6");
}
// new estimate
PosteriorEvidence pe = new PosteriorEvidence(areaEstimates);
// add
PosteriorEvidence.Add(pe);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
}
/// <summary>
/// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane
/// </summary>
/// <param name="closestGood"></param>
/// <param name="coordinates"></param>
/// <param name="ignorable"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStopType"></param>
/// <param name="navStop"></param>
private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance,
out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop)
{
ArbiterWaypoint current = null;
double minDist = Double.MaxValue;
StopType st = StopType.EndOfLane;
#region Closest Good Parameterization
foreach (ArbiterWaypoint aw in closestGood.WaypointList)
{
if (aw.IsStop || aw.NextPartition == null)
{
double dist = closestGood.DistanceBetween(coordinates, aw.Position);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
#region Opposing Parameterization
ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial;
int startIndex = opposing.WaypointList.IndexOf(opStart);
for (int i = startIndex; i >= 0; i--)
{
ArbiterWaypoint aw = opposing.WaypointList[i];
if (aw.IsStop || aw.PreviousPartition == null)
{
double dist = opposing.DistanceBetween(aw.Position, coordinates);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
double tmpDistanceIgnore;
this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore);
navStop = current;
navStopDistance = minDist;
navStopType = st;
}
/// <summary>
/// Updates the queuing monitors of all the vehicles
/// </summary>
public void UpdateQueuingMonitors(double currentTs)
{
List <VehicleAgent> updatedLowPri = new List <VehicleAgent>();
foreach (KeyValuePair <IVehicleArea, List <VehicleAgent> > areaVehicles in TacticalDirector.VehicleAreas)
{
if (areaVehicles.Key is ArbiterLane)
{
ArbiterLane al = (ArbiterLane)areaVehicles.Key;
foreach (VehicleAgent va in areaVehicles.Value)
{
if (!updatedLowPri.Contains(va))
{
if (va.IsStopped && va.StateMonitor.Observed.speedValid)
{
bool inSafety = false;
bool inInter = false;
foreach (ArbiterSafetyZone asz in al.SafetyZones)
{
if (asz.IsInSafety(va.ClosestPosition))
{
inSafety = true;
}
}
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(va.ClosestPosition))
{
inInter = true;
}
}
if (!inSafety && !inInter)
{
va.QueuingState.Update(QueueingUpdate.Queueing, currentTs);
updatedLowPri.Add(va);
}
else
{
va.QueuingState.Update(QueueingUpdate.NotQueueing, currentTs);
}
}
else
{
va.QueuingState.Update(QueueingUpdate.NotQueueing, currentTs);
}
}
}
}
}
}
// Is the posterior evidence consistent over time?
private bool Ct(List <PosteriorEvidence> e1t)
{
ArbiterLane lane = null;
if (e1t.Count == 50)
{
// check if previous are consistent
for (int i = 0; i < e1t.Count; i++)
{
if (e1t[i].LaneProbabilities.Count > 0)
{
double max = Double.MinValue;
ArbiterLane curLane = null;
foreach (KeyValuePair <ArbiterLane, double> est in e1t[i].LaneProbabilities)
{
if (est.Value > max || curLane == null)
{
max = est.Value;
curLane = est.Key;
}
}
if (lane == null)
{
lane = curLane;
}
else
{
if (!lane.Equals(curLane))
{
return(false);
}
}
}
}
if (lane == null)
{
return(false);
}
else
{
this.sceneLikelyLane = lane;
CoreCommon.CurrentInformation.LASceneLikelyLane = this.sceneLikelyLane.ToString();
return(true);
}
}
else
{
this.sceneLikelyLane = null;
return(false);
}
}
/// <summary>
/// Plan given that we are starting on a road
/// </summary>
/// <param name="currentLane"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
public RoadPlan PlanRoads(ArbiterLane currentLane, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable)
{
// get all downstream points of interest
List <DownstreamPointOfInterest> downstreamPoints = new List <DownstreamPointOfInterest>();
// get exits downstream from this current position in the way
downstreamPoints.AddRange(this.Downstream(currentPosition, currentLane.Way, true, ignorable));
// determine if goal is downstream in a specific lane, add to possible route times to consider
DownstreamPointOfInterest goalDownstream = this.IsGoalDownStream(currentLane.Way, currentPosition, goal);
// add goal to points downstream if it exists
if (goalDownstream != null)
{
downstreamPoints.Add(goalDownstream);
}
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.DetermineDownstreamPointRouteTimes(downstreamPoints, goal, currentLane.Way);
// get road plan
RoadPlan rp = this.GetRoadPlan(downstreamPoints, currentLane.Way);
// update arbiter information
List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition);
// make sure we're in a road state
if (CoreCommon.CorePlanningState == null ||
CoreCommon.CorePlanningState is TravelState ||
CoreCommon.CorePlanningState is TurnState)
{
// check route 1
if (routeInfo.Count > 0)
{
RouteInformation ri = routeInfo[0];
CoreCommon.CurrentInformation.Route1 = ri;
CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint;
}
// check route 2
if (routeInfo.Count > 1)
{
RouteInformation ri = routeInfo[1];
CoreCommon.CurrentInformation.Route2 = ri;
CoreCommon.CurrentInformation.Route2Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route2Wp = ri.Waypoint;
}
}
// return road plan
return(rp);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="lane"></param>
/// <param name="waypoint"></param>
/// <param name="turnDirection"></param>
/// <param name="isNavigationExit"></param>
/// <param name="desiredExit"></param>
public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition)
{
this.lane = lane;
this.waypoint = waypoint;
this.turnDirection = turnDirection;
this.isNavigationExit = isNavigationExit;
this.desiredExit = desiredExit;
this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
this.timeStamp = timeStamp;
this.currentPosition = currentPosition;
this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
/// <summary>
/// Set the road plan and populate the tasks
/// </summary>
/// <param name="rp"></param>
/// <param name="current"></param>
public void SetRoadPlan(RoadPlan rp, ArbiterLane current)
{
Dictionary <ArbiterLaneId, LanePlan> plans = rp.LanePlans;
// left
List <LanePlan> left = new List <LanePlan>();
// straight
List <LanePlan> straight = new List <LanePlan>();
// right
List <LanePlan> right = new List <LanePlan>();
// tmp
ArbiterLane temp = current.LaneOnLeft;
// left
while (temp != null && temp.Way.Equals(current.Way))
{
if (plans.ContainsKey(temp.LaneId))
{
left.Add(plans[temp.LaneId]);
}
temp = temp.LaneOnLeft;
}
// right
temp = current.LaneOnRight;
while (temp != null && temp.Way.Equals(current.Way))
{
if (plans.ContainsKey(temp.LaneId))
{
right.Add(plans[temp.LaneId]);
}
temp = temp.LaneOnRight;
}
// straight
if (plans.ContainsKey(current.LaneId))
{
straight.Add(plans[current.LaneId]);
}
// create tasks
tasks = new Dictionary <TypeOfTasks, List <LanePlan> >();
tasks.Add(TypeOfTasks.Left, left);
tasks.Add(TypeOfTasks.Straight, straight);
tasks.Add(TypeOfTasks.Right, right);
}
public static Polygon DefaultLanePolygon(ArbiterLane al)
{
// fist get the right boundary of the lane
LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0);
LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0);
rb.Reverse();
List <Coordinates> defaultPoly = lb;
defaultPoly.AddRange(rb);
// start the polygon
Polygon poly = new Polygon(defaultPoly);
return(poly);
}
public static Polygon LanePolygon(ArbiterLane al)
{
// fist get the right boundary of the lane
LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0);
LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0);
rb.Reverse();
List <Coordinates> defaultPoly = lb;
defaultPoly.AddRange(rb);
// start the polygon
Polygon poly = new Polygon(defaultPoly);
// loop through partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
//if (alp.Initial.PreviousPartition != null && alp.Final.NextPartition != null)
//{
// get the good polygon
Polygon pPoly = PartitionPolygon(alp);
// check not null
if (pPoly != null)
{
poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, pPoly }));
}
//}
}
// circles for intersections of partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
if (alp.Final.NextPartition != null)
{
double interAngle = Math.Abs(FinalIntersectionAngle(alp));
if (interAngle > 15)
{
Circle connect = new Circle((alp.Lane.Width / 2.0) + (interAngle / 15.0 * 0.5), alp.Final.Position);
poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, connect.ToPolygon(16) }));
}
}
}
// return the polygon
return(poly);
}
/// <summary>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(ArbiterLane lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath().Clone();
p.Reverse();
// 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;
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// gets distance from other vehicle to us along the lane
double frontDist = lane.DistanceBetween(frontPos, f);
if (frontDist >= 0 && frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <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]);
}
protected override bool InSafetyZone()
{
if (laneID != null && Services.RoadNetwork != null)
{
ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID];
if (lane.SafetyZones != null)
{
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
foreach (ArbiterSafetyZone safetyZone in lane.SafetyZones)
{
if (safetyZone.IsInSafety(pose.xy))
{
return(true);
}
}
}
}
return(false);
}
public OpposingLanesState(ArbiterLane opposingLane, bool resetLaneAgent, IState previous, VehicleState vs)
{
this.resetLaneAgent = resetLaneAgent;
this.OpposingLane = opposingLane;
this.OpposingWay = opposingLane.Way.WayId;
this.ClosestGoodLane = null;
this.SetClosestGood(vs);
if (previous is ChangeLanesState)
{
EntryParameters = ((ChangeLanesState)previous).Parameters;
}
else if (previous is OpposingLanesState)
{
EntryParameters = ((OpposingLanesState)previous).EntryParameters;
}
else
{
EntryParameters = null;
}
}
public void SetClosestGood(VehicleState vs)
{
ArbiterLane current = this.OpposingLane.LaneOnLeft;
while (current != null)
{
if (!current.Way.WayId.Equals(OpposingWay) && current.RelativelyInside(vs.Front))
{
this.ClosestGoodLane = current;
break;
}
if (!current.Way.WayId.Equals(this.OpposingLane.Way.WayId))
{
current = current.LaneOnRight;
}
else
{
current = current.LaneOnLeft;
}
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="globalMonitor"></param>
/// <param name="involved"></param>
public DominantLaneEntryMonitor(IntersectionMonitor globalMonitor, IntersectionInvolved involved)
{
this.waitingTimer = new Stopwatch();
this.globalMonitor = globalMonitor;
this.lane = (ArbiterLane)involved.Area;
this.involved = involved;
if (involved.Exit != null)
{
this.exit = (ArbiterWaypoint)involved.Exit;
}
else
{
this.exit = null;
}
if (this.exit != null)
{
// create the polygon
this.exitPolygon = this.ExitPolygon();
}
}
/// <summary>
/// Checks if we should pass the forward vehicle
/// </summary>
/// <param name="lci"></param>
/// <param name="lane"></param>
/// <returns></returns>
public bool ShouldPass(out LaneChangeInformation lci, ArbiterLane lane)
{
// passing reason set to none by default
lci = new LaneChangeInformation(LaneChangeReason.NotApplicable, this.CurrentVehicle);
// check the queuing state of the forward vehicle
if (this.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
lci = new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.CurrentVehicle);
return(true);
}
// check inside any safety zone
foreach (ArbiterSafetyZone asz in lane.SafetyZones)
{
if (asz.IsInSafety(this.CurrentVehicle.ClosestPosition))
{
return(false);
}
}
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(this.CurrentVehicle.ClosestPosition))
{
return(false);
}
}
if ((this.CurrentVehicle.Speed < CoreCommon.Communications.GetVehicleSpeed().Value ||
(this.CurrentVehicle.IsStopped && this.CurrentVehicle.StateMonitor.Observed.speedValid)) &&
this.CurrentVehicle.Speed < 0.7 * lane.Way.Segment.SpeedLimits.MaximumSpeed)
{
lci = new LaneChangeInformation(LaneChangeReason.SlowForwardVehicle, this.CurrentVehicle);
return(true);
}
// fall out
return(false);
}
public bool VehiclePassableInPolygon(ArbiterLane al, Polygon p, VehicleState ourState, Polygon circ)
{
List <Coordinates> vhcCoords = new List <Coordinates>();
for (int i = 0; i < this.trackedCluster.relativePoints.Length; i++)
{
vhcCoords.Add(this.TransformCoordAbs(this.trackedCluster.relativePoints[i], ourState));
}
Polygon vehiclePoly = Polygon.GrahamScan(vhcCoords);
vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
ArbiterLanePartition alp = al.GetClosestPartition(this.trackedCluster.closestPoint);
List <Coordinates> pointsOutside = new List <Coordinates>();
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>
/// Gets priotiy lane determination
/// </summary>
/// <param name="ii"></param>
/// <param name="path"></param>
/// <param name="end"></param>
/// <returns></returns>
private Coordinates?LaneIntersectsPath(IntersectionInvolved ii, LinePath path, IArbiterWaypoint end)
{
ArbiterLane al = (ArbiterLane)ii.Area;
LinePath.PointOnPath current = path.StartPoint;
bool go = true;
while (go) // && !(current.Location.DistanceTo(path.EndPoint.Location) < 0.1))
{
Coordinates alClose = al.LanePath().GetClosestPoint(current.Location).Location;
double alDist = alClose.DistanceTo(current.Location);
if (alDist <= 0.05)
{
return(al.LanePath().GetClosestPoint(current.Location).Location);
}
if (current.Location.Equals(path.EndPoint.Location))
{
go = false;
}
current = path.AdvancePoint(current, 0.1);
}
/*if (ii.Exit != null)
* {
* ITraversableWaypoint laneExit = ii.Exit;
* foreach (ArbiterInterconnect tmpAi in laneExit.OutgoingConnections)
* {
* if (tmpAi.FinalGeneric.Equals(end))
* {
* return tmpAi.FinalGeneric.Position;
* }
* }
* }*/
return(null);
}
