public PathFinderFast(byte[,] grid) { if (grid == null) { throw new Exception("Grid cannot be null"); } mGrid = grid; mGridX = (ushort)(mGrid.GetUpperBound(0) + 1); mGridY = (ushort)(mGrid.GetUpperBound(1) + 1); mGridXMinus1 = (ushort)(mGridX - 1); mGridYLog2 = (ushort)Math.Log(mGridY, 2); // This should be done at the constructor, for now we leave it here. //if (Math.Log(mGridX, 2) != (int) Math.Log(mGridX, 2) || // Math.Log(mGridY, 2) != (int) Math.Log(mGridY, 2)) // throw new Exception("Invalid Grid, size in X and Y must be power of 2"); // Chris: change this one to static variables, and add a flag to indicate whether the grid is a new one if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY)) { mCalcGrid = new PathFinderNodeFast[mGridX * mGridY]; } mCalcGridStatus = new byte[mGridX * mGridY]; mOpen = new PriorityQueueB <int>(new ComparePFNodeMatrix(mCalcGrid)); }
void Debug(PriorityQueueB <pathrecnode> processing, int iter) { Console.WriteLine("------------------------------------------------"); Console.WriteLine("Iteration " + iter); for (int i = 0; i < processing.Count; i++) { Console.WriteLine(processing[i].ToString()); } }
/// <summary> /// Initializes a new instance of the <see cref="AStar"/> class. /// </summary> /// <param name="map">The map.</param> public AStar(IMap map) { this.map = map; foreach (Waypoint way in map.Waypoints.GetWaypointsList()) { way.Node = new IAPathfinderNode(); } openQueue = new PriorityQueueB <Waypoint>(new IANodeComparer()); closeList = new List <Waypoint>(); }
/// <summary> /// Initializes a new instance of the <see cref="AStar"/> class. /// </summary> /// <param name="map">The map.</param> public AStar(IMap map) { this.map = map; foreach (Waypoint way in map.Waypoints.GetWaypointsList()) { way.Node = new IAPathfinderNode(); } openQueue = new PriorityQueueB<Waypoint>( new IANodeComparer()); closeList = new List<Waypoint>(); }
public virtual void Initilize() { if (initialized) { Exception err = new Exception("ERROR: AIPathfinder.Initialize() - Can't initialize more then once!"); } if (map == null) { Exception err = new Exception("ERROR: AIPathfinder.Initialize() - Set map before calling this method!"); } if (character == null) { Exception err = new Exception("ERROR: AIPathfinder.Initilize() - Set character before calling this method!"); } openQueue = new PriorityQueueB <AIPathfinderNode>(new AINodeComparer()); closeList = new List <AIPathfinderNode>(); if (grid == null) { grid = new byte[map.Width, map.Height]; // get the cost of each node as seend from character's perspective for (int w = 0; w < map.Width; w++) { for (int h = 0; h < map.Height; h++) { grid[w, h] = character.TypeToCost( map.Node(w, h).Type ); } } } parentNode = new AIPathfinderNode(); parentNode.G = 0; parentNode.H = heuristicEstimateValue; parentNode.F = parentNode.G + parentNode.H; parentNode.X = startNode.X; parentNode.Y = startNode.Y; parentNode.PX = parentNode.X; parentNode.PY = parentNode.Y; initialized = true; this.state = AIPathfinderState.Idle; }
void Start() { gridConstructor = GetComponent <GridConstructor>(); gridX = (ushort)gridConstructor.gridSizeX; gridY = (ushort)gridConstructor.gridSizeY; gridXMinus1 = (ushort)(gridX - 1); gridYLog2 = (ushort)Mathf.Log(gridY, 2); if (Mathf.Log(gridX, 2) != (int)Mathf.Log(gridX, 2) || Mathf.Log(gridY, 2) != (int)Mathf.Log(gridY, 2)) { Debug.Log("Invalid Grid, size in X and Y must be power of 2"); } if (calcGrid == null || calcGrid.Length != gridX * gridY) { calcGrid = new AStarNode[gridX * gridY]; } openSet = new PriorityQueueB <int>(new ComparePFNodeMatrix(calcGrid)); }
public PathFinder(byte[] grid, int gridSizeX, int gridSizeY) { if (grid == null) { throw new Exception("Grid cannot be null"); } if (grid.Length != gridSizeX * gridSizeY) { throw new Exception("Grid dimensions are inconsistent"); } _grid = grid; _gridSizeX = (ushort)gridSizeX; _gridSizeY = (ushort)gridSizeY; _calcGrid = new PathFinderNodeFast[_gridSizeX * _gridSizeY]; _open = new PriorityQueueB <int>(new ComparePFNode(_calcGrid)); }
public AIPathFinder(byte[,] xyGrid) { if (xyGrid == null) { throw new ArgumentException("xyGrid cannot be null"); } mGrid = xyGrid; mGridX = (ushort)(mGrid.GetLength(0)); mGridY = (ushort)(mGrid.GetLength(1)); mGridXMinus1 = (ushort)(mGridX - 1); mGridYLog2 = (ushort)Math.Log(mGridY, 2); if (Math.Log(mGridX, 2) != (int)Math.Log(mGridX, 2) || Math.Log(mGridY, 2) != (int)Math.Log(mGridY, 2)) { throw new Exception("Invalid Grid, size in X and Y must be power of 2"); } mCalcGrid = new PathFinderNodeFast[mGridX * mGridY]; mOpen = new PriorityQueueB <int>(new ComparePFNodeMatrix(mCalcGrid)); }
public Pathfinding(byte[,] grid, PathMap map) { if (map == null) { throw new Exception("Map is null"); } if (grid == null) { throw new Exception("Grid is null"); } Map = map; Grid = grid; GridX = (ushort)(Grid.GetUpperBound(0) + 1); GridY = (ushort)(Grid.GetUpperBound(1) + 1); GridXMinus1 = (ushort)(GridX - 1); GridXLog2 = (ushort)Math.Log(GridX, 2); if (Math.Log(GridX, 2) != (int)Math.Log(GridX, 2) || Math.Log(GridY, 2) != (int)Math.Log(GridY, 2)) { throw new Exception("Invalid Grid, size in X and Y must be power of 2"); } if (nodes == null || nodes.Length != (GridX * GridY)) { nodes = new List <Node> [GridX * GridY]; TouchedLocations = new Stack <int>(GridX * GridY); Close = new List <Point>(GridX * GridY); } for (var i = 0; i < nodes.Length; ++i) { nodes[i] = new List <Node>(1); } Open = new PriorityQueueB <Location>(new ComparePFNodeMatrix(nodes)); }
public Path FindPath(MapPoint startPoint, MapPoint endPoint, bool diagonal, int movementPoints = (short)-1) { var success = false; var matrix = new PathNode[MapPoint.MapSize + 1]; var openList = new PriorityQueueB <short>(new ComparePfNodeMatrix(matrix)); var closedList = new List <PathNode>(); var location = startPoint.CellId; var counter = 0; if (movementPoints == 0) { return(Path.GetEmptyPath(CellsInformationProvider.Map, CellsInformationProvider.Map.Cells[startPoint.CellId])); } matrix[location].Cell = location; matrix[location].Parent = -1; matrix[location].G = 0; matrix[location].F = EstimateHeuristic; matrix[location].Status = NodeState.Open; openList.Push(location); while (openList.Count > 0) { location = openList.Pop(); var locationPoint = new MapPoint(location); if (matrix[location].Status == NodeState.Closed) { continue; } if (location == endPoint.CellId) { matrix[location].Status = NodeState.Closed; success = true; break; } if (counter > SearchLimit) { return(Path.GetEmptyPath(CellsInformationProvider.Map, CellsInformationProvider.Map.Cells[startPoint.CellId])); } for (int i = 0; i < (diagonal ? 8 : 4); i++) { var newLocationPoint = locationPoint.GetNearestCellInDirection(Directions[i]); if (newLocationPoint == null) { continue; } var newLocation = newLocationPoint.CellId; if (newLocation < 0 || newLocation >= MapPoint.MapSize) { continue; } if (!MapPoint.IsInMap(newLocationPoint.X, newLocationPoint.Y)) { continue; } if (!CellsInformationProvider.IsCellWalkable(newLocation)) { continue; } double newG = matrix[location].G + 1; if ((matrix[newLocation].Status == NodeState.Open || matrix[newLocation].Status == NodeState.Closed) && matrix[newLocation].G <= newG) { continue; } matrix[newLocation].Cell = newLocation; matrix[newLocation].Parent = location; matrix[newLocation].G = newG; matrix[newLocation].H = GetHeuristic(newLocationPoint, endPoint); matrix[newLocation].F = newG + matrix[newLocation].H; openList.Push(newLocation); matrix[newLocation].Status = NodeState.Open; } counter++; matrix[location].Status = NodeState.Closed; } if (success) { var node = matrix[endPoint.CellId]; while (node.Parent != -1) { closedList.Add(node); node = matrix[node.Parent]; } closedList.Add(node); } closedList.Reverse(); if (movementPoints > 0 && closedList.Count + 1 > movementPoints) { return(new Path(CellsInformationProvider.Map, closedList.Take(movementPoints + 1).Select(entry => CellsInformationProvider.Map.Cells[entry.Cell]))); } return(new Path(CellsInformationProvider.Map, closedList.Select(entry => CellsInformationProvider.Map.Cells[entry.Cell]))); }
public MapPoint[] FindReachableCells(MapPoint from, int distance) { var result = new List <MapPoint>(); var matrix = new PathNode[MapPoint.MapSize + 1]; var openList = new PriorityQueueB <short>(new ComparePfNodeMatrix(matrix)); var location = from.CellId; var counter = 0; if (distance == 0) { return new [] { new MapPoint(from.CellId) } } ; matrix[location].Cell = location; matrix[location].Parent = -1; matrix[location].G = 0; matrix[location].F = 0; matrix[location].Status = NodeState.Open; openList.Push(location); while (openList.Count > 0) { location = openList.Pop(); var locationPoint = new MapPoint(location); if (matrix[location].Status == NodeState.Closed) { continue; } if (counter > SearchLimit) { break; } for (int i = 0; i < 4; i++) { var newLocationPoint = locationPoint.GetNearestCellInDirection(Directions[i]); if (newLocationPoint == null) { continue; } var newLocation = newLocationPoint.CellId; if (newLocation < 0 || newLocation >= MapPoint.MapSize) { continue; } if (!MapPoint.IsInMap(newLocationPoint.X, newLocationPoint.Y)) { continue; } if (!CellsInformationProvider.IsCellWalkable(newLocation)) { continue; } double newG = matrix[location].G + 1; if ((matrix[newLocation].Status == NodeState.Open || matrix[newLocation].Status == NodeState.Closed) && matrix[newLocation].G <= newG) { continue; } matrix[newLocation].Cell = newLocation; matrix[newLocation].Parent = location; matrix[newLocation].G = newG; matrix[newLocation].H = 0; matrix[newLocation].F = newG + matrix[newLocation].H; if (newG <= distance) { result.Add(newLocationPoint); openList.Push(newLocation); matrix[newLocation].Status = NodeState.Open; } } counter++; matrix[location].Status = NodeState.Closed; } return(result.ToArray()); }
public List <Node> Astar(int[,] map, Vector2Int startPos, Vector2Int goalPos) { bool found = false; bool stop = false; PriorityQueueB <Node> open = new PriorityQueueB <Node>(new ComparePFNode()); List <Node> close = new List <Node>(); Node parentNode = new Node(startPos); Node foundNode = null; parentNode.GCost = 0; parentNode.HCost = 0; parentNode.FCost = parentNode.GCost + parentNode.HCost; open.Push(parentNode); while (open.Count > 0 && !stop) { parentNode = open.Pop(); if (parentNode.Position.x == goalPos.x && parentNode.Position.y == goalPos.y) { foundNode = parentNode; close.Add(parentNode); found = true; break; } if (close.Count > SearchLimit) { Debug.LogWarning("Search limit exceeded."); return(null); } var successors = GetAllSuccessorNodes(map, parentNode); //Lets calculate each successors foreach (var node in successors) { float newG = parentNode.GCost + 0; // 0 is tile difficulty //if ((int)newG == (int)parentNode.GCost) //{ // Debug.Log("WHAT?"); // continue; //} // Need a proper search method int foundInOpenIndex = -1; for (int j = 0; j < open.Count; j++) { if (open[j].Position.x == node.Position.x && open[j].Position.y == node.Position.y) { foundInOpenIndex = j; break; } } if (foundInOpenIndex != -1 && open[foundInOpenIndex].GCost <= newG) { continue; } int foundInCloseIndex = -1; for (int j = 0; j < close.Count; j++) { if (close[j].Position.x == node.Position.x && close[j].Position.y == node.Position.y) { foundInCloseIndex = j; break; } } if (foundInCloseIndex != -1 && close[foundInCloseIndex].GCost <= newG) { continue; } node.Parent = parentNode; node.GCost = newG; node.HCost = Vector2Int.Distance(node.Position, goalPos); node.FCost = node.GCost + node.HCost; open.Push(node); } close.Add(parentNode); } if (found) { List <Node> path = new List <Node>(); ConstructPathRecursive(foundNode, path); path.Reverse(); return(path); } return(null); }
public bool Rebuild(IEnumerable<Point> blockers) { NavNode[] newGrid = new NavNode[_map.Width * _map.Height]; PriorityQueueB<int> open = new PriorityQueueB<int>( (a,b) => newGrid[a]._cost.CompareTo(newGrid[b]._cost) ); open.Push(_targetX + _targetY * _map.Width); for(int i = 0; i < newGrid.Length; ++i) { newGrid[i]._cost = _map.Blocks[i].Type == BlockType.Solid ? BlockerCost : 0; newGrid[i]._nextIndex = -1; } if(blockers != null) { foreach(Point p in blockers) { newGrid[p.X + p.Y * _map.Width]._cost = BlockerCost; } } while(open.Count > 0) { // Get the lowest cost open node int nodeIndex = open.Pop(); int nodeX = nodeIndex % _map.Width; int nodeY = nodeIndex / _map.Width; for(int i = 0; i < 8; ++i) { int nextNodeX = nodeX + NeighborLookups[i*2+0]; int nextNodeY = nodeY + NeighborLookups[i*2+1]; if(nextNodeX < 0 || nextNodeX >= _map.Width || nextNodeY < 0 || nextNodeY >= _map.Height) continue; int nextNodeIndex = nextNodeX + nextNodeY * _map.Width; int nextCost = newGrid[nodeIndex]._cost + (i >= 4 ? 14 : 10); if(IsValidMove(newGrid, nodeX, nodeY, nextNodeX, nextNodeY)) { // If the neighbor is not already visited, and walkable if(newGrid[nextNodeIndex]._nextIndex == -1) { newGrid[nextNodeIndex]._cost = nextCost; newGrid[nextNodeIndex]._nextIndex = nodeIndex; // add it to the open queue open.Push(nextNodeIndex); } else // Update the neighbor if this is a shorter path if(nextCost < newGrid[nextNodeIndex]._cost) { newGrid[nextNodeIndex]._cost = nextCost; newGrid[nextNodeIndex]._nextIndex = nodeIndex; } } } } // Verify that all enemy spawn points can reach the target. if( 0 != _map.EnemySpawns.Count(sp => newGrid[sp.BlockX + sp.BlockY * _map.Width]._nextIndex < 0)) { return false; } Grid = newGrid; return true; }
public override PlanSet CreatePlan(WorldState actual, Goal destiny) { int iter = 0; PlanSet PlanSet = new PlanSet(); PriorityQueueB <pathrecnode> processing = new PriorityQueueB <pathrecnode>(new comparer()); List <WorldState> close = new List <WorldState>(); processing.Push( new pathrecnode() { act = new List <Action>(), WorldState = actual, f = 0, g = 0, h = 0, } ); pathrecnode current = null; while (processing.Count != 0) { current = processing.Pop(); if (destiny.Evaluate(current.WorldState) == true) { break; } if (close.Contains(current.WorldState)) { continue; } else { close.Add(current.WorldState); } List <Action> acts = new List <Action>(); foreach (var item in Actions) { if (item.GetPreConditions(current.WorldState).isCompatibleSource(current.WorldState)) { if (item.ProceduralPreConditions(current.WorldState)) { acts.Add(item); } } } foreach (var item in acts) { WorldState ws = current.WorldState.Clone(); foreach (var item2 in item.GetEffects(current.WorldState).GetSymbols()) { ws.SetSymbol(item2.Clone()); } item.ApplyEffects(ws); pathrecnode pathrec = new pathrecnode(); pathrec.WorldState = ws; pathrec.act = new List <Action>(current.act.ToArray()); pathrec.act.Add(item); pathrec.g += 1 + item.Cost; pathrec.h = destiny.GetHeuristic(pathrec.WorldState); //pathrec.WorldState.GetHeuristic(destiny.WorldState); pathrec.f = pathrec.g + pathrec.h; processing.Push(pathrec); } iter++; if (iter > MaxIteration) { return(null); } Debug(processing, iter); } if (current != null) { foreach (var item in current.act) { PlanSet.Actions.Add(item); System.Diagnostics.Debug.WriteLine(item.Name); } } return(PlanSet); }
public PathFinderFast(byte[,] grid) { if (grid == null) throw new Exception("Grid cannot be null"); mGrid = grid; mGridX = (ushort) (mGrid.GetUpperBound(0) + 1); mGridY = (ushort) (mGrid.GetUpperBound(1) + 1); mGridXMinus1 = (ushort) (mGridX - 1); mGridYLog2 = (ushort) Math.Log(mGridY, 2); // This should be done at the constructor, for now we leave it here. if (Math.Log(mGridX, 2) != (int) Math.Log(mGridX, 2) || Math.Log(mGridY, 2) != (int) Math.Log(mGridY, 2)) throw new Exception("Invalid Grid, size in X and Y must be power of 2"); if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY)) mCalcGrid = new PathFinderNodeFast[mGridX * mGridY]; mOpen = new PriorityQueueB<int>(ComparePFNodeMatrix); }
public Path FindPath(short startCell, short endCell, bool diagonal, int movementPoints = -1) { bool flag = false; PathNode[] matrix = new PathNode[561]; PriorityQueueB <short> priorityQueueB = new PriorityQueueB <short>((IComparer <short>) new Pathfinder.ComparePfNodeMatrix(matrix)); List <PathNode> list = new List <PathNode>(); MapPoint mapPoint1 = new MapPoint(startCell); MapPoint pointB = new MapPoint(endCell); short num1 = startCell; int num2 = 0; Path path; if (movementPoints == 0) { path = Path.GetEmptyPath(this.CellsInformationProvider.Map, this.CellsInformationProvider.Map.Cells[(int)startCell]); } else { matrix[(int)num1].Cell = num1; matrix[(int)num1].Parent = (short)-1; matrix[(int)num1].G = 0.0; matrix[(int)num1].F = (double)Pathfinder.EstimateHeuristic; matrix[(int)num1].Status = NodeState.Open; priorityQueueB.Push(num1); while (priorityQueueB.Count > 0) { short cellId1 = priorityQueueB.Pop(); MapPoint mapPoint2 = new MapPoint(cellId1); if (matrix[(int)cellId1].Status != NodeState.Closed) { if ((int)cellId1 != (int)endCell) { if (num2 <= Pathfinder.SearchLimit) { for (int index = 0; index < (!diagonal ? 4 : 8); ++index) { MapPoint nearestCellInDirection = mapPoint2.GetNearestCellInDirection(Pathfinder.Directions[index]); if (nearestCellInDirection != null) { short cellId2 = nearestCellInDirection.CellId; if (((int)cellId2 < 0 ? 0 : ((long)cellId2 < 560L ? 1 : 0)) != 0 && MapPoint.IsInMap(nearestCellInDirection.X, nearestCellInDirection.Y) && this.CellsInformationProvider.IsCellWalkable(cellId2)) { double num3 = matrix[(int)cellId1].G + 1.0; if ((matrix[(int)cellId2].Status == NodeState.Open || matrix[(int)cellId2].Status == NodeState.Closed ? (matrix[(int)cellId2].G > num3 ? 1 : 0) : 1) != 0) { matrix[(int)cellId2].Cell = cellId2; matrix[(int)cellId2].Parent = cellId1; matrix[(int)cellId2].G = num3; matrix[(int)cellId2].H = Pathfinder.GetHeuristic(nearestCellInDirection, pointB); matrix[(int)cellId2].F = num3 + matrix[(int)cellId2].H; priorityQueueB.Push(cellId2); matrix[(int)cellId2].Status = NodeState.Open; } } } } ++num2; matrix[(int)cellId1].Status = NodeState.Closed; } else { path = Path.GetEmptyPath(this.CellsInformationProvider.Map, this.CellsInformationProvider.Map.Cells[(int)startCell]); goto label_23; } } else { matrix[(int)cellId1].Status = NodeState.Closed; flag = true; break; } } } if (flag) { PathNode pathNode; for (pathNode = matrix[(int)endCell]; (int)pathNode.Parent != -1; pathNode = matrix[(int)pathNode.Parent]) { list.Add(pathNode); } list.Add(pathNode); } list.Reverse(); path = (movementPoints <= 0 ? 1 : (list.Count + 1 <= movementPoints ? 1 : 0)) != 0 ? new Path(this.CellsInformationProvider.Map, Enumerable.Select <PathNode, Cell>((IEnumerable <PathNode>)list, (Func <PathNode, Cell>)(entry => this.CellsInformationProvider.Map.Cells[(int)entry.Cell]))) : new Path(this.CellsInformationProvider.Map, Enumerable.Select <PathNode, Cell>(Enumerable.Take <PathNode>((IEnumerable <PathNode>)list, movementPoints + 1), (Func <PathNode, Cell>)(entry => this.CellsInformationProvider.Map.Cells[(int)entry.Cell]))); } label_23: return(path); }