void DebugDrawGraphGizmo(RoadGraph graph, Color edgeColor, Color nodeColor) { if (graph == null || graph.nodes == null) { return; } // Create a hash table of the nodes for fast lookup var nodes = new Dictionary <int, RoadGraphNode>(); foreach (var node in graph.nodes) { nodes.Add(node.nodeId, node); } // Draw the edges Gizmos.color = edgeColor; foreach (var node in graph.nodes) { foreach (var edge in node.adjacentEdges) { var otherNode = nodes[edge.otherNodeId]; Gizmos.DrawLine(node.position, otherNode.position); } } // Draw the nodes Gizmos.color = nodeColor; foreach (var node in graph.nodes) { Gizmos.DrawSphere(node.position, .25f); } }
private void LocateAgent() { for (int i = 0; i < number_of_carriges; i++) { currentLength[i] = -i * (Size.X + 2); } if (WayPointsList.Count > 1) { CheckPointsList = new List <WayPoint>(); for (int i = 1; i < WayPointsList.Count; i++) { RoadGraph.ViewedNodes.Clear(); foreach (var node in RoadGraph.GetNodesWay(WayPointsList[i - 1], WayPointsList[i])) { if (CheckPointsList.Count == 0 || !CheckPointsList[CheckPointsList.Count - 1].Equals(node)) { CheckPointsList.Add(node); } } } positions[0] = CheckPointsList[0].LocationPoint; position = CheckPointsList[0].LocationPoint; need_draw[0] = true; CurrentSpeed = MaxSpeed; } }
/// <summary> /// Build graph from OSM elements. May contains duplicated elements (from multiple calls) /// </summary> /// <param name="elements"></param> /// <returns></returns> private static RoadGraph BuildGraph(List <Element> elements, bool ignoreOneWays) { RoadGraph result = new RoadGraph(); IEnumerable <Way> ways = elements.Where(x => x is Way).Select(x => x as Way).Distinct(); var nodeLookup = new Dictionary <long, OsmNode>(); foreach (var node in elements.Where(x => x is OsmNode).Select(x => x as OsmNode)) { nodeLookup[node.Id] = node; } var intersectionCounter = ways.Select(x => x.Nodes).CountRepeatedIds(); foreach (var way in ways) { string wayName = way.ParseName(); var subways = way.Nodes.InclusivePartitioning(id => intersectionCounter[id] > 1); foreach (var subway in subways) { // TODO : why are ids not in dictinoary sometimes? Data problem? var roadShape = subway.Where(id => nodeLookup.ContainsKey(id)).Select(id => nodeLookup[id].ToCoord()).ToList(); DirectedRoad road = new DirectedRoad(subway.First().ToString(), subway.Last().ToString(), roadShape, wayName); result.AddRoad(road); if (!way.IsOneWay() || ignoreOneWays) { DirectedRoad reverseRoad = road.Reverse(); result.AddRoad(reverseRoad); } } } return(result); }
public MapMatcher(List <T> data, Func <T, DirectedRoad> dataToRoad, MapMatcherParameters parameters, bool useSearchGrid = true, BoundingBox boundingBox = null) { // Initialize parameters Parameters = parameters; //Build graph _dataByRoadId = new Dictionary <string, T>(); var graph = new RoadGraph(); foreach (var datum in data) { var road = dataToRoad(datum); graph.AddRoad(road); _dataByRoadId[road.Squid] = datum; } Graph = graph; // Compute search grid (for accessing nearby roads) if (useSearchGrid) { // Compute bounding box if not given if (boundingBox == null) { boundingBox = graph.Nodes.Values.GetBoundingBox(); } SearchGrid = SearchGridFactory.ComputeSearchGrid(graph, parameters.NearbyRoadsThreshold, boundingBox); } // Initialize state State = MapMatchState.InitialState(); }
public HashSet <HotRoute> Run(RoadGraph roadGraph, int eps, int minTraffic) { this.eps = eps; this.minTraffic = minTraffic; this.roadGraph = roadGraph; hotRoutes = new HashSet <HotRoute>(); hotRouteStarts = new HashSet <Connection>(); //var test = roadGraph.Connections.Where(c => c.Traffic.Count() >= minTraffic); //Console.WriteLine(test.ToList().Count); this.aStart = new AstarPathfinder(this.roadGraph); FindHotRouteStarts(); Console.WriteLine("Return FindHotRouteStars(): " + hotRouteStarts.Count); foreach (var hrs in hotRouteStarts) { var r = new HotRoute(hrs); ExtendHotRoutes(ref r); hotRoutes.Add(r); } return(hotRoutes); }
//public static void PlotGraph(RoadGraph graph, GMapOverlay overlay, Color color) //{ // foreach (var intersection in graph.Values) // { // //PlotPoint(nodeCoord.Latitude, nodeCoord.Longitude, GMarkerGoogleType.blue_dot,overlay); // foreach (var link in intersection.Links) // { // PlotPolyLine(link, overlay, color, 3, true); // } // } //} public static void PlotGraph(RoadGraph graph, GMapOverlay overlay, Color color) { foreach (var road in graph.Roads) { PlotRoute(road.Geometry, overlay, color); } }
public static RoadSearchGrid ComputeSearchGrid(RoadGraph graph, double gridSizeInMeters, BoundingBox boundingBox = null) { var stopwatch = new System.Diagnostics.Stopwatch(); stopwatch.Start(); double minLat = double.NaN; double maxLat = double.NaN; double minLng = double.NaN; double maxLng = double.NaN; if (boundingBox != null) { minLat = boundingBox.LatLowerBound; maxLat = boundingBox.LatUpperBound; minLng = boundingBox.LngLowerBound; maxLng = boundingBox.LngUpperBound; } else { List <Coord> allCoords = graph.Roads.SelectMany(x => x.Geometry).ToList(); var latitudes = allCoords.Select(x => x.Latitude); var longitudes = allCoords.Select(x => x.Longitude); minLat = latitudes.Min(); maxLat = latitudes.Max(); minLng = longitudes.Min(); maxLng = longitudes.Max(); } // Get rough lat/lng delta values corresponding to the grid size double refLatInRadians = (((minLat + maxLat) / 2) * (Math.PI / 180.0)); double roughLngGridSize = ((gridSizeInMeters / (Constants.Earth_Radius_In_Meters * Math.Cos(refLatInRadians))) * (180.0 / Math.PI)); double roughLatGridSize = ((gridSizeInMeters / Constants.Earth_Radius_In_Meters) * (180.0 / Math.PI)); // Get total width and height of the grid double lngWidth = (maxLng + roughLngGridSize) - (minLng - roughLngGridSize); double latHeight = (maxLat + roughLatGridSize) - (minLat - roughLatGridSize); // Corners are the minimum values with an added margin double left = minLng - roughLngGridSize; double bottom = minLat - roughLatGridSize; // Dimensions are the total length over the grid size ( may not divide in perfectly) int cellCountX = (int)(lngWidth / roughLngGridSize); int cellCountY = (int)(latHeight / roughLatGridSize); // Actual grid sizes are obtained from the above dimensions and the length the sides double gridSizeX = lngWidth / cellCountX; double gridSizeY = latHeight / cellCountY; var result = new RoadSearchGrid(left, bottom, gridSizeX, gridSizeY, cellCountX, cellCountY); // TODO: correct roads input below?? result.Populate(graph.Values.SelectMany(x => x)); stopwatch.Stop(); return(result); }
private void LocateAgent() { if (WayPointsList.Count > 1) { RoadGraph.ViewedNodes.Clear(); CheckPointsList = RoadGraph.GetNodesWay(WayPointsList[0], WayPointsList[WayPointsList.Count - 1]); position = CheckPointsList[0].LocationPoint; CurrentSpeed = MaxSpeed; } }
public static Transition TransitionProbability(RoadGraph graph, ProjectToRoad projection1, ProjectToRoad projection2, MapMatcherParameters parameters) { DirectedRoad road1 = projection1.Road; DirectedRoad road2 = projection2.Road; //calculate on road distance double onRoadDistanceInMeters; Distance startingDist = projection1.DistanceToEnd; Distance endDist = projection2.DistanceFromStart; // Roads are the same: if (road1.Equals(road2)) { //negative if going backwards along road onRoadDistanceInMeters = projection2.DistanceFromStart.DistanceInMeters - projection1.DistanceFromStart.DistanceInMeters; } // Road start or end on the same node else if (road1.End == road2.End || road1.Start == road2.Start) { //make this transition impossible return(Transition.ImpossibleTransition(road1, road2)); } // Roads are connected (can be same road in opposite direction) else if (road1.End == road2.Start) { onRoadDistanceInMeters = startingDist.DistanceInMeters + endDist.DistanceInMeters; } // Try connect roads using Dijstra else { List <DirectedRoad> path; if (PathFinding.DijstraTryFindPath(graph, road1.End, road2.Start, parameters.DijstraUpperSearchLimit, out path)) { Distance connectingDist = Distance.Zero; foreach (var road in path) { connectingDist += road.Length; } onRoadDistanceInMeters = startingDist.DistanceInMeters + connectingDist.DistanceInMeters + endDist.DistanceInMeters; } else { //cannot connect up roads. transition probability is zero return(Transition.ImpossibleTransition(road1, road2)); } } Distance haversineDistance = projection1.Coordinate.HaversineDistance(projection2.Coordinate); return(new Transition(onRoadDistanceInMeters, haversineDistance.DistanceInMeters, road1, road2, parameters)); }
private void Button_Click(object sender, RoutedEventArgs e) { bool has_dont_linked = false; for (int i = 0; i < VertexVisualsList.Count; i++) { if (VertexVisualsList[i].InCount == 0 && VertexVisualsList[i].OutCount == 0) { has_dont_linked = true; } } if (has_dont_linked) { if (MessageBox.Show("Все точки, не имеющие связей (обозначены серым цветом) будут удалены. Продолжить?", "Внимание", MessageBoxButton.YesNo) == MessageBoxResult.No) { return; } List <WayPoint> remList = new List <WayPoint>(); foreach (var node in RoadGraph.Nodes) { int in_count = 0, out_count = 0; foreach (var edge in RoadGraph.GetEdgesTo(node)) { in_count++; } foreach (var edge in RoadGraph.GetEdgesFrom(node)) { out_count++; } if (in_count == 0 && out_count == 0) { remList.Add(node); } } for (int i = 0; i < remList.Count; i++) { RoadGraph.Remove(remList[i]); } } //Собираем граф дорожной сети //RoadGraph = new Graph<WayPoint, PathFigure>(); for (int i = 0; i < VertexVisualsList.Count; i++) { RoadGraph.Add(VertexVisualsList[i].Node); } for (int i = 0; i < EdgeVisualsList.Count; i++) { RoadGraph.AddEdge(EdgeVisualsList[i].NodeFrom, EdgeVisualsList[i].NodeTo, EdgeVisualsList[i].PathData); } DialogResult = true; Close(); }
public OsmMapMatcher(RoadGraph graph, MapMatcherParameters parameters = null, bool useSearchGrid = true) { if (parameters == null) { parameters = MapMatcherParameters.Default; } Parameters = parameters; if (useSearchGrid) { SearchGrid = SearchGridFactory.ComputeSearchGrid(graph, DefaultValues.Nearby_Road_Radius_In_Meters); } Graph = graph; State = MapMatchState.InitialState(); }
private void Awake() { if (Instance == null) { Instance = this; } if (Instance != this) { Destroy(gameObject); } roadGraph = new RoadGraph(); roadGraph.Init(); }
private void LocateAgent() { if (RouteList.Count > 1 || RoadGraph == null) { RoadGraph.ViewedNodes.Clear(); RouteList = RoadGraph.GetNodesWay(RouteList.First(), RouteList.Last()); Position = RouteList.First().Location; base.CurrentSpeed = base._maxSpeed; _located = true; } else { RouteList.Clear(); } }
public static RoadGraph BuildGraph1() { RoadGraph result = new RoadGraph(); DirectedRoad roadAB = new DirectedRoad("1000", "1001", GeometryAB().ToCoordList(), "AB"); DirectedRoad roadAC = new DirectedRoad("1000", "1002", GeometryAC().ToCoordList(), "AC"); DirectedRoad roadAD = new DirectedRoad("1000", "1003", GeometryAD().ToCoordList(), "AD"); DirectedRoad roadDB = new DirectedRoad("1003", "1001", GeometryDB().ToCoordList(), "DB"); result.AddRoad(roadAB); result.AddRoad(roadAC); result.AddRoad(roadAD); result.AddRoad(roadDB); return result; }
//////////////// // Graph where A->B, B->C is shorter than going A->C public static RoadGraph BuildGraphTwoShortRoadsOneLong() { RoadGraph result = new RoadGraph(); // Two short roads DirectedRoad roadAB = new DirectedRoad("1000", "1001", StraightAB().ToCoordList(), "AB"); DirectedRoad roadBC = new DirectedRoad("1001", "1002", StraightBC().ToCoordList(), "BC"); //Long road DirectedRoad roadAC = new DirectedRoad("1000", "1002", BigTriangleAC().ToCoordList(), "AC"); result.AddRoad(roadAB); result.AddRoad(roadBC); result.AddRoad(roadAC); return result; }
public MapInfo(int mapId, int width, int height, MapSettings settings, int numberOfPlayers, CancellationToken cancellationToken) { token = cancellationToken; MapId = mapId; Settings = settings; ObjectFactory = new ObjectFactory(); Tiles = new Grid <TileInfo>(width, height); TextureData = new Grid <Texture>(width + 1, height + 1); StartingPositions = new Coordinates[numberOfPlayers]; Roads = new RoadGraph(); FlattenedAreas = new List <Circle>(); BaseRadius = 40; ResourceRadiusNear = 5; ResourceRadiusFar = 40; MapIniTemplates = new List <string>(); }
private Point CalculateCoordinateLocationOfCarrige() { double tmp = currentLength; for (int i = 1; i < CheckPointsList.Count; i++) { //Поиск дороги PathFigure road = null; var edges = RoadGraph.GetEdgesFrom(CheckPointsList[i - 1]).GetEnumerator(); while (edges.MoveNext()) { if (edges.Current.End.Equals(CheckPointsList[i])) { road = edges.Current.Data; break; } } //Если дорога не найдена if (road == null) { CalculateAngle(position, CheckPointsList[i].LocationPoint); position = CheckPointsList[i].LocationPoint; return(new Point(-1, -1)); } //Если найдена double road_lenght = CalculateLength(road); //Если этот участок уже пройден, то смотрим следующий if (tmp > road_lenght) { tmp -= road_lenght; continue; } for (int g = 0; g < road.Segments.Count; g++) { double x = 0, y = 0, t = 0, seg_l = 0; Point startPoint = road.StartPoint; if (road.Segments[g] is BezierSegment) { BezierSegment seg = road.Segments[g] as BezierSegment; seg_l = CalculateLength(new PathFigure(startPoint, new List <PathSegment>() { seg }, false)); if (seg_l < tmp) { tmp -= seg_l; startPoint = seg.Point3; continue; } t = tmp / seg_l; x = (1 - t) * (1 - t) * (1 - t) * startPoint.X + (1 - t) * (1 - t) * 3 * t * seg.Point1.X + (1 - t) * 3 * t * t * seg.Point2.X + t * t * t * seg.Point3.X; y = (1 - t) * (1 - t) * (1 - t) * startPoint.Y + (1 - t) * (1 - t) * 3 * t * seg.Point1.Y + (1 - t) * 3 * t * t * seg.Point2.Y + t * t * t * seg.Point3.Y; } else if (road.Segments[g] is LineSegment) { LineSegment seg = road.Segments[g] as LineSegment; seg_l = CalculateLength(new PathFigure(startPoint, new List <PathSegment>() { seg }, false)); if (seg_l < tmp) { tmp -= seg_l; startPoint = seg.Point; continue; } t = tmp / seg_l; x = seg.Point.X * t + startPoint.X * (1 - t); if (startPoint.X == seg.Point.X) { y = startPoint.Y + (seg.Point.Y - startPoint.Y) * t; } else { y = (x - startPoint.X) * (seg.Point.Y - startPoint.Y) / (seg.Point.X - startPoint.X) + startPoint.Y; } } else if (road.Segments[g] is PolyLineSegment) { PolyLineSegment seg = road.Segments[g] as PolyLineSegment; int l; for (l = 0; l < seg.Points.Count; l++) { seg_l = Math.Sqrt(Math.Pow(startPoint.X - seg.Points[l].X, 2) + Math.Pow(startPoint.Y - seg.Points[l].Y, 2)); if (seg_l < tmp) { tmp -= seg_l; startPoint = seg.Points[l]; continue; } } t = tmp / seg_l; x = seg.Points[l].X * t + startPoint.X * (1 - t); y = (x - startPoint.X) * (seg.Points[l].Y - startPoint.Y) / (seg.Points[l].X - startPoint.X) + startPoint.Y; } CalculateAngle(position, new Point(x, y)); return(new Point(x, y)); } } return(new Point(-1, -1)); }
//Creates the initial Scene point. public void Start() { rand = new System.Random(); graph = new RoadGraph <RoadNode>(); //Initialize the initial state. //Adjacents. /* * graph.add( * new RoadNode( * true, * GameStateController.Instance.GetComponent<BuildingPrefabWrapper>().home, * randomSingleBuilding(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(1,0) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(1,0)).getblockNW(), * randomSingleBuilding(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(0, -1) * )); * * graph.add( * new RoadNode( * true, * randomSingleBuilding(), * graph.GetNode(new Vector2(1,0)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-1, 0) * )); * * graph.add( * new RoadNode( * false, * randomSingleBuilding(), * graph.GetNode(new Vector2(1,0)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(0, 1) * )); * * //Verticals * graph.add( * new RoadNode( * true, * graph.GetNode(new Vector2(0, 1)).getblockNW(), * randomSingleBuilding(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(1, 2) * )); * * graph.add( * new RoadNode( * true, * graph.GetNode(new Vector2(0, -1)).getblockES(), * randomSingleBuilding(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(1, -2) * )); * * graph.add( * new RoadNode( * true, * randomSingleBuilding(), * graph.GetNode(new Vector2(0, -1)).getblockES(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-1, -2) * )); * * graph.add( * new RoadNode( * true, * randomSingleBuilding(), * graph.GetNode(new Vector2(0, 1)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-1, 2) * )); * * //Fill in. * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(1, 2)).getblockES(), * graph.GetNode(new Vector2(1, 0)).getblockES(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(2, 1) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(1, 0)).getblockES(), * graph.GetNode(new Vector2(1, -2)).getblockES(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(2, -1) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(-1, 0)).getblockES(), * graph.GetNode(new Vector2(-1, -2)).getblockES(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-2, -1) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(-1, 2)).getblockES(), * graph.GetNode(new Vector2(-1, 0)).getblockES(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-2, 1) * )); * * //Edges of 3x3 block. * graph.add( * new RoadNode( * true, * graph.GetNode(new Vector2(2, 1)).getblockNW(), * null, * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(3, 2) * )); * * graph.add( * new RoadNode( * true, * graph.GetNode(new Vector2(1, 0)).getblockES(), * null, * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(3, 0) * )); * * graph.add( * new RoadNode( * true, * graph.GetNode(new Vector2(2, -1)).getblockES(), * null, * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(3, -2) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(2, -1)).getblockES(), * null, * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(2, -3) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(0, -1)).getblockES(), * null, * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(0, -3) * )); * * graph.add( * new RoadNode( * false, * graph.GetNode(new Vector2(-2, -1)).getblockES(), * null, * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-2, -3) * )); * * graph.add( * new RoadNode( * true, * null, * graph.GetNode(new Vector2(-1, -2)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-3, -2) * )); * * graph.add( * new RoadNode( * true, * null, * graph.GetNode(new Vector2(-1, 0)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-3, 0) * )); * * graph.add( * new RoadNode( * true, * null, * graph.GetNode(new Vector2(-1, 2)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-3, 2) * )); * * graph.add( * new RoadNode( * false, * null, * graph.GetNode(new Vector2(-2, 1)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(-2, 3) * )); * * graph.add( * new RoadNode( * false, * null, * graph.GetNode(new Vector2(0, 1)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(0, 3) * )); * * graph.add( * new RoadNode( * false, * null, * graph.GetNode(new Vector2(2, 1)).getblockNW(), * RoadNode.EndType.Four, * RoadNode.EndType.Four, * new Vector2(2, 3) * )); * * * //Edges. * graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 0))); * graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, 1))); * * graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, -1))); * graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(1, 0))); * * graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, 0))); * graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, -1))); * * graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, 1))); * graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 0))); * * //--- * * graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 2))); * graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(0, 1))); * * graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(2, 1))); * graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 2))); * * graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 0))); * graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(2, 1))); * * //--- * * graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(2, -1))); * graph.add(graph.GetNode(new Vector2(2, -1)), graph.GetNode(new Vector2(1, 0))); * * graph.add(graph.GetNode(new Vector2(2, -1)), graph.GetNode(new Vector2(1, -2))); * graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(2, -1))); * * graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(0, -1))); * graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(1, -2))); * * //--- * * graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, -2))); * graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(0, -1))); * * graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(-2, -1))); * graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, -2))); * * graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, 0))); * graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(-2, -1))); * * //--- * * graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(-2, 1))); * graph.add(graph.GetNode(new Vector2(-2, 1)), graph.GetNode(new Vector2(-1, 0))); * * graph.add(graph.GetNode(new Vector2(-2, 1)), graph.GetNode(new Vector2(-1, 2))); * graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(-2, 1))); * * graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(0, 1))); * graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 2))); */ StartCoroutine(generateBlock(new Vector3(0, 0, 0), Direction.NONE)); }
public override void Step() { try { if (position == new Point(-1, -1)) { LocateAgent(); } //Длина пути, которая будет пройдена за один шаг моделирвания double step_lenght = (double)Scenario.STEP_TIME_MS / CurrentSpeed; double tmp = currentLength; double route_length = 0; for (int i = 1; i < CheckPointsList.Count; i++) { //Поиск дороги PathFigure road = null; var edges = RoadGraph.GetEdgesFrom(CheckPointsList[i - 1]).GetEnumerator(); while (edges.MoveNext()) { if (edges.Current.End.Equals(CheckPointsList[i])) { road = edges.Current.Data; break; } } //Расчет длины маршрута и данного участка double road_length = CalculateLength(road); route_length += road_length; //Если этот участок еще не пройден if (tmp < road_length) { //Если участок не будет пройден за этот шаг if (tmp + step_lenght + Size.X / 2 < road_length) { currentLength += step_lenght; position = CalculateCoordinateLocationOfCarrige(); break; } else if (tmp + step_lenght + Size.X / 2 > road_length) { if (tmp + Size.X / 2 <= road_length) { if (CheckPointsList[i].IsServicePoint) { //Паркуемся if (tmp != road_length - Size.X / 2) { currentLength += road_length - tmp - Size.X / 2; position = CalculateCoordinateLocationOfCarrige(); } //Работаем с сервисом if (!ReadyToIOOperation) { ServiceBase service = scenario.ServicesList.Find(delegate(ServiceBase sb) { return(sb.ID == CheckPointsList[i].ServiceID); }); if (service.ID == -1) { throw new ArgumentException("Service not found, service id = " + CheckPointsList[i].ServiceID); } if (service is StopService) { (service as StopService).OpenInputPoints(ID); ReadyToIOOperation = true; } } if (go) { ReadyToIOOperation = false; go = false; } if (ReadyToIOOperation) { return; } } //Задержка на маршрутной точке if (CheckPointsList[i].IsWaitPoint) { if (CheckPointsList[i].MinWait > 0) { CheckPointsList[i].MinWait -= Scenario.STEP_TIME_MS; return; } } //Точка с присвоением фиксированной скорости if (CheckPointsList[i].IsFixSpeedPoint) { CurrentSpeed = Convert.ToInt32(CheckPointsList[i].FixSpeed); } else { CurrentSpeed = MaxSpeed; } if (CheckPointsList.Count > i + 1 && i > 0 && CheckPointsList[i + 1].Equals(CheckPointsList[i - 1])) { Revers(route_length); } } currentLength += step_lenght; position = CalculateCoordinateLocationOfCarrige(); break; } } else { if (i == CheckPointsList.Count - 1) { currentLength += step_lenght; } tmp -= road_length; } } } catch (Exception ex) { Console.WriteLine(string.Format("Error in Thread! Agent ID [{0}]. Error message [{1}]", ID, ex.Message)); WayPointsList.Clear(); return; } }
/// <summary> /// Connects gaps and removes duplicates in a sequence of roads in a graph. /// </summary> /// <param name="graph"></param> /// <param name="sequence"></param> /// <returns></returns> public static List <DirectedRoad> DijstraConnectUpSequence(this List <DirectedRoad> sequence, RoadGraph graph) { if (sequence.Count == 0) { throw new System.ArgumentException("Empty sequence"); } if (sequence.Count == 1) { return(sequence); } var cleanedSequence = sequence.RemoveDuplicateNeighbors(); var result = new List <DirectedRoad>() { cleanedSequence.First() }; for (int i = 1; i < cleanedSequence.Count; i++) { //add all necessary roads up to and including the i'th List <DirectedRoad> connection; if (DijstraTryFindPath(graph, cleanedSequence[i - 1].End, cleanedSequence[i].Start, double.PositiveInfinity, out connection)) { result.AddRange(connection); result.Add(cleanedSequence[i]); } else { throw new Exception("Unable to connect up sequence of roads"); } } return(result); }
/// <summary> /// Finds the shortest sequence of directed roads from the origin to destination node using Dijstra's algorithm. /// </summary> /// <param name="graph"></param> /// <param name="origin"></param> /// <param name="destination"></param> /// <returns></returns> public static bool DijstraTryFindPath(RoadGraph graph, string origin, string destination, double upperSearchLimitInMeters, out List <DirectedRoad> path) { path = new List <DirectedRoad>(); // If there are no outgoing roads from the origin if (!graph.ContainsKey(origin)) { return(false); } // If there are no incoming roads to the destination if (!graph.InverseGraph.ContainsKey(destination)) { return(false); } if (origin == destination) { // correct path is empty return(true); } DijstraSearchItem currentSearch = new DijstraSearchItem(origin, null, null, 0); IntervalHeap <DijstraSearchItem> heap = new IntervalHeap <DijstraSearchItem>(); var itemLookup = new Dictionary <string, DijstraSearchItem>(); heap.Add(currentSearch); itemLookup[origin] = currentSearch; while (heap.Count != 0) { // find the least item and remove from heap currentSearch = heap.FindMin(); if (currentSearch.Distance > upperSearchLimitInMeters) { return(false); } heap.DeleteMin(); if (currentSearch.Id == destination) { break; } if (graph.ContainsKey(currentSearch.Id)) { foreach (var road in graph[currentSearch.Id]) { // calculate new possible distance var alt = currentSearch.Distance + road.Length.DistanceInMeters; var potentialNewItem = new DijstraSearchItem(road.End, road, currentSearch, alt); if (itemLookup.ContainsKey(road.End)) { DijstraSearchItem searchItem = itemLookup[road.End]; if (alt < searchItem.Distance) { itemLookup[road.End] = potentialNewItem; heap.Add(potentialNewItem); } } else { itemLookup[road.End] = potentialNewItem; heap.Add(potentialNewItem); } } } } if (currentSearch.Id != destination) { // Perhaps there is no path from origin to destination // To throw exception or return false?? return(false); } // Trace back path List <DirectedRoad> result = new List <DirectedRoad>(); DijstraSearchItem tracer = currentSearch; while (tracer.PrevRoad != null) { result.Add(tracer.PrevRoad); tracer = tracer.Prev; } result.Reverse(); if (result.First().Start != origin || result.Last().End != destination) { throw new Exception("connecting path does not correctly connect origin and destination"); } path = result; return(true); }
//Creates the initial Scene point. Generator_Sys() { rand = new System.Random(); graph = new RoadGraph <RoadNode>(); //Initialize the initial state. //Adjacents. graph.add( new RoadNode( true, GameStateController.Instance.GetComponent <BuildingPrefabWrapper>().home, randomSingleBuilding(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(1, 0) )); graph.add( new RoadNode( false, graph.GetNode(new Vector2(1, 0)).getblockNW(), randomSingleBuilding(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(0, -1) )); graph.add( new RoadNode( true, randomSingleBuilding(), graph.GetNode(new Vector2(1, 0)).getblockNW(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(-1, 0) )); graph.add( new RoadNode( false, randomSingleBuilding(), graph.GetNode(new Vector2(1, 0)).getblockNW(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(0, 1) )); //Verticals graph.add( new RoadNode( true, graph.GetNode(new Vector2(0, 1)).getblockNW(), randomSingleBuilding(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(1, 2) )); graph.add( new RoadNode( true, graph.GetNode(new Vector2(0, -1)).getblockES(), randomSingleBuilding(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(1, -2) )); graph.add( new RoadNode( true, randomSingleBuilding(), graph.GetNode(new Vector2(0, -1)).getblockES(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(-1, -2) )); graph.add( new RoadNode( true, randomSingleBuilding(), graph.GetNode(new Vector2(0, 1)).getblockNW(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(-1, 2) )); //Fill in. graph.add( new RoadNode( false, graph.GetNode(new Vector2(1, 2)).getblockES(), graph.GetNode(new Vector2(1, 0)).getblockES(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(2, 1) )); graph.add( new RoadNode( false, graph.GetNode(new Vector2(1, 0)).getblockES(), graph.GetNode(new Vector2(1, -2)).getblockES(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(1, -1) )); graph.add( new RoadNode( false, graph.GetNode(new Vector2(-1, 0)).getblockES(), graph.GetNode(new Vector2(-1, -2)).getblockES(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(-2, -1) )); graph.add( new RoadNode( false, graph.GetNode(new Vector2(-1, 2)).getblockES(), graph.GetNode(new Vector2(-1, 0)).getblockES(), RoadNode.EndType.Four, RoadNode.EndType.Four, new Vector2(1, -2) )); //Edges. graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 0))); graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, 1))); graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(0, -1))); graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(1, 0))); graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, 0))); graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, -1))); graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(0, 1))); graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 0))); graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(1, 2))); graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(0, 1))); graph.add(graph.GetNode(new Vector2(1, 2)), graph.GetNode(new Vector2(2, 1))); graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 2))); graph.add(graph.GetNode(new Vector2(2, 1)), graph.GetNode(new Vector2(1, 0))); graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(2, 1))); graph.add(graph.GetNode(new Vector2(1, 0)), graph.GetNode(new Vector2(1, -1))); graph.add(graph.GetNode(new Vector2(1, -1)), graph.GetNode(new Vector2(1, 0))); graph.add(graph.GetNode(new Vector2(1, -1)), graph.GetNode(new Vector2(-1, -2))); graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(1, -1))); graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(0, -1))); graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, -2))); graph.add(graph.GetNode(new Vector2(0, -1)), graph.GetNode(new Vector2(-1, -2))); graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(0, -1))); graph.add(graph.GetNode(new Vector2(-1, -2)), graph.GetNode(new Vector2(-2, -1))); graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, -2))); graph.add(graph.GetNode(new Vector2(-2, -1)), graph.GetNode(new Vector2(-1, 0))); graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(-2, -1))); graph.add(graph.GetNode(new Vector2(-1, 0)), graph.GetNode(new Vector2(1, -2))); graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(-1, 0))); graph.add(graph.GetNode(new Vector2(1, -2)), graph.GetNode(new Vector2(-1, 2))); graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(1, -2))); graph.add(graph.GetNode(new Vector2(-1, 2)), graph.GetNode(new Vector2(0, 1))); graph.add(graph.GetNode(new Vector2(0, 1)), graph.GetNode(new Vector2(-1, 2))); }
static void Main(string[] args) { string osmPath = ""; int eps = -1; int minTraffic = -1; bool showHelp = false; OptionSet parameters = new OptionSet() { { "osm=", "path to the map file", v => osmPath = v }, { "eps=", "size of the eps-neighborhood to be considered (integer)", v => eps = Convert.ToInt32(v) }, { "minTraffic=", "minimum traffic considered (integer)", v => minTraffic = Convert.ToInt32(v) }, { "h|?|help", v => showHelp = v != null }, }; try { parameters.Parse(args); } catch (OptionException e) { Console.Write("TRoute: "); Console.WriteLine(e.Message); Console.WriteLine("Try `TRoute --help' for more information."); return; } if (showHelp || string.IsNullOrEmpty(osmPath) || eps < 0 || minTraffic < 0) { ShowHelp(parameters); return; } var osmFile = new OSMDB(); osmFile.Load(osmPath); var roadGraph = new RoadGraph(); roadGraph.Build(osmFile); // Getting all the nodes with traffic signals Dictionary <double, OSMNode> tagsNodes = new Dictionary <double, OSMNode>(); foreach (var node in osmFile.Nodes) { foreach (var tag in node.Tags) { if (tag.Value.Equals("traffic_signals")) { if (!tagsNodes.Keys.Contains(node.Latitude + node.Longitude)) { tagsNodes.Add(node.Latitude + node.Longitude, node); } } } } var hotRoutes = new FlowScan().Run(roadGraph, eps, minTraffic); // Saving GPX file of the Hot Route HashSet <GPXPoint> listPoints; HashSet <GPXTrackSegment> listSegments; GPXTrackSegment segTrack; List <GPXTrack> track = new List <GPXTrack>(); GPXTrack tr; //Console.WriteLine(hotRoutes.Count); foreach (var hr in hotRoutes) { //Console.WriteLine("Number segs: " + hr.Segments.Count); listSegments = new HashSet <GPXTrackSegment>(); foreach (var seg in hr.Segments) { listPoints = new HashSet <GPXPoint>(); foreach (var segInner in seg.Geometry.Segments) { GPXPoint start; if (tagsNodes.Keys.Contains(segInner.StartPoint.Latitude + segInner.StartPoint.Longitude)) { OSMNode osmNode = tagsNodes[segInner.StartPoint.Latitude + segInner.StartPoint.Longitude]; start = new GPXPoint() { Id = osmNode.ID, Latitude = segInner.StartPoint.Latitude, Longitude = segInner.StartPoint.Longitude, TrafficSignal = true }; } else { OSMNode osmNode = osmFile.Nodes.ToList().First(x => x.Latitude == segInner.StartPoint.Latitude && x.Longitude == segInner.StartPoint.Longitude); start = new GPXPoint() { Id = osmNode.ID, Latitude = segInner.StartPoint.Latitude, Longitude = segInner.StartPoint.Longitude, TrafficSignal = false }; } GPXPoint end; if (tagsNodes.Keys.Contains(segInner.EndPoint.Latitude + segInner.EndPoint.Longitude)) { OSMNode osmNode = tagsNodes[segInner.EndPoint.Latitude + segInner.EndPoint.Longitude]; end = new GPXPoint() { Id = osmNode.ID, Latitude = segInner.EndPoint.Latitude, Longitude = segInner.EndPoint.Longitude, TrafficSignal = true }; } else { OSMNode osmNode = osmFile.Nodes.ToList().First(x => x.Latitude == segInner.EndPoint.Latitude && x.Longitude == segInner.EndPoint.Longitude); end = new GPXPoint() { Id = osmNode.ID, Latitude = segInner.EndPoint.Latitude, Longitude = segInner.EndPoint.Longitude, TrafficSignal = false }; } listPoints.Add(start); listPoints.Add(end); } segTrack = new GPXTrackSegment(listPoints, seg.AvgSpeed, seg.Speed, seg.Id); // passing the traffic segTrack.Traffic = seg.Traffic; listSegments.Add(segTrack); } tr = new GPXTrack(); tr.Segments.AddRange(listSegments); track.Add(tr); } // Bucket Information GPXTrack tBucket = new GPXTrack(); GPXPoint pBucket = new GPXPoint(0, 0, 0, false); GPXTrackSegment sBucket = new GPXTrackSegment(); var bucketInfo = osmFile.Nodes.ToList().Find(x => x.ID == 0); if (bucketInfo != null) { pBucket.StartBucket = TimeSpan.Parse(bucketInfo.Tags.First().Value); pBucket.EndBucket = TimeSpan.Parse(bucketInfo.Tags.Last().Value); } sBucket.Nodes.Add(pBucket); tBucket.Segments.Add(sBucket); var gpx = new GPXDocument() { Tracks = track }; gpx.Tracks.Add(tBucket); gpx.Save("mapWithHotRoutes.gpx"); }
public override void DoStep(double msInterval) { try { if (!_located) { LocateAgent(); } //Длина пути, которая может быть пройдена за один шаг моделирования double step_lenght = msInterval / CurrentSpeed; double tmp = _currentLength; double route_length = 0; for (int i = 1; i < RouteList.Count; i++) { //Поиск дороги string pathData = RoadGraph.GetEdgeData(RouteList[i - 1], RouteList[i]); PathFigure road = GetPathFigureFromString(pathData); if (road == null) { throw new InvalidCastException("Не удалось десериализовать часть пути из строки (" + pathData + ")"); } //Расчет длины маршрута и данного участка double road_length = CalculatePathLength(road); route_length += road_length; //Если этот участок еще не пройден if (_currentLength < road_length) { //Если участок не будет пройден за этот шаг if (_currentLength + step_lenght + Size.X / 2 < road_length) { _currentLength += step_lenght; Position = CalculateCoordinateLocationOfCarrige(); break; } else if (tmp + step_lenght + Size.X / 2 > road_length) { if (tmp + Size.X / 2 <= road_length) { if (RouteList[i].IsServicePoint) { //Паркуемся if (tmp != road_length - Size.X / 2) { _currentLength += road_length - tmp - Size.X / 2; Position = CalculateCoordinateLocationOfCarrige(); } //Работаем с сервисом if (!ReadyToIOOperation) { var service = _services.FirstOrDefault(s => s.Id == RouteList[i].ServiceId); if (service == null) { throw new ArgumentException("Service not found, service id = " + RouteList[i].ServiceId); } service.AddAgent(this); ReadyToIOOperation = true; //if (service is StopService) //{ // (service as StopService).OpenInputPoints(Id); // ReadyToIOOperation = true; //} } if (_go) { ReadyToIOOperation = false; _go = false; } if (ReadyToIOOperation) { return; } } if (RouteList.Count > i + 1 && i > 0 && RouteList[i + 1].Equals(RouteList[i - 1])) { Revers(route_length); } } _currentLength += step_lenght; Position = CalculateCoordinateLocationOfCarrige(); } } else { if (i == RouteList.Count - 1) { _currentLength += step_lenght; RouteList.Clear(); } tmp -= road_length; } } } catch (Exception ex) { Console.WriteLine(string.Format("Error in Thread! Agent Id [{0}]", Id)); Console.WriteLine(ex.ToString()); RouteList.Clear(); } }
private Point CalculateCoordinateLocationOfCarrige() { double tmp = _currentLength; for (int i = 1; i < RouteList.Count; i++) { //Поиск дороги string pathData = RoadGraph.GetEdgeData(RouteList[i - 1], RouteList[i]); PathFigure road = PathGeometry.CreateFromGeometry(PathGeometry.Parse(pathData)).Figures[0]; //Если дорога не найдена if (road == null) { CalculateAngle(Position, RouteList[i].Location); Position = RouteList[i].Location; return(new Point()); } //Если найдена double road_lenght = CalculatePathLength(road); //Если этот участок уже пройден, то смотрим следующий if (tmp > road_lenght) { tmp -= road_lenght; continue; } for (int g = 0; g < road.Segments.Count; g++) { double x = 0, y = 0, t = 0, seg_l = 0; Point startPoint = road.StartPoint; if (road.Segments[g] is BezierSegment) { BezierSegment seg = road.Segments[g] as BezierSegment; seg_l = CalculatePathLength(new PathFigure(startPoint, new List <PathSegment>() { seg }, false)); if (seg_l < tmp) { tmp -= seg_l; startPoint = seg.Point3; continue; } t = tmp / seg_l; x = (1 - t) * (1 - t) * (1 - t) * startPoint.X + (1 - t) * (1 - t) * 3 * t * seg.Point1.X + (1 - t) * 3 * t * t * seg.Point2.X + t * t * t * seg.Point3.X; y = (1 - t) * (1 - t) * (1 - t) * startPoint.Y + (1 - t) * (1 - t) * 3 * t * seg.Point1.Y + (1 - t) * 3 * t * t * seg.Point2.Y + t * t * t * seg.Point3.Y; } else if (road.Segments[g] is LineSegment) { LineSegment seg = road.Segments[g] as LineSegment; seg_l = CalculatePathLength(new PathFigure(startPoint, new List <PathSegment>() { seg }, false)); if (seg_l < tmp) { tmp -= seg_l; startPoint = seg.Point; continue; } t = tmp / seg_l; x = seg.Point.X * t + startPoint.X * (1 - t); if (startPoint.X == seg.Point.X) { y = startPoint.Y + (seg.Point.Y - startPoint.Y) * t; } else { y = (x - startPoint.X) * (seg.Point.Y - startPoint.Y) / (seg.Point.X - startPoint.X) + startPoint.Y; } } else if (road.Segments[g] is PolyLineSegment) { PolyLineSegment seg = road.Segments[g] as PolyLineSegment; int l; for (l = 0; l < seg.Points.Count; l++) { seg_l = Math.Sqrt(Math.Pow(startPoint.X - seg.Points[l].X, 2) + Math.Pow(startPoint.Y - seg.Points[l].Y, 2)); if (seg_l < tmp) { tmp -= seg_l; startPoint = seg.Points[l]; continue; } } t = tmp / seg_l; x = seg.Points[l].X * t + startPoint.X * (1 - t); y = (x - startPoint.X) * (seg.Points[l].Y - startPoint.Y) / (seg.Points[l].X - startPoint.X) + startPoint.Y; } CalculateAngle(Position, new Point(x, y)); return(new Point(x, y)); } } return(new Point()); }