/// <summary> /// Plans over an intersection /// </summary> /// <param name="exitWaypoint"></param> /// <param name="goal"></param> /// <returns></returns> public IntersectionPlan PlanIntersection(ITraversableWaypoint exitWaypoint, INavigableNode goal) { // road plan if the itnersection has a road available to take from it RoadPlan rp = null; // check if road waypoint if (exitWaypoint is ArbiterWaypoint) { // get exit ArbiterWaypoint awp = (ArbiterWaypoint)exitWaypoint; // check if it has lane partition moving outwards if (awp.NextPartition != null) { // road plan ignoring exit List <ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(awp.Lane.Way, awp.Position, awp); rp = this.PlanNavigableArea(awp.Lane, awp.Position, goal, iws); } } // get exit plan IntersectionPlan ip = this.GetIntersectionExitPlan(exitWaypoint, goal); // add road plan if exists ip.SegmentPlan = rp; // return the plan return(ip); }
/// <summary> /// Check if we can go /// </summary> /// <param name="vs"></param> private bool CanGo(VehicleState vs) { #region Moving Vehicles Inside Turn // check if we can still go through this turn if (TacticalDirector.VehicleAreas.ContainsKey(this.turn)) { // get the subpath of the interconnect we care about LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); if (aiSubpath.PathLength > 4.0) { aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0); // get vehicles List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn]; // loop vehicles foreach (VehicleAgent va in turnVehicles) { // check if inside turn LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition); if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint)) { ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping"); return(false); } } } } #endregion // test if this turn is part of an intersection if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId)) { // intersection ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId]; // check if priority lanes exist for this interconnect if (inter.PriorityLanes.ContainsKey(this.turn)) { // get all the default priority lanes List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn]; // get the subpath of the interconnect we care about //LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position })); //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); // check if path ended if (aiSubpath.Count < 2) { return(true); } // determine all of the new priority lanes List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>(); #region Determine new priority areas given position // loop through old priorities foreach (IntersectionInvolved ii in priorities) { // check ii lane if (ii.Area is ArbiterLane) { #region Lane Intersects Turn Path w/ Point of No Return // check if the waypoint is not the last waypoint in the lane if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null) { // check where line intersects path Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric); // check for an intersection if (intersect.HasValue) { // distance to intersection double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0; // determine if we can stop before or after the intersection double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value); // check dist to intersection > distance to stoppage if (distanceToIntersection > distanceToStoppage) { // add updated priority updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)), ii.Area, ArbiterTurnDirection.Straight)); } else { ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString()); } } } #endregion // we know there is an exit and it is the last waypoint of the segment, fuxk! else { #region Turns Intersect // get point to look at if exists ArbiterInterconnect interconnect; Coordinates? intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect); // check for the intersect if (intersect.HasValue) { ArbiterLane al = (ArbiterLane)ii.Area; LinePath lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position)); lp.Add(interconnect.InterconnectPath.EndPoint.Location); // get our time to the intersection point //double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // get closest vehicle in that lane to the intersection List <VehicleAgent> toLook = new List <VehicleAgent>(); if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area]) { double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position); if (upstreamDist > 0 && tmpVa.PassedDelayedBirth) { toLook.Add(tmpVa); } } } if (TacticalDirector.VehicleAreas.ContainsKey(interconnect)) { toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]); } // check length of stuff to look at if (toLook.Count > 0) { foreach (VehicleAgent va in toLook) { // distance along path to location of intersect double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location)); double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double vaTime = distToIntersect / Math.Abs(speed); if (vaTime > 0 && vaTime < ourTime) { ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO"); return(false); } } } } #endregion } } } #endregion #region Updated Priority Intersection Code // loop through updated priorities bool updatedPrioritiesClear = true; foreach (IntersectionInvolved ii in updatedPriorities) { // lane ArbiterLane al = (ArbiterLane)ii.Area; // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get closest vehicle in that lane to the intersection if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { // get lane vehicles List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area]; // determine for all VehicleAgent closestLaneVa = null; double closestDistanceVa = Double.MaxValue; double closestTime = Double.MaxValue; foreach (VehicleAgent testVa in vas) { // check upstream double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position); // get speed double speed = testVa.Speed; double time = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // check distance > 0 if (distance > 0) { // check if closer or none other exists if (closestLaneVa == null || time < closestTime) { closestLaneVa = testVa; closestDistanceVa = distance; closestTime = time; } } } // check if closest exists if (closestLaneVa != null) { // set va VehicleAgent va = closestLaneVa; double distance = closestDistanceVa; // check dist and birth time if (distance > 0 && va.PassedDelayedBirth) { // check time double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double time = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // too close if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) && distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) && CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping"); //return false; updatedPrioritiesClear = false; return(false); } else if (time > 0 && time < ourTime) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return false; updatedPrioritiesClear = false; return(false); } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return true; } } } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles"); } } } return(updatedPrioritiesClear); #endregion } } // fall through fine to go ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream"); return(true); }