public bool Contains(PathNode inValue)
{
PathNode item = m_Map[inValue.X, inValue.Y];
if (item == null)
{
return(false);
}
#if DEBUG
if (!inValue.Equals(item))
{
throw new ApplicationException();
}
#endif
return(true);
}
private List <Vector2i> FindFinalPath(PathNode start, PathNode end)
{
closedList.Add(end);
PathNode parent = end.parent;
while (!parent.Equals(start))
{
closedList.Add(parent);
parent = parent.parent;
}
List <Vector2i> finalPath = new List <Vector2i>();
for (int i = closedList.Count - 1; i >= 0; i--)
{
finalPath.Add(closedList[i].tilePos);
}
return(finalPath);
}
private List <PathNode> _findPath(PathNode start, PathNode goal)
{
PathNode node = null;
openList = new List <PathNode>();
closedList = new List <PathNode>();
start.totalCost = 0.0f;
start.estimatedCost = heuristicEstimatedCost(start, goal);
openList.Enqueue(start);
while (openList.Count != 0)
{
node = openList.Dequeue();
closedList.Enqueue(node);
if (node.Equals(goal))
{
return(calculatePath(node));
}
var neighbours = pathfinder.getNeighbours(node);
foreach (var neighbour in neighbours)
{
var curDistance = neighbour.totalCost;
if (!openList.Contains(neighbour) && !closedList.Contains(neighbour))
{
neighbour.totalCost = curDistance + node.totalCost;
neighbour.estimatedCost = heuristicEstimatedCost(neighbour, goal);
neighbour.parent = node;
openList.Enqueue(neighbour);
}
else if (curDistance + node.totalCost < neighbour.totalCost)
{
neighbour.totalCost = curDistance + node.totalCost;
neighbour.parent = node;
if (openList.Contains(neighbour))
{
openList.Remove(neighbour);
}
if (closedList.Contains(neighbour))
{
closedList.Remove(neighbour);
}
openList.Enqueue(neighbour);
}
}
}
if (!node.Equals(goal))
{
return(null);
}
return(calculatePath(node));
}
protected List <PathNode> FindNearestPathToUnknown(PathNode start)
{
List <PathNode> open = new List <PathNode>();
List <PathNode> closed = new List <PathNode>();
// Add starting cell to open list
start.Parent = null;
start.GScore = 0;
start.OverallScore = 0;
open.Add(start);
// Set end parent to null so we know afterwards if we found it
PathNode end = null;
float endScore = 99999;
// Loop while open list has cells
while (open.Count > 0)
{
// Find node in open list with lowest cost
PathNode parent = open[0];
foreach (PathNode node in open)
{
if (node.OverallScore < parent.OverallScore)
{
parent = node;
}
}
// Remove this node from the open list and add it to the closed list
open.Remove(parent);
closed.Add(parent);
// Check adjacent nodes
foreach (PathNode node in parent.AdjacentNodesNoDiag)
{
// Store parrent if node is unknown
CellState state = cellStates[(int)node.Position.x, (int)node.Position.y];
if (state == CellState.UNKNOWN)
{
float score = parent.GScore;
if (currentTargets.Contains(node))
{
score *= 4.0f;
}
if (parent.GScore < endScore)
{
end = parent;
endScore = parent.GScore;
// End search if in fast mode (this will give us an
// unknown, but not necessarily the closest one)
#if FAST_NODE
goto end_search;
#endif
}
}
// Ignore blocked nodes
if (!node.Accessible)
{
continue;
}
if (closed.Contains(node))
{
continue;
}
// Calculate cost to get to this node from current node
float xdiff = Mathf.Abs(node.Position.x - start.Position.x);
float ydiff = Mathf.Abs(node.Position.y - start.Position.y);
float manhattenDistance = xdiff + ydiff;
float G = parent.GScore;
// Calculate score
float F = G;
// If this node is already on the open list
if (open.Contains(node))
{
// If this path is better, replace it
if (G < node.GScore)
{
node.GScore = G;
node.OverallScore = F;
node.Parent = parent;
}
}
else
{
node.GScore = G;
node.OverallScore = F;
node.Parent = parent;
// Add this node to the open list
open.Add(node);
}
}
}
#if FAST_NODE
end_search:
#endif
// Return an empty path if we couldn't find the end
if (end == null)
{
return(new List <PathNode>());
}
// Trace the path back
List <PathNode> path = new List <PathNode>();
PathNode currentNode = end;
while (!currentNode.Equals(start))
{
path.Add(currentNode);
currentNode = currentNode.Parent;
}
path.Add(currentNode);
path.Reverse();
return(path);
}
protected bool LookForPlayer()
{
const float MAX_TIME_LOOKING = 1.3f;
// Time spent looking to the left and then to the right
// The rest of the time is spent spinning around
// Looking to right time should be 2 * looking to left time
// since it has to undo the looking to left time
const float LOOKING_TO_LEFT_TIME = 0.3f;
const float LOOKING_TO_RIGHT_TIME = 0.6f;
float dt = Time.time - stateStartTime;
// Check if we can see the player and switch to chase player if so
if (CanSeeObjectOnLayer(FacingDirection, playerLayer, "Player"))
{
SwitchState(State.ATTACK_PLAYER);
currentPath = null;
return(false);
}
// Get player's last node
PathNode lastPlayerNode = cellGrid.GetPathfindingNode(cellGrid.GetCellAtPos(playerPos));
// Move to player's last known location if not there
if (gotPlayerPos)
{
if (!currentNode.Equals(lastPlayerNode) && currentPath == null)
{
if (Time.time - lastTimePlayerPathCalculated > PLAYER_PATH_REGEN_TIME)
{
lastTimePlayerPathCalculated = Time.time;
// Otherwise, pathfind to the last known position
List <PathNode> newPath = PathFinding.FindPath(currentNode,
cellGrid.GetPathfindingNode(cellGrid.GetCellAtPos(playerPos)));
// If we couldn't find a path there, look for the player at our current location
if (newPath.Count != 0)
{
currentPath = newPath;
}
else
{
// If there's no way to get to the player from our current location,
// Keep patroling the maze
SwitchState(State.MAP_LEVEL);
return(false);
}
}
}
}
if (currentNode.Equals(lastPlayerNode) || !gotPlayerPos)
{
// Only look for MAX_ITME_LOOKING seconds and then go back to normal activity
if (dt > MAX_TIME_LOOKING)
{
SwitchState(State.MAP_LEVEL);
currentPath = null;
return(false);
}
// Turn to the left to look for player
if (dt < LOOKING_TO_LEFT_TIME)
{
facingDirectionTarget = Quaternion.AngleAxis(-10.0f, Vector3.forward) * facingDirectionTarget;
}
// Turn to the right to look for player
else if (dt > LOOKING_TO_LEFT_TIME && dt < LOOKING_TO_LEFT_TIME + LOOKING_TO_RIGHT_TIME)
{
facingDirectionTarget = Quaternion.AngleAxis(10.0f, Vector3.forward) * facingDirectionTarget;
}
// Just spin around in circles
else
{
facingDirectionTarget = Quaternion.AngleAxis(-10.0f, Vector3.forward) * facingDirectionTarget;
}
}
else
{
TurnTowards(movementDir);
}
return(true);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
if (endNode == default)
{
return(null);
}
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
GenerateGrid();
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode.Equals(endNode))
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (var neighbourNode in GetNeighboursList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closedList.Add(neighbourNode);
continue;
}
int tentaiveGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentaiveGCost < neighbourNode.gCost)
{
neighbourNode.previousNode = currentNode;
neighbourNode.gCost = tentaiveGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
return(null);
}
//A* implementation
public static IEnumerator CalculatePath(PathNode start, PathNode end, List <PathNode> allNodes, System.Action <List <Vector2> > callback)
{
float timer = 0;
PriorityQueue openList = new PriorityQueue();
PriorityQueue closedList = new PriorityQueue();
openList.Push(start);
start.cost = 0;
start.estimatedCost = HeuristicEstimate(start, end, heuristicWeight);
PathNode currentNode = null;
while (openList.Count != 0)
{
currentNode = openList.Front();
if (currentNode == end)
{
break;
}
List <int> links = currentNode.links;
for (int i = 0; i != links.Count; i++)
{
PathNode endNode = allNodes[links[i]];
float incrementalCost = GetCost(currentNode, endNode);
float endNodeCost = currentNode.cost + incrementalCost;
if (closedList.Contains(endNode))
{
if (endNode.cost <= endNodeCost)
{
continue;
}
closedList.Remove(endNode);
}
else if (openList.Contains(endNode))
{
if (endNode.cost <= endNodeCost)
{
continue;
}
}
float endNodeHeuristic = HeuristicEstimate(endNode, end, heuristicWeight);
endNode.cost = endNodeCost;
endNode.parent = currentNode;
endNode.estimatedCost = endNodeCost + endNodeHeuristic;
if (!openList.Contains(endNode))
{
openList.Push(endNode);
}
}
closedList.Push(currentNode);
openList.Remove(currentNode);
timer = Mathf.Repeat(timer + Time.deltaTime, yieldInterval + 1);
if (timer >= yieldInterval)
{
yield return(0);
}
}
if (!currentNode.Equals(end))
{
// Debug.LogWarning("No path found :(");
callback(new List <Vector2>());
}
else
{
List <Vector2> path = new List <Vector2>();
while (currentNode != null)
{
path.Add(currentNode.pos);
currentNode = currentNode.parent;
}
path.Reverse();
callback(path);
}
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY, bool forced = false)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
var possibleFinalNodes = GetNeighbourList(endNode);
if (possibleFinalNodes == null || possibleFinalNodes.Count == 0)
{
return(null);
}
for (int i = 0; i < possibleFinalNodes.Count; i++)
{
if (!possibleFinalNodes[i].isWalkable)
{
possibleFinalNodes.Remove(possibleFinalNodes[i]);
}
}
if (possibleFinalNodes.Count == 0)
{
return(null);
}
DebugDrawer.DeleteCreateWorldText();
if (startNode == null || endNode == null)
{
// Invalid Path
return(null);
}
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < grid.GetWidth(); x++)
{
for (int y = 0; y < grid.GetHeight(); y++)
{
PathNode pathNode = grid.GetGridObject(x, y);
pathNode.gCost = 99999999;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode.Equals(endNode) || possibleFinalNodes.Contains(currentNode))
{
// Reached final node
return(CalculatePath(currentNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
/* Code for optimizing pathfinding with multiblocks;
* if (neighbourNode.structure is IMultipleNodesStructure) //Adding all other occupied nodes from the multiblock to the closedlist;
* {
* List<PathNode> disposableNodes = (neighbourNode.structure as IMultipleNodesStructure).getPathNodeList();
* foreach (var node in disposableNodes)
* {
* closedList.Add(node);
* }
* }
*/
if (!neighbourNode.isWalkable && !forced)
{
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
DebugDrawer.CreateWorldText(neighbourNode.hCost.ToString(), null,
grid.GetWorldPosition(neighbourNode.x, neighbourNode.y) - Vector3.one * 0.2f +
new Vector3(grid.GetCellSize(), grid.GetCellSize()) * .5f, 20,
neighbourNode.isWalkable ? Color.white : Color.red,
TextAnchor.MiddleCenter);
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
//PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList);
}
}
// Out of nodes on the openList
return(null);
}
// Calculate the A* path
public static List <PathNode> Calculate(PathNode start, PathNode goal)
{
List <PathNode> closedset = new List <PathNode>(); // The set of nodes already evaluated.
List <PathNode> openset = new List <PathNode>(); // The set of tentative nodes to be evaluated.
openset.Add(start);
Dictionary <PathNode, PathNode> came_from = new Dictionary <PathNode, PathNode>(); // The map of navigated nodes.
Dictionary <PathNode, float> g_score = new Dictionary <PathNode, float>();
g_score[start] = 0.0f; // Cost from start along best known path.
Dictionary <PathNode, float> h_score = new Dictionary <PathNode, float>();
h_score[start] = HeuristicCostEstimate(start, goal);
Dictionary <PathNode, float> f_score = new Dictionary <PathNode, float>();
f_score[start] = h_score[start]; // Estimated total cost from start to goal through y.
while (openset.Count != 0)
{
PathNode x = LowestScore(openset, f_score);
if (x.Equals(goal))
{
List <PathNode> result = new List <PathNode>();
ReconstructPath(came_from, x, ref result);
return(result);
}
openset.Remove(x);
closedset.Add(x);
foreach (PathNode y in x.Connections)
{
if (AStarScript.Invalid(y) || closedset.Contains(y))
{
continue;
}
float tentative_g_score = g_score[x] + Distance(x, y);
bool tentative_is_better = false;
if (!openset.Contains(y))
{
openset.Add(y);
tentative_is_better = true;
}
else if (tentative_g_score < g_score[y])
{
tentative_is_better = true;
}
if (tentative_is_better)
{
came_from[y] = x;
g_score[y] = tentative_g_score;
h_score[y] = HeuristicCostEstimate(y, goal);
f_score[y] = g_score[y] + h_score[y];
}
}
}
return(null);
}
private void FindPath(int2 startPosition, int2 endPosition)
{
Debug.Log("Pathfinding to: " + endPosition);
if (startPosition.Equals(endPosition) || Grid[GetIndex(endPosition)] == -1)
{
Debug.LogError("asking to go into an obstacle, or is already there!");
foundPath = false;
return;
}
PathNode head = new PathNode(startPosition, CalculateDistanceCost(startPosition, endPosition));
OpenSet.Push(head);
while (itterationLimit > 0 && OpenSet.HasNext())
{
int currentIndex = OpenSet.Pop();
PathNode current = OpenSet[currentIndex];
int ind = GetIndex(current.Position);
PathNode cameFromNode = CameFrom[ind];
if (current.Position.Equals(endPosition))
{
//Found our destination, we will let the cleanup job handle the path reconstruction for now
//ReconstructPath(startPosition, endPosition);
return;
}
float initialCost = CostSoFar[GetIndex(current.Position)];
PathNode[] neighbourNodes = new PathNode[Neighbours.Length];
for (int i = 0; i < Neighbours.Length; i++)
{
int2 neighbour = Neighbours[i];
int2 position = current.Position + neighbour;
if (position.x < 0 || position.x >= DimX || position.y < 0 || position.y >= DimY)
{
continue;
}
int index = GetIndex(position);
float cellCost = GetCellCost(currentIndex, index, true);
if (float.IsInfinity(cellCost))
{
current.NextToObstacle = true;
continue;
}
neighbourNodes[i] = new PathNode(position, cellCost);
}
if (!cameFromNode.Equals(null) && cameFromNode.NextToObstacle && current.NextToObstacle && IsDiagonal(current.Position, cameFromNode.Position))
{
//In this case, the path came from point that was next to a obstacle, and is moving diagonally towards a point next to an obstacle. so we are assuming they are moving diagonally through the obstacle
//TODO: this is not always the case, will need to resolve later
continue;
}
for (int i = 0; i < neighbourNodes.Length; i++)
{
int2 neighbour = Neighbours[i];
PathNode neighbourNode = neighbourNodes[i];
int index = GetIndex(neighbourNode.Position);
if (neighbourNode.Equals(null))
{
Debug.Log("neighbour null");
continue;
}
float neighbourCost = 10;
if ((math.abs(neighbour.x) + math.abs(neighbour.y)) == 2)
{
neighbourCost = 14;
}
float newCost = initialCost + neighbourCost + neighbourNode.ExpectedCost;
float oldCost = CostSoFar[index];
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
CostSoFar[index] = newCost;
CameFrom[index] = current;
neighbourNode.ExpectedCost = newCost + CalculateDistanceCost(neighbourNode.Position, endPosition);
OpenSet.Push(neighbourNode);
}
itterationLimit--;
}
if (OpenSet.HasNext())
{
//We ran out of itterations
//We will just give out where we stapped at for now
//TODO: fix this
var currentIndex = OpenSet.Pop();
endPosition = OpenSet[currentIndex].Position;
}
}
// A* Pathfinding
public static List<PathNode> FindPath(PathNode start, PathNode end)
{
// TODO: diagonal cost not working right??}{???
const float STRAIGHT_COST = 1.0f;
const float DIAG_COST = 1.41421356237f;
List<PathNode> open = new List<PathNode>();
List<PathNode> closed = new List<PathNode>();
// Return immediately if we're already at the end
// or the cell we're on is blocked
if (start.Equals(end))
{
List<PathNode> nodes = new List<PathNode>();
nodes.Add(start);
return nodes;
}
// Add starting cell to open list
start.Parent = null;
start.GScore = 0;
start.OverallScore = 0;
open.Add(start);
// Set end parent to null so we know afterwards if we found it
end.Parent = null;
// Loop while open list has cells
while (open.Count > 0)
{
// Find node in open list with lowest cost
PathNode parent = open[0];
foreach (PathNode node in open)
{
if (node.OverallScore < parent.OverallScore)
parent = node;
}
// Remove this node from the open list and add it to the closed list
open.Remove(parent);
closed.Add(parent);
// Check if we've found the end
if (parent == end)
break;
// Check adjacent nodes
foreach (PathNode node in parent.AdjacentNodes)
{
// Ignore blocked nodes
if (!node.Accessible)
continue;
if (closed.Contains(node))
continue;
// Calculate cost to get to this node from current node
float xdiff = Mathf.Abs(node.Position.x - start.Position.x);
float ydiff = Mathf.Abs(node.Position.y - start.Position.y);
float manhattenDistance = xdiff + ydiff;
float G = parent.GScore;
if (manhattenDistance == 1.0f)
G += STRAIGHT_COST;
else
G += DIAG_COST;
// Calculate heuristic distance from this cell to the end
xdiff = Mathf.Abs(node.Position.x - end.Position.x);
ydiff = Mathf.Abs(node.Position.x - end.Position.x);
manhattenDistance = xdiff + ydiff;
float H = manhattenDistance * STRAIGHT_COST;
// Calculate score
float F = G + H;
// If this node is already on the open list
if (open.Contains(node))
{
// If this path is better, replace it
if (G < node.GScore)
{
node.GScore = G;
node.OverallScore = F;
node.Parent = parent;
}
}
else
{
node.GScore = G;
node.OverallScore = F;
node.Parent = parent;
// Add this node to the open list
open.Add(node);
}
}
}
// Return an empty path if we couldn't find the end
if (end.Parent == null)
return new List<PathNode>();
// Trace the path back
List<PathNode> path = new List<PathNode>();
PathNode currentNode = end;
while (!currentNode.Equals(start))
{
path.Add(currentNode);
currentNode = currentNode.Parent;
}
path.Add(currentNode);
path.Reverse();
return path;
}
// A* Pathfinding
public static List <PathNode> FindPath(PathNode start, PathNode end)
{
// TODO: diagonal cost not working right??}{???
const float STRAIGHT_COST = 1.0f;
const float DIAG_COST = 1.41421356237f;
List <PathNode> open = new List <PathNode>();
List <PathNode> closed = new List <PathNode>();
// Return immediately if we're already at the end
// or the cell we're on is blocked
if (start.Equals(end))
{
List <PathNode> nodes = new List <PathNode>();
nodes.Add(start);
return(nodes);
}
// Add starting cell to open list
start.Parent = null;
start.GScore = 0;
start.OverallScore = 0;
open.Add(start);
// Set end parent to null so we know afterwards if we found it
end.Parent = null;
// Loop while open list has cells
while (open.Count > 0)
{
// Find node in open list with lowest cost
PathNode parent = open[0];
foreach (PathNode node in open)
{
if (node.OverallScore < parent.OverallScore)
{
parent = node;
}
}
// Remove this node from the open list and add it to the closed list
open.Remove(parent);
closed.Add(parent);
// Check if we've found the end
if (parent == end)
{
break;
}
// Check adjacent nodes
foreach (PathNode node in parent.AdjacentNodes)
{
// Ignore blocked nodes
if (!node.Accessible)
{
continue;
}
if (closed.Contains(node))
{
continue;
}
// Calculate cost to get to this node from current node
float xdiff = Mathf.Abs(node.Position.x - start.Position.x);
float ydiff = Mathf.Abs(node.Position.y - start.Position.y);
float manhattenDistance = xdiff + ydiff;
float G = parent.GScore;
if (manhattenDistance == 1.0f)
{
G += STRAIGHT_COST;
}
else
{
G += DIAG_COST;
}
// Calculate heuristic distance from this cell to the end
xdiff = Mathf.Abs(node.Position.x - end.Position.x);
ydiff = Mathf.Abs(node.Position.x - end.Position.x);
manhattenDistance = xdiff + ydiff;
float H = manhattenDistance * STRAIGHT_COST;
// Calculate score
float F = G + H;
// If this node is already on the open list
if (open.Contains(node))
{
// If this path is better, replace it
if (G < node.GScore)
{
node.GScore = G;
node.OverallScore = F;
node.Parent = parent;
}
}
else
{
node.GScore = G;
node.OverallScore = F;
node.Parent = parent;
// Add this node to the open list
open.Add(node);
}
}
}
// Return an empty path if we couldn't find the end
if (end.Parent == null)
{
return(new List <PathNode>());
}
// Trace the path back
List <PathNode> path = new List <PathNode>();
PathNode currentNode = end;
while (!currentNode.Equals(start))
{
path.Add(currentNode);
currentNode = currentNode.Parent;
}
path.Add(currentNode);
path.Reverse();
return(path);
}