/// <summary> /// Gets the nearest path graph vertex from the specified position. /// </summary> /// <param name="from"></param> /// <returns></returns> public PathGraphVertex FindNearestPathGraphVertex(Vector2 from) { PathGraphVertex res = null; PathGraphVertex fallDownResult = null; float minDistance = float.MaxValue; foreach (GridField field in fields) { foreach (PathGraphVertex vertex in field.PathGraphVertices) { if (fallDownResult == null) { fallDownResult = vertex; } Vector2 way = (from - vertex.Position.PositionInQuarter); float direction = (way.GetAngle() + 1 * MathHelper.PiOver2) % MathHelper.TwoPi; Quadrangle pathObj = Quadrangle.CreateBand(vertex.Position.PositionInQuarter, direction, 0.5f, way.Length()); //Quadrangle pathObj = new Quadrangle(vertex.Position.PositionInQuarter, vertex.Position.PositionInQuarter, from, from); if ((vertex.Position.PositionInQuarter - from).Length() < minDistance && !IsInCollision(pathObj, x => !(x is Human))) { res = vertex; minDistance = (vertex.Position.PositionInQuarter - from).Length(); } } } if (res != null) { return(res); } return(fallDownResult); }
/// <summary> /// Registers a new path graph vertex into the grid. /// </summary> /// <param name="vertex">The path graph vertex</param> public void AddPathGraphVertex(PathGraphVertex vertex) { int x = (int)(vertex.Position.PositionInQuarter.X / fieldWidth); int y = (int)(vertex.Position.PositionInQuarter.Y / fieldHeight); GetField(x, y).AddPathGraphVertex(vertex); }
/// <summary> /// Searches for shortest path in the graph. Uses Dijkstra 1-1 form (A*). /// </summary> /// <param name="from">Source vertex</param> /// <param name="to">Target vertex</param> /// <param name="keepInTheSameQuarter">Indicator whether the searching has to use only vertices from the same quarter as the from position is in</param> /// <returns>Set of vertices forming result path</returns> public static IEnumerable<PathGraphVertex> FindShortestPath(PathGraphVertex from, PathGraphVertex to, bool keepInTheSameQuarter) { keepInTheSameQuarter = keepInTheSameQuarter && from.Position.Quarter == to.Position.Quarter; TownQuarter fromQuarter = from.Position.Quarter; LinkedList<PathGraphVertex> resultPath = new LinkedList<PathGraphVertex>(); HashSet<PathGraphVertex> closed = new HashSet<PathGraphVertex>(); Dictionary<PathGraphVertex, PathGraphVertex> cameFrom = new Dictionary<PathGraphVertex, PathGraphVertex>(); Dictionary<PathGraphVertex, float> gScore = new Dictionary<PathGraphVertex, float>(); gScore.Add(from, 0); Dictionary<PathGraphVertex, float> fScore = new Dictionary<PathGraphVertex, float>(); fScore.Add(from, gScore[from] + HeuristicDistance(from, to)); HashSet<PathGraphVertex> open = new HashSet<PathGraphVertex>(); open.Add( from); while (open.Count != 0) { float lowestScore = float.MaxValue; PathGraphVertex lowestVertex = null; foreach (PathGraphVertex openedOne in open) { float score = fScore[openedOne]; if (score < lowestScore) { lowestVertex = openedOne; } } PathGraphVertex current = lowestVertex;//open.OrderBy(x => fScore[x]).First(); if (current == to) { PathGraphVertex t = to; while(t != from) { resultPath.AddFirst(t); t = cameFrom[t]; } resultPath.AddFirst(from); return resultPath; } open.Remove(current); closed.Add(current); foreach (PathGraphVertex n in current.Neighbors) { if (closed.Contains(n) || (keepInTheSameQuarter && n.Position.Quarter != fromQuarter)) { continue; } else { float tempGSore = gScore[current] + current.GetDistanceToNeighbor(n); if (!open.Contains(n) || tempGSore <= gScore[n]) { cameFrom.SetValue(n, current); gScore.SetValue(n, tempGSore); fScore.SetValue(n, gScore[n] + HeuristicDistance(current, n)); if (!open.Contains(n)) { open.Add(n); } } } } } throw new PathNotFoundException("Source and target vertices aren't in the same component."); }
static float HeuristicDistance(PathGraphVertex u, PathGraphVertex v) { return u.Position.MinimalDistanceTo(v.Position); }
/// <summary> /// Generates the path graph part situated in this quarter. /// </summary> /// <param name="emptyRectanglesInsideRoads">The inner empty rectangles - the path are around them</param> /// <returns>List of path graph vertices</returns> IList<PathGraphVertex> GeneratePathGraph(List<Rectangle> emptyRectanglesInsideRoads) { List<Point> pointsOfInterests = new List<Point>(); foreach (Rectangle rect in emptyRectanglesInsideRoads) { pointsOfInterests.AddRange( new Point[] { new Point(rect.X, rect.Y), //new Point(rect.X + rect.Width - 1, rect.Y), //new Point(rect.X, rect.Y + rect.Height - 1), new Point(rect.X + rect.Width - 1, rect.Y + rect.Height - 1) }); } Dictionary<Point, Tuple<TownQuarterInterface, bool>> interfaceByPoint = new Dictionary<Point, Tuple<TownQuarterInterface, bool>>();//tuple: interface, isLeftPoint foreach (var iface in interfaces) { Point left, right; switch (iface.SidePosition) { case TownQuarterInterfacePosition.Top: left.X = iface.BitmapPosition - 1; left.Y = 0; right.X = iface.BitmapPosition + 1; right.Y = 0; break; case TownQuarterInterfacePosition.Right: left.X = bitmapSize.Width - 1; left.Y = iface.BitmapPosition - 1; right.X = bitmapSize.Width - 1; right.Y = iface.BitmapPosition + 1; break; case TownQuarterInterfacePosition.Bottom: left.X = iface.BitmapPosition + 1; left.Y = bitmapSize.Height - 1; right.X = iface.BitmapPosition - 1; right.Y = bitmapSize.Height - 1; break; case TownQuarterInterfacePosition.Left: left.X = 0; left.Y = iface.BitmapPosition + 1; right.X = 0; right.Y = iface.BitmapPosition - 1; break; default: throw new InvalidOperationException("Unknown SidePosition value."); } pointsOfInterests.Add(left); pointsOfInterests.Add(right); interfaceByPoint.Add(left, new Tuple<TownQuarterInterface, bool>(iface, true)); interfaceByPoint.Add(right, new Tuple<TownQuarterInterface, bool>(iface, false)); } pointsOfInterests.AddRange(new Point[] { new Point(BlockWidth - 1, BlockWidth - 1), new Point(bitmapSize.Width - BlockWidth, bitmapSize.Height - BlockWidth) }); SortedSet<int> xCoordinates = new SortedSet<int>(); SortedSet<int> yCoordinates = new SortedSet<int>(); foreach (Point point in pointsOfInterests) { if (!xCoordinates.Contains(point.X)) xCoordinates.Add(point.X); if (!yCoordinates.Contains(point.Y)) yCoordinates.Add(point.Y); } List<PathGraphVertex> innerVertices = new List<PathGraphVertex>(); Dictionary<int, SortedDictionary<int, PathGraphVertex>> verticalIndexedPaths = new Dictionary<int, SortedDictionary<int, PathGraphVertex>>(yCoordinates.Count); Dictionary<int, SortedDictionary<int, PathGraphVertex>> horizontalIndexedPaths = new Dictionary<int, SortedDictionary<int, PathGraphVertex>>(xCoordinates.Count); foreach (int y in yCoordinates) { verticalIndexedPaths.Add(y, new SortedDictionary<int, PathGraphVertex>()); } foreach (int x in xCoordinates) { horizontalIndexedPaths.Add(x, new SortedDictionary<int, PathGraphVertex>()); foreach (int y in yCoordinates) { if (mapBitmap.Index2D(bitmapSize.Height, x, y) == MapFillType.Sidewalk) { PathGraphVertex vertex = new PathGraphVertex(new PositionInTown(this, new Vector2(x + 0.5f, y + 0.5f) * SquareWidth)); innerVertices.Add(vertex); verticalIndexedPaths[y].Add(x, vertex); horizontalIndexedPaths[x].Add(y, vertex); Point p = new Point(x, y); if (interfaceByPoint.ContainsKey(p)) { if (interfaceByPoint[p].Item2) { interfaceByPoint[p].Item1.LeftPathGraphVertex = vertex; } else { interfaceByPoint[p].Item1.RightPathGraphVertex = vertex; } } } } } SweepPathVertices(verticalIndexedPaths, AxisDirection.Horizontal); SweepPathVertices(horizontalIndexedPaths, AxisDirection.Vertical); return innerVertices; }
/// <summary> /// Registers a path graph vertex. /// </summary> /// <param name="vertex">The path graph vertex</param> public void AddPathGraphVertex(PathGraphVertex vertex) { pathGraphVertices.Add(vertex); }