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);
}
