private bool FindPath(int2 startPosition, int2 endPosition, out NativeList <int2> pathList)
{
int2 gridSize = new int2(59, 42);
NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; y < gridSize.y; y++)
{
PathNode pathNode = new PathNode
{
x = x,
y = y,
index = CalculateIndex(x, y, gridSize.x),
gCost = int.MaxValue,
hCost = CalculateDistanceCost(new int2(x, y), endPosition)
};
pathNode.CalculateFCost();
pathNode.isWalkable = GridManager.Instance.IsWalkable(x, y);
pathNode.cameFromNodeIndex = -1;
pathNodeArray[pathNode.index] = pathNode;
}
}
NativeArray <int2> neighbourOffsetArray = new NativeArray <int2>(new int2[]
{
new int2(-1, 0),
new int2(1, 0),
new int2(0, 1),
new int2(0, -1),
new int2(-1, -1),
new int2(-1, 1),
new int2(1, -1),
new int2(1, 1),
}, Allocator.Temp);
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currentNodeIndex];
if (currentNodeIndex == endNodeIndex)
{
break;
}
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currentNodeIndex);
for (int i = 0; i < neighbourOffsetArray.Length; i++)
{
int2 neighbourOffest = neighbourOffsetArray[i];
int2 neighbourPosition = new int2(currentNode.x + neighbourOffest.x, currentNode.y + neighbourOffest.y);
if (!isInsideGrid(neighbourPosition, gridSize))
{
continue;
}
int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x);
if (closedList.Contains(neighbourNodeIndex))
{
continue;
}
PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
if (neighbourNode.isWalkable == 1)
{
continue;
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNodeIndex = currentNodeIndex;
neighbourNode.gCost = tentativeGCost;
neighbourNode.CalculateFCost();
pathNodeArray[neighbourNodeIndex] = neighbourNode;
if (!openList.Contains(neighbourNodeIndex))
{
openList.Add(neighbourNodeIndex);
}
}
}
}
PathNode endNode = pathNodeArray[endNodeIndex];
if (endNode.cameFromNodeIndex == -1)
{
pathNodeArray.Dispose();
openList.Dispose();
closedList.Dispose();
neighbourOffsetArray.Dispose();
Debug.Log("No Paths Available");
pathList = new NativeList <int2>();
return(false);
}
else
{
NativeList <int2> path = CalculatePath(pathNodeArray, endNode);
// for (int i = 0; i < path.Length; i++)
// {
// Debug.Log(path[i]);
// }
pathList = path;
pathNodeArray.Dispose();
openList.Dispose();
closedList.Dispose();
neighbourOffsetArray.Dispose();
return(true);
}
}
private void FindPath(int2 startPositon, int2 endPosition)
{
int2 gridSize = new int2(4, 4);
NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; x < gridSize.y; y++)
{
PathNode pathNode = new PathNode();
pathNode.x = x;
pathNode.y = y;
pathNode.index = CalculateIndex(x, y, gridSize.x);
pathNode.gCost = int.MaxValue;
pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition);
pathNode.CalculateFCost();
pathNode.isWalkable = true;
pathNode.cameFromNodeIndex = -1;
pathNodeArray[pathNode.index] = pathNode;
}
}
NativeArray <int2> neighbourOffseArray = new NativeArray <int2>(new int2[] {
new int2(-1, 0), // Left
new int2(+1, 0), // Right
new int2(0, +1), // Up
new int2(0, -1), // Down
new int2(-1, -1), // Left Down
new int2(-1, +1), // Left Up
new int2(+1, -1), // Right Down
new int2(+1, +1), // Right UP
}, Allocator.Temp);
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startPositon.x, startPositon.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currentNodeIndex];
if (currentNodeIndex == endNodeIndex)
{
// Destination reached! Congrats!! I Hope you have a good travel!
break;
}
// Remove Current node from the OPEN LIST
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currentNodeIndex);
for (int i = 0; i < neighbourOffseArray.Length; i++)
{
int2 neighbourOffset = neighbourOffseArray[i];
int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);
if (!IsPositionInsideGrid(neighbourPosition, gridSize))
{
// This neighbour is not a valid position
continue;
}
int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x);
if (closedList.Contains(neighbourNodeIndex))
{
// Node Already Searched
continue;
}
PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
if (!neighbourNode.isWalkable)
{
// Ops... you can´t walk here.
continue;
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNodeIndex = currentNodeIndex;
neighbourNode.gCost = tentativeGCost;
neighbourNode.CalculateFCost();
pathNodeArray[neighbourNodeIndex] = neighbourNode;
if (!openList.Contains(neighbourNode.index))
{
openList.Add(neighbourNode.index);
}
}
}
}
PathNode endNode = pathNodeArray[endNodeIndex];
if (endNode.cameFromNodeIndex == -1)
{
// NO WAY!! TO BAD...
}
else
{
// FOUND THE PERFECT PATCH!!! LET´S GO MF
NativeList <int2> path = CalculatePath(pathNodeArray, endNode);
path.Dispose();
}
// DISPOSE DE TRASH
// Skype this and wacth the show...
pathNodeArray.Dispose();
neighbourOffseArray.Dispose();
openList.Dispose();
closedList.Dispose();
}
// finds a path between two positions in the graph
public List <PathNode> FindPath(Vector2Int startPos, Vector2Int endPos)
{
// get the first Node (startPos) and add it to open list
PathNode startNode = GetNode(startPos.x, startPos.y);
PathNode endNode = GetNode(endPos.x, endPos.y);
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 = GetNode(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
// CYCLE:
while (openList.Count > 0)
{
PathNode currNode = GetLowestFCostNode(openList);
if (currNode == endNode)
{
// reached final node
return(CalculatePath(endNode));
}
openList.Remove(currNode);
closedList.Add(currNode);
foreach (PathNode neighborNode in GetNeighbors(currNode))
{
if (closedList.Contains(neighborNode))
{
continue;
}
// ignores nodes that are not walkable
if (!neighborNode.isWalkable)
{
closedList.Add(neighborNode);
continue;
}
int tentCost = currNode.gCost + CalculateDistanceCost(currNode, neighborNode);
if (tentCost < neighborNode.gCost)
{
neighborNode.cameFromNode = currNode;
neighborNode.gCost = tentCost;
neighborNode.hCost = CalculateDistanceCost(neighborNode, endNode);
neighborNode.CalculateFCost();
if (!openList.Contains(neighborNode))
{
openList.Add(neighborNode);
}
}
}
}
// Out of nodes in open list!
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = pathGrid.GetGridObject(startX, startY);
PathNode endNode = pathGrid.GetGridObject(endX, endY);
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < pathGrid.GetWidth(); x++)
{
for (int y = 0; y < pathGrid.GetHeight(); y++)
{
PathNode pathNode = pathGrid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCost(openList);
if (currentNode == endNode)
{
//End reached
return(CalculatePath(endNode));
}
openList.Remove(currentNode); //current node already searched
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
Debug.DrawLine(pathGrid.GetTilePosition(currentNode.x, currentNode.y) + 0.45f * Vector3.one, pathGrid.GetTilePosition(neighbourNode.x, neighbourNode.y) + 0.45f * Vector3.one, Color.yellow, 200f, false);
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode); // PROBLEM IF MULTIPLE SOLUTIONS???
}
}
}
}
// out of node in the open list
return(null);
}
public List <PathNode> FindNodePath(GridLocation startGridLocation, GridLocation endGridLocation)
{
PathNode startNode = GameManager.Instance.CurrentGameLevel.TilesByLocation[startGridLocation].PathNode;
PathNode endNode = GameManager.Instance.CurrentGameLevel.TilesByLocation[endGridLocation].PathNode;
if (startNode == null || endNode == null)
{
// Invalid Path
Logger.Log("path is invalid. Could not find a node.");
return(new List <PathNode>());
}
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
// go through all nodes. LATER: go through all nodes of a particular area, to filter out number of available nodes.
// Because it seems like currently a path from tile A in accessible zone to tile B in an accessible zone could be calculated outside of the area
for (int x = 0; x < AllPathNodes.Count; x++)
{
PathNode pathNode = AllPathNodes[x];
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 == endNode)
{
// Reached final node
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (KeyValuePair <ObjectDirection, PathNode> item in GetNeighbourList(currentNode))
{
PathNode neighbourNode = item.Value;
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.Tile.Walkable)
{
closedList.Add(neighbourNode);
continue;
}
// restrictions for Enemies
if (_character is EnemyCharacter)
{
if (neighbourNode.Tile.TryGetPlayerOnly())
{
closedList.Add(neighbourNode);
continue;
}
//restrictions for walking on bridges in the right direction
if (!ValidateForBridge(currentNode, neighbourNode, item.Key))
{
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();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
return(new List <PathNode>());
}
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 (startNode == null || endNode == null) {
// // Invalid Path
// return null;
// }
openList = new List <PathNode> {
startNode
};
closeList = new List <PathNode>();
for (var x = 0; x < grid.GetWidth(); x++)
{
for (var y = 0; y < grid.GetHeight(); y++)
{
PathNode pathNode = grid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
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 == endNode)
{
//reached final node
return(ClaculatePath(endNode));
}
openList.Remove(currentNode); // if current node isnt end node remove from list
closeList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closeList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closeList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost) // cycle through different option when ever neighbour node become comes current node due to its gcost being lower than the current node
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
return(null);
}
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);
}
//Execute
public void Execute()
{
for (int i = 0; i < pathNodeArray.Length; i++)
{
PathNode pathNode = pathNodeArray[i];
pathNode.hCost = CalculateDistanceCost(new int2(pathNode.x, pathNode.y), endPosition);
pathNode.cameFromNodeIndex = -1;
pathNodeArray[i] = pathNode;
}
//Neighbour positions
NativeArray <int2> neighbourOffsetArray = new NativeArray <int2>(8, Allocator.Temp);
neighbourOffsetArray[0] = new int2(-1, 0); //Left
neighbourOffsetArray[1] = new int2(+1, 0); //Right
neighbourOffsetArray[2] = new int2(0, +1); //Up
neighbourOffsetArray[3] = new int2(0, -1); //Down
neighbourOffsetArray[4] = new int2(-1, -1); //Left Down
neighbourOffsetArray[5] = new int2(-1, +1); //Left Up
neighbourOffsetArray[6] = new int2(+1, -1); //Right Down
neighbourOffsetArray[7] = new int2(+1, +1); //Right up
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestFCostNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currentNodeIndex];
//Has the destination been reached?
if (currentNodeIndex == endNodeIndex)
{
break;
}
//Remove node from open list
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currentNodeIndex);
//Cycle through neighbours
for (int i = 0; i < neighbourOffsetArray.Length; i++)
{
int2 neighbourOffset = neighbourOffsetArray[i];
int2 neighbourPos = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);
if (!IsPositionInsideGrid(neighbourPos, gridSize))
{
//Neighbour not in valid pos
continue;
}
int neighbourNodeIndex = CalculateIndex(neighbourPos.x, neighbourPos.y, gridSize.x);
if (closedList.Contains(neighbourNodeIndex))
{
//Already searched this node
continue;
}
PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
if (!neighbourNode.isWalkable)
{
//Not walkable
continue;
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPos);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNodeIndex = currentNodeIndex;
neighbourNode.gCost = tentativeGCost;
neighbourNode.CalculateFCost();
pathNodeArray[neighbourNodeIndex] = neighbourNode;
if (!openList.Contains(neighbourNode.index))
{
openList.Add(neighbourNode.index);
}
}
}
}
//Array cleanup
openList.Dispose();
closedList.Dispose();
neighbourOffsetArray.Dispose();
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetValue(startX, startY);
PathNode endNode = grid.GetValue(endX, endY);
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < grid.width; x++)
{
for (int y = 0; y < grid.height; y++)
{
PathNode pathNode = grid.GetValue(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.previousNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.previousNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
//Out of node in the openList
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = Grid.GetGridObjet(startX, startY);
PathNode endNode = Grid.GetGridObjet(endX, endY);
if (isInvalidePath(startNode, endNode))
{
// 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.GetGridObjet(x, y);
pathNode.GCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.CameFromeNode = null;
}
}
startNode.GCost = 0;
startNode.HCost = CalculateDistance(startNode, endNode);
startNode.CalculateFCost();
while (_openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(_openList);
if (currentNode == endNode)
{
// Reached finalnode
//PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList);
//PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(grid, CalculatePath(endNode));
return(CalculatePath(endNode));
}
_openList.Remove(currentNode);
_closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (_closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
_closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.GCost + CalculateDistance(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.GCost)
{
neighbourNode.CameFromeNode = currentNode;
neighbourNode.GCost = tentativeGCost;
neighbourNode.HCost = CalculateDistance(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!_openList.Contains(neighbourNode))
{
_openList.Add(neighbourNode);
}
}
}
}
return(null);
}
public int FindShortestDistance(Point startPos, Point endPos)
{
var distanceCounter = 0;
for (int i = 0; i < mapArray.GetLength(0); i++)
{
for (int j = 0; j < mapArray.GetLength(1); j++)
{
PathNode pathNode = new PathNode(j, i);
pathNode.GCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.CameFrom = null;
if (mapArray[i, j].MapObjectType == MapObjectType.Block)
{
pathNode.IsBlock = true;
}
mapArray[i, j].Node = pathNode;
}
}
PathNode startNode = GetNode(startPos.X, startPos.Y);
PathNode endNode = GetNode(endPos.X, endPos.Y);
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
startNode.GCost = 0;
startNode.HCost = CalculateDistance(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(currentNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (var neighbour in GetNeighbours(currentNode))
{
if (closedList.Contains(neighbour))
{
continue;
}
if (neighbour.IsBlock)
{
closedList.Add(neighbour);
continue;
}
int tempGCost = currentNode.GCost + CalculateDistance(currentNode, neighbour);
if (tempGCost < neighbour.GCost)
{
neighbour.CameFrom = currentNode;
neighbour.GCost = tempGCost;
neighbour.HCost = CalculateDistance(neighbour, endNode);
neighbour.CalculateFCost();
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
}
}
distanceCounter++;
}
return(0);
}
//Main function to calculate path
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
//Validating start position
if (startX < 0 || startX >= grid.GetWidth() || startY < 0 || startY >= grid.GetHeight())
{
return(null);
}
//Validating end position
if (endX < 0 || endX >= grid.GetWidth() || endY < 0 || endY >= grid.GetHeight())
{
return(null);
}
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
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 = int.MaxValue;
pathNode.CalculateFCost();
pathNode.previousNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
//Main algorithm
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
//Check every neighbour for path
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.CheckWalkable(walkableCheckRadius))
{
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.previousNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
//No path was found, return null
return(null);
}
public void Execute()
{
for (int i = 0; i < pathNodeArray.Length; i++)
{
PathNode pathNode = pathNodeArray[i];
pathNode.hCost = CalculateHCost(new int2(pathNode.x, pathNode.y), endPosition);
pathNode.cameFromNodeIndex = -1;
}
NativeArray <int2> neighborOffsetArray = new NativeArray <int2>(8, Allocator.Temp);
neighborOffsetArray[0] = new int2(-1, 0); // left
neighborOffsetArray[1] = new int2(1, 0); //right
neighborOffsetArray[2] = new int2(0, 1); // up
neighborOffsetArray[3] = new int2(0, -1); //down
neighborOffsetArray[4] = new int2(-1, 1); // left up
neighborOffsetArray[5] = new int2(-1, -1); // left down
neighborOffsetArray[6] = new int2(1, 1); // right up
neighborOffsetArray[7] = new int2(1, -1); // right down
NativeArray <bool> areStraightNeighborsWalkableArray = new NativeArray <bool>(4, Allocator.Temp);
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
//NativeList<int> closedList = new NativeList<int>(Allocator.Temp);
// 4 instances of closed list including this one ^
NativeArray <bool> checkedArray = new NativeArray <bool>(gridSize.x * gridSize.y, Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestFCostNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currentNodeIndex];
if (currentNode.index == endNodeIndex)
{
// YIPEE we reached our destination
break;
}
// remove current node from open list
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
//closedList.Add(currentNodeIndex);
checkedArray[currentNodeIndex] = true;
for (int i = 0; i < neighborOffsetArray.Length; i++)
{
int2 neighborOffset = neighborOffsetArray[i];
int2 neighborPosition = new int2(currentNode.x + neighborOffset.x, currentNode.y + neighborOffset.y);
// if neighbor is a diagonal, check to make sure appropriate straight cells are walkable
if (i >= 4)
{
if (neighborOffset.x == -1)
{
if (areStraightNeighborsWalkableArray[0] == false)
{
continue;
}
}
else
{
if (areStraightNeighborsWalkableArray[1] == false)
{
continue;
}
}
if (neighborOffset.y == -1)
{
if (areStraightNeighborsWalkableArray[3] == false)
{
continue;
}
}
else
{
if (areStraightNeighborsWalkableArray[2] == false)
{
continue;
}
}
}
if (IsPositionInsideGrid(neighborPosition, gridSize) == false)
{
// neighbor not valid position
continue;
}
int neighborNodeIndex = CalculateIndex(neighborPosition.x, neighborPosition.y, gridSize.x);
//if (closedList.Contains(neighborNodeIndex))
//{
// // already searched this node
// continue;
//}
if (checkedArray[neighborNodeIndex] == true)
{
// already searched this node
continue;
}
PathNode neighborNode = pathNodeArray[neighborNodeIndex];
if (neighborNode.isWalkable == false)
{
// not walkable
if (i < 4)
{
areStraightNeighborsWalkableArray[i] = false;
}
continue;
}
else
{
// cache the fact that this straight neighbor is walkable
if (i < 4)
{
areStraightNeighborsWalkableArray[i] = true;
}
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
// tentativeG is current g plus distance cost to get to neighbor node
int tentativeGCost = currentNode.gCost + CalculateHCost(currentNodePosition, neighborPosition);
if (tentativeGCost < neighborNode.gCost)
{
neighborNode.cameFromNodeIndex = currentNodeIndex;
neighborNode.gCost = tentativeGCost;
neighborNode.CalculateFCost();
pathNodeArray[neighborNodeIndex] = neighborNode;
if (openList.Contains(neighborNode.index) == false)
{
openList.Add(neighborNode.index);
}
}
}
}
pathPositionBufferFromEntity[entity].Clear();
PathNode endNode = pathNodeArray[endNodeIndex];
if (endNode.cameFromNodeIndex == -1)
{
// Did not find path
Debug.Log("Failed to find path");
pathFollowComponentDataFromEntity[entity] = new PathFollowComp {
pathIndex = -1
};
}
else
{
// Found path! Set pathIndex to buffer length - 1 because we read path from end to start
CalculatePathVoid(pathNodeArray, endNode, pathPositionBufferFromEntity[entity]);
pathFollowComponentDataFromEntity[entity] = new PathFollowComp {
pathIndex = pathPositionBufferFromEntity[entity].Length - 1
};
}
areStraightNeighborsWalkableArray.Dispose();
neighborOffsetArray.Dispose();
openList.Dispose();
//closedList.Dispose();
checkedArray.Dispose();
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
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 = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFrom = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode.x == endNode.x && currentNode.y == endNode.y)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbour in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbour))
{
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbour);
if (tentativeGCost < neighbour.gCost)
{
neighbour.cameFrom = currentNode;
neighbour.gCost = tentativeGCost;
neighbour.hCost = CalculateDistanceCost(neighbour, endNode);
neighbour.CalculateFCost();
}
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
}
}
//out of nodes
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY, bool reverse = true)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
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();
if (PathfindingDebugStepVisual.Instance != null)
{
PathfindingDebugStepVisual.Instance.ClearSnapshots();
PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, startNode, openList, closedList);
}
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
// Reached final node
if (PathfindingDebugStepVisual.Instance != null)
{
PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList);
PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(grid, CalculatePath(endNode, reverse));
}
return(CalculatePath(endNode, reverse));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
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();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
if (PathfindingDebugStepVisual.Instance != null)
{
PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList);
}
}
}
// Out of nodes on the openList
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
openList = new List <PathNode>()
{
startNode
};
closeList = new List <PathNode>();
// Initialize Grid
for (int x = 0; x < grid.Width; x++)
{
for (int y = 0; y < grid.Height; y++)
{
PathNode pathNode = grid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.PriorNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode.GetPosition(), endNode.GetPosition());
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closeList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closeList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closeList.Add(neighbourNode);
continue;
}
//neighbourNode.gCost = CalculateDistanceCost(currentNode.GetPosition(), neighbourNode.GetPosition());
int tentativeGCost = currentNode.gCost +
CalculateDistanceCost(currentNode.GetPosition(), neighbourNode.GetPosition());
if (neighbourNode.gCost > tentativeGCost)
{
neighbourNode.PriorNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode.GetPosition(), endNode.GetPosition());
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
return(null);
}
public void Execute()
{
PrepareNodes();
int startPositionIndex = CalculateIndex(StartPosition);
PathNode startPathNode = Grid[startPositionIndex];
int endPositionIndex = CalculateIndex(EndPosition);
startPathNode.GCost = 0;
startPathNode.CalculateFCost();
Grid[startPositionIndex] = startPathNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startPositionIndex);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestCostFNodeIndex(openList);
PathNode currentNode = Grid[currentNodeIndex];
if (currentNodeIndex == endPositionIndex)
{
break;
}
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currentNodeIndex);
for (int i = 0; i < NeighboursOffset.Length; i++)
{
int2 neighbourOffset = NeighboursOffset[i];
int2 neighbourPosition = new int2(currentNode.Position.x + neighbourOffset.x,
currentNode.Position.y + neighbourOffset.y);
if (!IsPositionInsideGrid(neighbourPosition))
{
continue;
}
int neighbourNodeIndex = CalculateIndex(neighbourPosition);
if (closedList.Contains(neighbourNodeIndex))
{
continue;
}
PathNode neighbourNode = Grid[neighbourNodeIndex];
if (!neighbourNode.IsWalkable)
{
continue;
}
int tentativeGCost = currentNode.GCost +
CalculateDistanceCost(currentNode.Position, neighbourPosition);
if (tentativeGCost < neighbourNode.GCost)
{
neighbourNode.CameFromNodeIndex = currentNodeIndex;
neighbourNode.GCost = tentativeGCost;
neighbourNode.CalculateFCost();
Grid[neighbourNodeIndex] = neighbourNode;
if (!openList.Contains(neighbourNode.Index))
{
openList.Add(neighbourNode.Index);
}
}
}
}
PathNode endNode = Grid[endPositionIndex];
if (endNode.CameFromNodeIndex == -1)
{
PathResult[0] = (int)PathQueryResult.Invalid;
}
else
{
ResultPath = CalculatePath(endNode);
PathResult[0] = (int)PathQueryResult.Complete;
}
openList.Dispose();
closedList.Dispose();
}
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);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
//Debug.Log("FindPath: start" + startX + "," + startY + " end" + endX + "," + endY);
PathNode startNode = GetNode(startX, startY);
PathNode endNode = GetNode(endX, endY);
if (startNode == null || endNode == null)
{
// Invalid Path
Debug.LogWarning("Invalid Path!");
return(null);
}
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
// Search
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if ((currentNode.Vector2Int() - endNode.Vector2Int()).magnitude < 15)
{
// Reached final node
return(CalculatePath(currentNode)); //not endnode because not exact hit
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
/*if (!neighbourNode.isWalkable) {
* closedList.Add(neighbourNode);
* continue;
* }*/
float tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode) + InclineCost(currentNode, neighbourNode) + WaterCost(neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
Debug.Log("Failed to find path to " + endNode);
return(null);
}
// Calculate and return path from startNode to endNode
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = nodeGrid.GetGridObject(startX, startY);
PathNode endNode = nodeGrid.GetGridObject(endX, endY);
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
// Set all node to make gCost
for (int x = 0; x < nodeGrid.Width; x++)
{
for (int y = 0; y < nodeGrid.Height; y++)
{
PathNode pathNode = nodeGrid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.prevNode = null;
}
}
// Calculate cost for each nodes
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
// Search loop
while (openList.Count > 0)
{
PathNode currentNode = getLowestCostNode(openList);
// Reached final node
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
// Assigned current node to "searched"
// Remove current node from openList and add to closedList
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
// Continue if node is in closed list or not walkable
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closedList.Add(neighbourNode);
continue;
}
// Continue if node adjacent to current and neighbor node is not walkable
if (!AvoidWall(currentNode, neighbourNode))
{
continue;
}
// Calculate cost of current neighbour node
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.prevNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
// Add to open list if not on open list
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes, cannot find path
return(null);
}
private void FindPath(int2 startPos, int2 endPos)
{
var gridSize = new int2(20, 20);
var pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);
for (int i = 0; i < gridSize.x; i++)
{
for (int j = 0; j < gridSize.y; j++)
{
var pathNode = new PathNode
{
x = i,
y = j,
index = CalculateIndex(i, j, gridSize.x),
gCost = int.MaxValue,
hCost = CalculateDistanceCost(new int2(i, j), endPos),
isWalkable = true,
cameFromNodeIndex = -1,
};
pathNode.CalculateFCost();
pathNodeArray[pathNode.index] = pathNode;
}
}
var neighbourOffsetArray = new NativeArray <int2>(new int2[]
{
new int2(-1, 0),
new int2(1, 0),
new int2(0, 1),
new int2(0, -1),
new int2(-1, -1),
new int2(-1, 1),
new int2(1, -1),
new int2(1, 1),
}, Allocator.Temp);
var endPosIndex = CalculateIndex(endPos.x, endPos.y, gridSize.x);
var startNode = pathNodeArray[CalculateIndex(startPos.x, startPos.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
var openList = new NativeList <int>(Allocator.Temp);
var closeList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
var currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
var currentNode = pathNodeArray[currentNodeIndex];
if (currentNodeIndex == endPosIndex)
{
// reach end point
break;
}
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closeList.Add(currentNodeIndex);
for (int i = 0; i < neighbourOffsetArray.Length; i++)
{
var neighbourOffset = neighbourOffsetArray[i];
var neighbourNodeIndex = CalculateIndex(currentNode.x + neighbourOffset.x,
currentNode.y + neighbourOffset.y,
gridSize.x);
if (neighbourNodeIndex >= pathNodeArray.Length || neighbourNodeIndex < 0)
{
continue;
}
if (closeList.Contains(neighbourNodeIndex))
{
continue;
}
var neighbourNode =
pathNodeArray[
CalculateIndex(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y,
gridSize.x)];
if (!neighbourNode.isWalkable)
{
closeList.Add(neighbourNodeIndex);
continue;
}
var currentNodePos = new int2(currentNode.x, currentNode.y);
var neighbourNodePos = new int2(neighbourNode.x, neighbourNode.y);
var tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePos, neighbourNodePos);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNodeIndex = currentNodeIndex;
neighbourNode.gCost = tentativeGCost;
neighbourNode.CalculateFCost();
pathNodeArray[neighbourNodeIndex] = neighbourNode;
if (!openList.Contains(neighbourNode.index))
{
openList.Add(neighbourNode.index);
}
}
}
}
var endNode = pathNodeArray[endPosIndex];
if (endNode.cameFromNodeIndex == -1)
{
//didn't find the path
Debug.Log("Did't find a path");
}
else
{
//find a path
var path = CalculatePath(pathNodeArray, endNode);
foreach (var p in path)
{
Debug.Log(p);
}
path.Dispose();
}
pathNodeArray.Dispose();
openList.Dispose();
closeList.Dispose();
neighbourOffsetArray.Dispose();
}
private List <PathNode> FindPath(int startX, int startY, int endX, int endY, int xCount, int yCount)
{
allPathNodes.Clear();
for (int i = 0; i < xCount; i++)
{
for (int j = 0; j < yCount; j++)
{
allPathNodes.Add(new PathNode(i, j, RefHolder.instance.worldGen.GetGridGroundObject(i, j).GetComponent <GroundScript>().isEmpty));
}
}
PathNode startNode = GetNode(startX, startY);
PathNode endNode = GetNode(endX, endY);
if (startNode == null || endNode == null)
{
// Invalid Path
return(null);
}
List <PathNode> openList = new List <PathNode>();
List <PathNode> closedList = new List <PathNode>();
openList.Add(startNode);
for (int x = 0; x < xCount; x++)
{
for (int y = 0; y < yCount; y++)
{
PathNode pathNode = GetNode(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 == endNode)
{
// Reached final node
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
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();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
return(null);
}
private List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
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 = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDis(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowfCostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closedList.Add(neighbourNode);
continue;
}
int tempGCost = currentNode.gCost + CalculateDis(currentNode, neighbourNode);
if (tempGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tempGCost;
neighbourNode.hCost = CalculateDis(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
return(null);
}
public List <PathNode> FindPath(int startX, int startZ, int endX, int endZ)
{
PathNode startNode = GetNode(startX, startZ);
PathNode endNode = GetNode(endX, endZ);
openList = new List <PathNode>()
{
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < grid.GetWidth(); x++)
{
for (int z = 0; z < grid.GetHeight(); z++)
{
PathNode pathNode = GetNode(x, z);
pathNode.gCost = int.MaxValue;
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 == endNode)
{
// Reached final node
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
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();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Could not find the path
return(null);
}
//public BufferFromEntity<PathPosition> pathPositionBuffer;
public void Execute()
{
for (int i = 0; i < pathNodeArray.Length; i++)
{
PathNode pathNode = pathNodeArray[i];
pathNode.hCost = CalculateDistanceCost(new int2(pathNode.x, pathNode.y), endPosition);
pathNode.cameFromNodeIndex = -1;
pathNodeArray[i] = pathNode;
}
NativeArray <int2> neighbourOffsetArray = new NativeArray <int2>(4, Allocator.Temp);
neighbourOffsetArray[0] = new int2(-1, 0); // Left
neighbourOffsetArray[1] = new int2(+1, 0); // Right
neighbourOffsetArray[2] = new int2(0, +1); // Up
neighbourOffsetArray[3] = new int2(0, -1); // Down
//neighbourOffsetArray[4] = new int2(-1, -1); // Left Down
// neighbourOffsetArray[5] = new int2(-1, +1); // Left Up
//neighbourOffsetArray[6] = new int2(+1, -1); // Right Down
//neighbourOffsetArray[7] = new int2(+1, +1); // Right Up
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currentNodeIndex];
if (currentNodeIndex == endNodeIndex)
{
// Reached our destination!
break;
}
// Remove current node from Open List
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currentNodeIndex);
for (int i = 0; i < neighbourOffsetArray.Length; i++)
{
int2 neighbourOffset = neighbourOffsetArray[i];
int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);
if (!IsPositionInsideGrid(neighbourPosition, gridSize))
{
// Neighbour not valid position
continue;
}
int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x);
if (closedList.Contains(neighbourNodeIndex))
{
// Already searched this node
continue;
}
PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
if (!neighbourNode.isWalkable)
{
// Not walkable
continue;
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNodeIndex = currentNodeIndex;
neighbourNode.gCost = tentativeGCost;
neighbourNode.CalculateFCost();
pathNodeArray[neighbourNodeIndex] = neighbourNode;
if (!openList.Contains(neighbourNode.index))
{
openList.Add(neighbourNode.index);
}
}
}
}
//pathPositionBuffer.Clear();
/*
* PathNode endNode = pathNodeArray[endNodeIndex];
* if (endNode.cameFromNodeIndex == -1) {
* // Didn't find a path!
* //Debug.Log("Didn't find a path!");
* pathFollowComponentDataFromEntity[entity] = new PathFollow { pathIndex = -1 };
* } else {
* // Found a path
* //CalculatePath(pathNodeArray, endNode, pathPositionBuffer);
* //pathFollowComponentDataFromEntity[entity] = new PathFollow { pathIndex = pathPositionBuffer.Length - 1 };
* }
*/
neighbourOffsetArray.Dispose();
openList.Dispose();
closedList.Dispose();
}
public List <PathNode> FindPathNodes(Vector2Int startPos, Vector2Int endPos, Cell[,] grid)
{
myGrid = new Grid <PathNode>(grid.GetLength(0), grid.GetLength(1), 10f, Vector3.zero, (Grid <PathNode> g, int x, int y) => new PathNode(g, x, y));
PathNode startNode = myGrid.GetGridObject(startPos.x, startPos.y);
PathNode endNode = myGrid.GetGridObject(endPos.x, endPos.y);
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < myGrid.GetWidth(); x++)
{
for (int y = 0; y < myGrid.GetHeight(); y++)
{
PathNode pathNode = myGrid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
//Heuristic
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
//Reached final node
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neibhourNode in GetNeighbourList(currentNode, grid))
{
if (closedList.Contains(neibhourNode))
{
continue;
}
if (!neibhourNode.isWalkable)
{
closedList.Add(neibhourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neibhourNode);
if (tentativeGCost < neibhourNode.gCost)
{
neibhourNode.cameFromNode = currentNode;
neibhourNode.gCost = tentativeGCost;
neibhourNode.hCost = CalculateDistanceCost(neibhourNode, endNode);
neibhourNode.CalculateFCost();
if (!openList.Contains(neibhourNode))
{
openList.Add(neibhourNode);
}
}
}
}
//Out of nodes on the openlist
return(null);
}
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 (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 = int.MaxValue;
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 == endNode)
{
// Reached final node
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighborNode in GetNeighborList(currentNode))
{
if (closedList.Contains(neighborNode))
{
continue;
}
if (!neighborNode.isWalkable)
{
closedList.Add(neighborNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighborNode);
if (tentativeGCost < neighborNode.gCost)
{
neighborNode.cameFromNode = currentNode;
neighborNode.gCost = tentativeGCost;
neighborNode.hCost = CalculateDistanceCost(neighborNode, endNode);
neighborNode.CalculateFCost();
if (!openList.Contains(neighborNode))
{
openList.Add(neighborNode);
}
}
}
}
// Out of nodes on the openList
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY, bool safePathOnly)
{
if (!GridCoords.IsInGrid(startX, startY))
{
return(null);
}
if (!GridCoords.IsInGrid(endX, endY))
{
return(null);
}
PathNode startNode = grid[startX, startY].PathNode;
PathNode endNode = grid[endX, endY].PathNode;
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < currentGridInfo.gameGridSize.x; x++)
{
for (int y = 0; y < currentGridInfo.gameGridSize.y; y++)
{
PathNode pathNode = grid[x, y].PathNode;
pathNode.gCost = int.MaxValue;
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 == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetValidNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isTraversable)
{
closedList.Add(neighbourNode);
continue;
}
else if (!neighbourNode.isSafe && safePathOnly)
{
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();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
return(null);
}
public List <PathNode> FindPathOnLoop(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
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();
//PathfindingDebugStepVisual.Instance.ClearSnapshots();
//PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, startNode, openList, closedList);
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)
{
// Reached final node
//PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList);
//PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(grid, CalculatePath(endNode));
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
Grid <WorldTileObject> worldTileGrid = DelegateController.getWorldTileGrid.Invoke();
PlacedObject_WorldTile currentTileObject = worldTileGrid.GetGridObject(neighbourNode.x, neighbourNode.y).GetPlacedObject();
if (!currentTileObject.MyAspects.Contains(LoopPathAspect))
{
//Debug.Log("Closed: " + neighbourNode.x + ", " + neighbourNode.y);
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();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
//PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList);
}
}
// Out of nodes on the openList
return(null);
}
public void Execute()
{
int2 gridSize = new int2(20, 20);
NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; y < gridSize.y; y++)
{
PathNode pathNode = new PathNode();
pathNode.x = x;
pathNode.y = y;
pathNode.index = CalculateIndex(x, y, gridSize.x);
pathNode.gCost = int.MaxValue;
pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition);
pathNode.CalculateFCost();
pathNode.isWalkable = true;
pathNode.cameFronNodeIndex = -1;
pathNodeArray[pathNode.index] = pathNode;
}
}
// place walls
// {
// PathNode walkablePathNode = pathNodeArray[CalculateIndex(1, 0, gridSize.x)];
// walkablePathNode.SetIsWalkable(false);
// pathNodeArray[CalculateIndex(1, 0, gridSize.x)] = walkablePathNode;
// walkablePathNode = pathNodeArray[CalculateIndex(1, 1, gridSize.x)];
// walkablePathNode.SetIsWalkable(false);
// pathNodeArray[CalculateIndex(1, 1, gridSize.x)] = walkablePathNode;
// walkablePathNode = pathNodeArray[CalculateIndex(1, 2, gridSize.x)];
// walkablePathNode.SetIsWalkable(false);
// pathNodeArray[CalculateIndex(1, 2, gridSize.x)] = walkablePathNode;
// }
NativeArray <int2> neighbourOffsetsArray = new NativeArray <int2>(8, Allocator.Temp);
neighbourOffsetsArray[0] = new int2(-1, 0); // left
neighbourOffsetsArray[1] = new int2(+1, 0); // right
neighbourOffsetsArray[2] = new int2(0, +1); // up
neighbourOffsetsArray[3] = new int2(0, -1); // down
neighbourOffsetsArray[4] = new int2(-1, -1); // left down
neighbourOffsetsArray[5] = new int2(-1, +1); // left up
neighbourOffsetsArray[6] = new int2(+1, -1); // right down
neighbourOffsetsArray[7] = new int2(+1, +1); // right up
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startingPosition.x, startingPosition.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currrentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currrentNodeIndex];
if (currrentNodeIndex == endNodeIndex)
{
// reached dest
break;
}
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currrentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currrentNodeIndex);
for (int i = 0; i < neighbourOffsetsArray.Length; i++)
{
int2 neighbourOffset = neighbourOffsetsArray[i];
int2 neightbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);
if (!IsPositionInsideGrid(neightbourPosition, gridSize))
{
// Neighbour not valid position
continue;
}
int neightbourNodeIndex = CalculateIndex(neightbourPosition.x, neightbourPosition.y, gridSize.x);
if (closedList.Contains(neightbourNodeIndex))
{
// Allready searched this one
continue;
}
PathNode neightbourNode = pathNodeArray[neightbourNodeIndex];
if (!neightbourNode.isWalkable)
{
// Not walkable
continue;
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neightbourPosition);
if (tentativeGCost < neightbourNode.gCost)
{
neightbourNode.cameFronNodeIndex = currrentNodeIndex;
neightbourNode.gCost = tentativeGCost;
neightbourNode.CalculateFCost();
pathNodeArray[neightbourNodeIndex] = neightbourNode;
if (!openList.Contains(neightbourNode.index))
{
openList.Add(neightbourNode.index);
}
}
}
}
PathNode endNode = pathNodeArray[endNodeIndex];
if (endNode.cameFronNodeIndex != -1)
{
// Found a path
NativeList <int2> path = CalculatePath(pathNodeArray, endNode);
// foreach (int2 pathPosition in path) {
// Debug.Log(pathPosition);
// }
path.Dispose();
}
pathNodeArray.Dispose();
neighbourOffsetsArray.Dispose();
openList.Dispose();
closedList.Dispose();
}
