/***************************************************************************/
public List <NodePathfinding> GetNeighbours(NodePathfinding node, bool eightConnectivity)
{
List <NodePathfinding> neighbours = new List <NodePathfinding>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if ((x == 0 && y == 0))
{
continue;
}
if (!eightConnectivity && (Mathf.Abs(x) + Mathf.Abs(y) > 1))
{
continue;
}
int checkX = node.mGridX + x;
int checkY = node.mGridY + y;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbours.Add(grid[checkX, checkY]);
}
}
}
return(neighbours);
}
/***************************************************************************/
void RetracePath(NodePathfinding startNode, NodePathfinding endNode)
{
List <NodePathfinding> path = new List <NodePathfinding>();
bool end = false;
NodePathfinding newNode = endNode;
while (!end)
{
path.Add(newNode);
newNode = newNode.mParent;
if (newNode == startNode)
{
path.Add(newNode);
end = true;
}
}
path.Reverse();
#if Smooth
SmoothPath(path);
#else
Grid.path = path;
#endif
}
/***************************************************************************/
float Heuristic(NodePathfinding nodeA, NodePathfinding nodeB)
{
// Heuristic function
//nodeA.mGridX nodeB.mGridX
//nodeA.mGridY nodeB.mGridY
return(Vector2.Distance(new Vector2(nodeB.mGridX, nodeB.mGridY), new Vector2(nodeA.mGridX, nodeA.mGridY)) * hMultiplier);
}
public NodePathfinding(int gridX, int gridY, float gCost = 0f, float hCost = 0f, NodePathfinding parent = null)
{
this.GCost = gCost;
this.HCost = hCost;
this.Parent = parent;
this.GridX = gridX;
this.GridY = gridY;
FCost = gCost + hCost;
}
private void CountTiles()
{
//walkableTiles = new List<Vector2Int>();
//worldWalkableTiles = new List<Vector3>();
//walkableNodes = new List<NodePathfinding>();
BoundsInt bounds = tilemapBase.cellBounds;
tilemapGrid = tilemapBase.layoutGrid;
//tilemapGrid.
TileBase[] allTiles = tilemapBase.GetTilesBlock(bounds);
walkableNodesArray = new NodePathfinding[bounds.size.x, bounds.size.y];
worldWalkableTilesArray = new Vector3[bounds.size.x, bounds.size.y];
tilesCount = 0;
for (int i = 0; i < bounds.size.x; i++)
{
for (int j = 0; j < bounds.size.y; j++)
{
/*
* if(tilemapBase.HasTile(new Vector3Int(i, j, 0)))
* {
* tilesCount++;
* tilemapBase.SetColor(new Vector3Int(i, j, 0), Color.green);
* }
*/
TileBase tempTile = allTiles[i + j * bounds.size.x];
if (tempTile != null)
{
tilesCount++;
//walkableTiles.Add(new Vector2Int(i, j));
//Debug.Log("x: " + i + " y: " + j + " tile: " + tempTile.name);
walkableNodesArray[i, j] = new NodePathfinding(i, j);
worldWalkableTilesArray[i, j] = new Vector3(i + tilemapBase.origin.x, j + tilemapBase.origin.y);
//Vector3Int localPlace = (new Vector3Int(i, j, (int)tilemapBase.transform.position.y));
//Vector3 place = tilemapBase.CellToWorld(localPlace);
//Debug.Log("place: ")
}
}
}
//Debug.Log("Tiles count: " + tilesCount);
/*
* foreach (Vector2Int tile in walkableTiles)
* {
* Vector3 tileWorldPos = new Vector3(tile.x + tilemapBase.origin.x, tile.y + tilemapBase.origin.y, 0);
* //Instantiate(prefab, tileWorldPos, prefab.transform.rotation);
* worldWalkableTiles.Add(tileWorldPos);
* walkableNodes.Add(new NodePathfinding(tile.x, tile.y));
* }
*/
}
private NodePathfinding GetNodeWithLowestF(List <NodePathfinding> nodeList)
{
NodePathfinding lowest = nodeList[0];
foreach (NodePathfinding node in nodeList)
{
if (node.FCost < lowest.FCost)
{
lowest = node;
}
}
return(lowest);
}
public bool MyEquals(NodePathfinding other)
{
if (other != null)
{
if (this.GridX == other.GridX)
{
if (this.GridY == other.GridY)
{
return(true);
}
}
}
return(false);
}
List <NodePathfinding> GetNeighbours(NodePathfinding curr)
{
var list = new List <NodePathfinding>();
for (int i = 0; i < curr.neightbourds.Count; i++)
{
if (curr.neightbourds[i].isTrap)
{
continue;
}
list.Add(curr.neightbourds[i]);
}
return(list);
}
private bool NodeExistsInList(List <NodePathfinding> nodeList, NodePathfinding node)
{
/*
* foreach(NodePathfinding n in nodeList)
* {
* if(node.GridX == n.GridX)
* {
* if(node.GridY == n.GridY)
* {
* return true;
* }
* }
* }
* return false;
*/
return(nodeList.Contains(node));
}
/***************************************************************************/
float GetDistance(NodePathfinding nodeA, NodePathfinding nodeB)
{
// Distance function
//nodeA.mGridX nodeB.mGridX
//nodeA.mGridY nodeB.mGridY
if (EightConnectivity)
{
int vertDistance = Mathf.Abs(nodeA.mGridY - nodeB.mGridY);
int horzDistance = Mathf.Abs(nodeA.mGridX - nodeB.mGridX);
float distance = Mathf.Sqrt(Mathf.Pow(vertDistance, 2) + Mathf.Pow(horzDistance, 2));
return(distance);
}
else
{
return((Mathf.Abs(nodeA.mGridX - nodeB.mGridX)) + (Mathf.Abs(nodeA.mGridY - nodeB.mGridY)));
}
}
/***************************************************************************/
/***************************************************************************/
void SmoothPath(List <NodePathfinding> path)
{
//TODO
//Comparamos si el nodo final es el mismo que el inicial
//Cuando bresenham sea true, el nodo inicial se cambia al nodo que ha salido true y se vuelve a recalcular
//Se añade ese nodo para guardarlo y se vuelve a iterar por el resto de nodos hasta que vuelva a encontrar el nodo inicial
List <NodePathfinding> targetNodes = new List <NodePathfinding>();
NodePathfinding newOriginNode = path[0];
NodePathfinding endNode = path[path.Count - 1];
List <NodePathfinding> auxNodeList = new List <NodePathfinding>();
List <NodePathfinding> newNodeList = new List <NodePathfinding>();
newNodeList = path;
newNodeList.Reverse();
int a = 0;
for (a = 0; a < 10000; a++)
{
foreach (NodePathfinding n in newNodeList)
{
if (BresenhamWalkable(newOriginNode.mGridX, newOriginNode.mGridY, n.mGridX, n.mGridY))
{
targetNodes.Add(n);
newOriginNode = n;
newNodeList = new List <NodePathfinding>(auxNodeList);
auxNodeList.Clear();
break;
}
auxNodeList.Add(n);
}
if (endNode == newOriginNode)
{
a = 999999;
}
}
targetNodes.Add(path[0]);
Grid.path = targetNodes;
}
/***************************************************************************/
void CreateGrid()
{
grid = new NodePathfinding[gridSizeX, gridSizeY];
Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2;
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius);
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
float costMultiplier =
(Physics.CheckSphere(worldPoint, nodeRadius, costMultiplierMask)) ?
1.5f : 1.0f;
grid[x, y] = new NodePathfinding(walkable, worldPoint, x, y, costMultiplier);
}
}
}
public bool DiagonalNeighbour(NodePathfinding neighbour)
{
if ((neighbour.GridX == this.GridX + 1) && (neighbour.GridY == this.GridY + 1))
{
return(true);
}
else if ((neighbour.GridX == this.GridX + 1) && (neighbour.GridY == this.GridY - 1))
{
return(true);
}
else if ((neighbour.GridX == this.GridX - 1) && (neighbour.GridY == this.GridY - 1))
{
return(true);
}
else if ((neighbour.GridX == this.GridX - 1) && (neighbour.GridY == this.GridY + 1))
{
return(true);
}
else
{
return(false);
}
}
/***************************************************************************/
public List <NodePathfinding> FindPath(Vector3 startPos, Vector3 targetPos, int iterations)
{
CurrentStartNode = Grid.NodeFromWorldPoint(startPos);
CurrentTargetNode = Grid.NodeFromWorldPoint(targetPos);
List <NodePathfinding> openSet = new List <NodePathfinding>();
HashSet <NodePathfinding> closedSet = new HashSet <NodePathfinding>();
openSet.Add(CurrentStartNode);
Grid.openSet = openSet;
int currentIteration = 0;
NodePathfinding node = CurrentStartNode;
while (openSet.Count > 0 && node != CurrentTargetNode && (iterations == -1 || currentIteration < iterations))
{
// Select best node from open list
node = openSet[0];
foreach (NodePathfinding nod in openSet)
{
if (nod.fCost < node.fCost)
{
node = nod;
}
}
// Manage open/closed list
openSet.Remove(node);
closedSet.Add(node);
Grid.openSet = openSet;
Grid.closedSet = closedSet;
// Check destination
if (node != CurrentTargetNode)
{
// Open neighbours
foreach (NodePathfinding neighbour in Grid.GetNeighbours(node, EightConnectivity))
{
if (!neighbour.mWalkable || closedSet.Contains(neighbour))
{
continue;
}
//Cost of reaching my neighbor = cost of reaching ME +
float neighbourGcost = (node.gCost + (GetDistance(node, neighbour) * node.mCostMultiplier));
if (neighbourGcost < node.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = neighbourGcost;
neighbour.hCost = Heuristic(neighbour, CurrentTargetNode);
neighbour.mParent = node;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
currentIteration++;
}
else
{
//Path found!
//Comment / Uncomment the define Smooth to deactive / active the smoothing with Bresenham.
RetracePath(CurrentStartNode, CurrentTargetNode);
// Path found
totalNodes = openSet.Count + closedSet.Count;
openNodes = openSet.Count;
closedNodes = closedSet.Count;
Debug.Log("Statistics:");
Debug.LogFormat("Total nodes: {0}", totalNodes);
Debug.LogFormat("Open nodes: {0}", openNodes);
Debug.LogFormat("Closed nodes: {0}", closedNodes);
}
}
return(Grid.path);
}
//A* Pathfinding Algorithm Implementation
public List <Vector3> ExecutePathfinding(Vector3 startingPosition)
{
CountTiles();
Vector3 playerPosition = PlayerCharacter.instance.transform.position;
List <NodePathfinding> open = new List <NodePathfinding>();
List <NodePathfinding> closed = new List <NodePathfinding>();
Vector2Int startingTile = GetTileFromWorldPosition(startingPosition);
Vector3 centerOfStartTile = GetWorldPositionOfCenterOfTile(startingTile);
//NodePathfinding startNode = new NodePathfinding(startingTile.x, startingTile.y, 0, Vector3.Distance(startingPosition, playerPosition));
//NodePathfinding startNode = GetNode(startingTile.x, startingTile.y);
NodePathfinding startNode = walkableNodesArray[startingTile.x, startingTile.y];
startNode.HCost = Vector3.Distance(startingPosition, playerPosition);
open.Add(startNode);
Vector2Int targetTile = GetTileFromWorldPosition(playerPosition);
//NodePathfinding targetNode = new NodePathfinding(targetTile.x, targetTile.y);
NodePathfinding targetNode = walkableNodesArray[targetTile.x, targetTile.y];
NodePathfinding currentNode = null;
for (int i = 0; i < tilesCount; i++)
{
currentNode = GetNodeWithLowestF(open);
open.Remove(currentNode);
closed.Add(currentNode);
if (currentNode.MyEquals(targetNode))
{
//break
i = tilesCount;
}
foreach (Vector2Int neighbourTile in currentNode.Neighbours())
{
NodePathfinding neighbourNode;
if ((neighbourTile.x < walkableNodesArray.GetLength(0)) &&
(neighbourTile.x >= 0) &&
(neighbourTile.y < walkableNodesArray.GetLength(1)) &&
(neighbourTile.y >= 0))
{
neighbourNode = walkableNodesArray[neighbourTile.x, neighbourTile.y];
}
else
{
neighbourNode = null;
}
if (ExistsInWalkableTiles(neighbourTile) && (!NodeExistsInList(closed, neighbourNode)))
{
float traverseDistance = GetTraverseNeighbourDistance(currentNode, neighbourNode);
float newPath = currentNode.GCost + traverseDistance;
if ((newPath < neighbourNode.GCost) || (!NodeExistsInList(open, neighbourNode)))
{
neighbourNode.GCost = newPath;
Vector3 nodeWorldCenter = GetWorldPositionOfCenterOfTile(new Vector2Int(neighbourNode.GridX, neighbourNode.GridY));
neighbourNode.HCost = Vector3.Distance(nodeWorldCenter, playerPosition);
neighbourNode.FCost = neighbourNode.GCost + neighbourNode.HCost;
neighbourNode.Parent = currentNode;
if (!NodeExistsInList(open, neighbourNode))
{
open.Add(neighbourNode);
}
}
}
}
}
List <Vector3> results = new List <Vector3>();
NodePathfinding currentResultNode = currentNode;
while (currentResultNode.Parent != null)
{
results.Add(GetWorldPositionOfCenterOfTile(new Vector2Int(currentResultNode.GridX, currentResultNode.GridY)));
currentResultNode = currentResultNode.Parent;
}
results.Reverse();
return(results);
}
private float GetTraverseNeighbourDistance(NodePathfinding nb1, NodePathfinding nb2)
{
return(nb1.DiagonalNeighbour(nb2) ? tileDiag : tileSize);
}
bool Satisfies(NodePathfinding curr)
{
return(curr == finit);
}
void Start()
{
pathfinding = transform.GetComponent <NodePathfinding>();
Target = Player.Instance.transform;
actor = transform.GetComponent <NPC>();
}
