private Path ReconstructPath(Tile end) {
List<Tile> reversePath = new List<Tile>();
Path p = new Path();
reversePath.Add(end);
while (end.cameFrom != null) {
end = end.cameFrom;
reversePath.Add(end);
}
for (int i = reversePath.Count - 1; i >= 0; i--) {
p.AddPoint(grid.GetWindowCenterPos(reversePath[i]));
}
return p;
}
private Path ReconstructPath(Tile end)
{
List <Tile> reversePath = new List <Tile>();
Path p = new Path();
reversePath.Add(end);
while (end.cameFrom != null)
{
end = end.cameFrom;
reversePath.Add(end);
}
for (int i = reversePath.Count - 1; i >= 0; i--)
{
p.AddPoint(grid.GetWindowCenterPos(reversePath[i]));
}
return(p);
}
public Path FindPath(Tile start, Tile end, float goalTolerance = 0.0f)
{
if (!start.passable || (!end.passable && goalTolerance <= 0.0f)) {
Path p1 = new Path();
p1.AddPoint(grid.GetWindowCenterPos(start));
return p1;
}
openSet.Clear();
closedSet.Clear();
start.g = 0;
start.f = start.g + start.DistanceHeuristic(end);
openSet.Add(start);
while (openSet.Count > 0) {
Tile current = openSet.First();
openSet.Remove(current);
closedSet.Add(current);
float dh = current.DistanceHeuristic(end);
if (current == end || current.DistanceHeuristic(end) <= goalTolerance)
return ReconstructPath(current);
foreach (Tile t in current.neighbours) {
if (closedSet.Contains(t))
{
continue;
}
float tentative_g = current.g + 1;
Tile newParent = current;
if (current.cameFrom != null && grid.LineOfSight(current.cameFrom, t))
{
float cost = (float)Math.Sqrt(Math.Pow(current.cameFrom.gridX - t.gridX, 2) + Math.Pow(current.cameFrom.gridY - t.gridY, 2));
tentative_g = current.cameFrom.g + cost;
newParent = current.cameFrom;
}
if (!openSet.Contains(t)) {
//t.cameFrom = current;
//t.g = tentative_g;
//t.f = t.g + t.ManhattanDistance(end);
openSet.Add(t);
}
else if (tentative_g >= t.g)
{
continue;
}
t.cameFrom = newParent;
t.g = tentative_g;
t.f = t.g + t.DistanceHeuristic(end) - goalTolerance;
}
openSet.Sort();
}
//If we get here, no path was found
Path p2 = new Path();
p2.AddPoint(grid.GetWindowCenterPos(start));
return p2;
}
public Path FindPath(Vector2 start, Vector2 end) {
if (Bounds.Contains(new Point((int)start.X, (int)start.Y)) && Bounds.Contains(new Point((int)end.X, (int)end.Y)))
{
if (LineOfSight(start,end))
{
Path p = new Path();
p.AddPoint(start);
p.AddPoint(end);
return p;
}
return FindPath(tiles[(int)(start.X / TileSize), (int)(start.Y / TileSize)], tiles[(int)(end.X / TileSize), (int)(end.Y / TileSize)]);
}
return null;
}
public Path FindPath(int x1, int y1, int x2, int y2)
{
if (Bounds.Contains(new Point(x1, y1)) && Bounds.Contains(new Point(x2, y2)))
{
if (LineOfSight(x1, y1, x2, y2))
{
Path p = new Path();
p.AddPoint(new Vector2(x1, y1));
p.AddPoint(new Vector2(x2, y2));
return p;
}
return FindPath(tiles[x1, y1], tiles[x2, y2]);
}
return null;
}
public Path FindPath(Tile start, Tile end, float goalTolerance = 0.0f)
{
if (!start.passable || (!end.passable && goalTolerance <= 0.0f))
{
Path p1 = new Path();
p1.AddPoint(grid.GetWindowCenterPos(start));
return(p1);
}
openSet.Clear();
closedSet.Clear();
start.g = 0;
start.f = start.g + start.DistanceHeuristic(end);
openSet.Add(start);
while (openSet.Count > 0)
{
Tile current = openSet.First();
openSet.Remove(current);
closedSet.Add(current);
float dh = current.DistanceHeuristic(end);
if (current == end || current.DistanceHeuristic(end) <= goalTolerance)
{
return(ReconstructPath(current));
}
foreach (Tile t in current.neighbours)
{
if (closedSet.Contains(t))
{
continue;
}
float tentative_g = current.g + 1;
Tile newParent = current;
if (current.cameFrom != null && grid.LineOfSight(current.cameFrom, t))
{
float cost = (float)Math.Sqrt(Math.Pow(current.cameFrom.gridX - t.gridX, 2) + Math.Pow(current.cameFrom.gridY - t.gridY, 2));
tentative_g = current.cameFrom.g + cost;
newParent = current.cameFrom;
}
if (!openSet.Contains(t))
{
//t.cameFrom = current;
//t.g = tentative_g;
//t.f = t.g + t.ManhattanDistance(end);
openSet.Add(t);
}
else if (tentative_g >= t.g)
{
continue;
}
t.cameFrom = newParent;
t.g = tentative_g;
t.f = t.g + t.DistanceHeuristic(end) - goalTolerance;
}
openSet.Sort();
}
//If we get here, no path was found
Path p2 = new Path();
p2.AddPoint(grid.GetWindowCenterPos(start));
return(p2);
}
