public static Path FindPath(VGArea mMap, PGNode mStart, PGNode mEnd) { foreach(VGTile tile in mMap.Tiles) tile.Node.Clear(); var closed = new HashSet<PGNode>(); var queue = new PriorityQueue<double, Path>(); queue.Enqueue(0, new Path(mStart)); while (!queue.IsEmpty) { var path = queue.Dequeue(); if (!path.LastStep.Passable || closed.Contains(path.LastStep)) continue; if (path.LastStep.Equals(mEnd)) { return path; } closed.Add(path.LastStep); foreach (PGNode n in mMap.GetTileNodeNeighbors(path.LastStep.Tile.X, path.LastStep.Tile.Y)) { double d = PGUtils.GetNodeDiagonalDistance(path.LastStep, n); var newPath = path.AddStep(n, d); queue.Enqueue(newPath.TotalCost + PGUtils.GetNodeManhattanDistance(n, mEnd), newPath); } } return null; }
public static Path FindPath(VGArea mMap, PGNode mStart, PGNode mEnd) { foreach (VGTile tile in mMap.Tiles) { tile.Node.Clear(); } var closed = new HashSet <PGNode>(); var queue = new PriorityQueue <double, Path>(); queue.Enqueue(0, new Path(mStart)); while (!queue.IsEmpty) { var path = queue.Dequeue(); if (!path.LastStep.Passable || closed.Contains(path.LastStep)) { continue; } if (path.LastStep.Equals(mEnd)) { return(path); } closed.Add(path.LastStep); foreach (PGNode n in mMap.GetTileNodeNeighbors(path.LastStep.Tile.X, path.LastStep.Tile.Y)) { double d = PGUtils.GetNodeDiagonalDistance(path.LastStep, n); var newPath = path.AddStep(n, d); queue.Enqueue(newPath.TotalCost + PGUtils.GetNodeManhattanDistance(n, mEnd), newPath); } } return(null); }
internal VGTile(int mX, int mY, int mValue, bool mPassable = true) { Debug.Assert(X >= 0); Debug.Assert(Y >= 0); X = mX; Y = mY; Value = mValue; Passable = mPassable; Node = new PGNode(this); }
public Path(PGNode start) : this(start, null, 0) { }
public Path AddStep(PGNode step, double stepCost) { return new Path(step, this, TotalCost + stepCost); }
private Path(PGNode lastStep, Path previousSteps, double totalCost) { LastStep = lastStep; PreviousSteps = previousSteps; TotalCost = totalCost; }
public static double GetNodeEuclideanDistance(PGNode mStart, PGNode mEnd) { return(Math.Sqrt((mStart.X - mEnd.X) ^ 2 + (mStart.Y - mEnd.Y) ^ 2)); }
public static double GetNodeDiagonalDistance(PGNode mStart, PGNode mEnd) { return(Math.Abs(mStart.X - mEnd.X) + Math.Abs(mStart.Y - mEnd.Y)); }
public static double GetNodeManhattanDistance(PGNode mStart, PGNode mEnd) { return(Math.Max(Math.Abs(mStart.X - mEnd.X), Math.Abs(mStart.Y - mEnd.Y))); }
public Path AddStep(PGNode step, double stepCost) { return(new Path(step, this, TotalCost + stepCost)); }