private void DetermineNodeValues(PathNode currentNode, PathNode testing, PathNode targetNode)
{
// Dont work on null nodes
if (testing == null)
{
return;
}
// Check to see if the node is the target
if (testing == targetNode)
{
targetNode.parent = currentNode;
foundTarget = true;
return;
}
// Ignore Walls
if (currentNode.weight == WALL_WEIGHT || testing.weight == WALL_WEIGHT)
{
return;
}
// While the node has not already been tested
if (!testing.isOnClosedList)
{
// Check to see if the node is already on the open list
if (testing.isOnOpenList)
{
// Get a Gcost to move from this node to the testing node
var newGcost = currentNode.gValue + currentNode.weight + movementCost;
// If the G cost is better then change the nodes parent and update its costs.
if (newGcost < testing.gValue)
{
testing.parent = currentNode;
testing.gValue = newGcost;
Debug.Log(testing);
binaryTree.RemoveNode(testing);
testing.CalculateFValue();
binaryTree.AddNode(testing);
}
}
else
{
// Set the testing nodes parent to the current location, calculate its costs, and add it to the open list
testing.parent = currentNode;
testing.gValue = currentNode.gValue + currentNode.weight + movementCost;
testing.CalculateFValue();
AddToOpenList(testing);
}
}
}
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);
}