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));
}
public Path(PGNode start)
: this(start, null, 0)
{
}
private Path(PGNode lastStep, Path previousSteps, double totalCost)
{
LastStep = lastStep;
PreviousSteps = previousSteps;
TotalCost = totalCost;
}
