private int CalculateHCost(PathNode node, PathNode endNode)
{
int hgt = Mathf.Abs(endNode.Cords().y - node.Cords().y);
int wdt = Mathf.Abs(endNode.Cords().x - node.Cords().x);
int ret = Mathf.RoundToInt(Mathf.Sqrt(Mathf.Pow(wdt, 2) + Mathf.Pow(hgt, 2))) * 10;
return(ret);
}
private List <PathNode> GetClose(PathNode node)
{
int x = node.Cords().x;
int y = node.Cords().y;
List <PathNode> ret = new List <PathNode>();
if (x + 1 < width)
{
ret.Add(pathMap[x + 1, y]);
}
if (x - 1 >= 0)
{
ret.Add(pathMap[x - 1, y]);
}
if (y + 1 < height)
{
ret.Add(pathMap[x, y + 1]);
}
if (y - 1 >= 0)
{
ret.Add(pathMap[x, y - 1]);
}
if (x + 1 < width && y + 1 < height)
{
ret.Add(pathMap[x + 1, y + 1]);
}
if (x - 1 >= 0 && y + 1 < height)
{
ret.Add(pathMap[x - 1, y + 1]);
}
if (y + 1 < height && x - 1 >= 0)
{
ret.Add(pathMap[x - 1, y + 1]);
}
if (y - 1 >= 0 && x + 1 < width)
{
ret.Add(pathMap[x + 1, y - 1]);
}
return(ret);
}
public List <Vector3Int> PathFind(Vector3 from, Vector3 to, WalkSpeeds walkSpeed)
{
Debug.Log("Pathfinding from: " + from + " to: " + to);
Vector2Int fromPix = GetMapPos(from);
Vector2Int toPix = GetMapPos(to);
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
pathMap[x, y] = new PathNode(x, y);
}
}
PathNode startNode = pathMap[fromPix.x, fromPix.y];
PathNode endNode = pathMap[toPix.x, toPix.y];
startNode.gCost = 0;
startNode.hCost = CalculateHCost(startNode, endNode);
startNode.CalculateFValue();
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
while (true)
{
PathNode currentNode = openList[0];
foreach (PathNode node in openList)
{
if (node.fCost < currentNode.fCost)
{
currentNode = node;
}
}
closedList.Add(currentNode);
openList.Remove(currentNode);
if (currentNode == endNode)
{
break;
}
List <PathNode> closeNeighbor = GetClose(currentNode);
foreach (PathNode ngb in closeNeighbor)
{
if (!closedList.Contains(ngb))
{
int dist;
if (currentNode.Cords().x != ngb.Cords().x&& currentNode.Cords().y != ngb.Cords().y)
{
dist = 14;
}
else
{
dist = 10;
}
int newG = currentNode.gCost + walkSpeed.GetSpeed((Color)(map.texture.GetPixel(ngb.Cords().x, ngb.Cords().y)), dist);
if (openList.Contains(ngb))
{
if (newG < ngb.gCost)
{
ngb.gCost = newG;
ngb.CalculateFValue();
ngb.cameFromNode = currentNode;
}
}
else
{
ngb.gCost = newG;
ngb.hCost = CalculateHCost(ngb, endNode);
ngb.CalculateFValue();
ngb.cameFromNode = currentNode;
openList.Add(ngb);
}
}
}
}
PathNode findNode = endNode;
List <Vector3Int> track = new List <Vector3Int>();
while (true)
{
int x = findNode.Cords().x;
int y = findNode.Cords().y;
track.Add(new Vector3Int(x, y, walkSpeed.GetSpeed(map.texture.GetPixel(x, y), 10)));
if (findNode.cameFromNode == null)
{
break;
}
findNode = findNode.cameFromNode;
}
track.Reverse();
Debug.Log("Pathfinding finnished");
return(track);
}