public void SetManhattanHeuristic(PathNode node, Point end) { node.H = Math.Abs(end.X - node.X) + Math.Abs(end.Y - node.Y); }
public void OpenWalkableChildren(PathNode parent, Point end) { //up if (parent.Y > 0 && GetNode(parent.X, parent.Y - 1).Walkable && !Closed.Contains(GetNode(parent.X, parent.Y - 1))) { //if open and g is higher than it would be with new parent, reparent if (Open.Contains(GetNode(parent.X, parent.Y - 1))) { if (GetNode(parent.X, parent.Y - 1).G >= parent.G + 1) GetNode(parent.X, parent.Y - 1).G = parent.G + 1; } else { Open.Add(GetNode(parent.X, parent.Y - 1)); SetManhattanHeuristic(GetNode(parent.X, parent.Y - 1), end); GetNode(parent.X, parent.Y - 1).Parent = parent; GetNode(parent.X, parent.Y - 1).G = parent.G+1; } } //down if (parent.Y < Height - 1 && GetNode(parent.X, parent.Y + 1).Walkable && !Closed.Contains(GetNode(parent.X, parent.Y + 1))) { if (Open.Contains(GetNode(parent.X, parent.Y + 1))) { if (GetNode(parent.X, parent.Y + 1).G >= parent.G + 1) GetNode(parent.X, parent.Y + 1).G = parent.G + 1; } else { Open.Add(GetNode(parent.X, parent.Y + 1)); SetManhattanHeuristic(GetNode(parent.X, parent.Y + 1), end); GetNode(parent.X, parent.Y + 1).Parent = parent; GetNode(parent.X, parent.Y + 1).G = parent.G + 1; } } //left if (parent.X > 0 && GetNode(parent.X - 1, parent.Y).Walkable && !Closed.Contains(GetNode(parent.X -1, parent.Y))) { if (Open.Contains(GetNode(parent.X -1, parent.Y))) { if (GetNode(parent.X -1, parent.Y).G >= parent.G + 1) GetNode(parent.X -1, parent.Y).G = parent.G + 1; } else { Open.Add(GetNode(parent.X - 1, parent.Y)); SetManhattanHeuristic(GetNode(parent.X - 1, parent.Y), end); GetNode(parent.X - 1, parent.Y).Parent = parent; GetNode(parent.X - 1, parent.Y).G = parent.G + 1; } } //right if (parent.X < Width - 1 && GetNode(parent.X + 1, parent.Y).Walkable && !Closed.Contains(GetNode(parent.X + 1, parent.Y))) { if (Open.Contains(GetNode(parent.X +1, parent.Y))) { if (GetNode(parent.X+1, parent.Y).G >= parent.G + 1) GetNode(parent.X+1, parent.Y).G = parent.G + 1; } else { Open.Add(GetNode(parent.X + 1, parent.Y)); SetManhattanHeuristic(GetNode(parent.X + 1, parent.Y), end); GetNode(parent.X + 1, parent.Y).Parent = parent; GetNode(parent.X + 1, parent.Y).G = parent.G + 1; } } }
public void PushNewPath(Stack<PathNode> path, PathNode node) { path.Push(node); if (node.Parent != null) PushNewPath(path, node.Parent); }
public void init(int width,int height) { Width = width; Height = height; Nodes = new List<PathNode>(); Closed = new List<PathNode>(); Open = new List<PathNode>(); for (int y = 0; y < height; y++) { for (int x = 0; x < width; x++) { PathNode node = new PathNode(); node.P = new Point(x, y); node.Location = new Vector3(x * GameTypes.SPRITE_WIDTH, y * GameTypes.SPRITE_HEIGHT, 0); node.Bounds = new BoundingBox(node.Location,new Vector3(node.Location.X+GameTypes.SPRITE_WIDTH,node.Location.Y+GameTypes.SPRITE_HEIGHT,0)); Nodes.Add(node); } } }