public NodeCost GetNodeCost(Node start, Node end)
{
NodeCost cost = new NodeCost();
cost.gCost = start.gCost + 10;
cost.hCost = grid.MD(end, Cost_Target);
return(cost);
}
private void calculateBestNode(List <Node> neighbors)
{
Node targetNode = currentNode;
//calculate best node out of directly attached neighbors
//Step 1. Calculate cost for each neighbor and track lowest
int lowestCost = int.MaxValue;
foreach (Node neighbor in neighbors)
{
NodeCost nCost = GetNodeCost(currentNode, neighbor);
neighbor.gCost = nCost.gCost;
neighbor.hCost = nCost.hCost;
if (neighbor.fCost < lowestCost)
{
targetNode = neighbor;
lowestCost = neighbor.fCost;
}
else if (neighbor.fCost == lowestCost)
{
//pick closer to end than start
if (neighbor.hCost < targetNode.hCost)
{
targetNode = neighbor;
lowestCost = neighbor.fCost;
}
}
}
//compare this node to non-visited discovered nodes
if (bestDiscoveredNode == null || targetNode.fCost < bestDiscoveredNode.fCost)
{
//set destination to targetNode
destinationVector = targetNode.worldPos;
}
else
{
//set destination to bestDiscoveredNode
//we use [0] because the list should be sorted with lowest fCost first
backtrackPath = BackTrackV2(discoveredNodes[0]);
discoveredNodes.RemoveAt(0);
currentlyBacktracking = true;
movementSpeed *= 2;
destinationVector = backtrackPath.Dequeue().worldPos;
}
//keep track of node in list with lowest cost?
//update the list when we actually go to that node, then calculate new best node, saves on loops
//compare best neighbor with best previous discovered nodes
//if best node is neighbor, set node as target destination
//if best node is non-visited node, set target to backtrack queue to target node
}
public void Execute()
{
int2 startNode = grid.GetNodeIndex(srcPosition);
int2 endNode = grid.GetNodeIndex(dstPosition);
if (startNode.x == -1 || endNode.x == -1)
{
return;
}
open.Add(new NodeCost(startNode, startNode));
int2 boundsMin = new int2(0, 0);
int2 boundsMax = new int2 {
x = grid.width, y = grid.height
};
NodeCost currentNode = new NodeCost(startNode, startNode);
while (open.Count > 0)
{
currentNode = open.RemoveFirst();
if (!closed.TryAdd(currentNode.idx, currentNode))
{
break;
}
if (math.all(currentNode.idx == endNode))
{
break;
}
for (int xC = -1; xC <= 1; xC++)
{
for (int yC = -1; yC <= 1; yC++)
{
int2 newIdx = +currentNode.idx + new int2(xC, yC);
if (math.all(newIdx >= boundsMin & newIdx < boundsMax))
{
Node neighbor = grid.GetNode(newIdx.x, newIdx.y);
NodeCost newCost = new NodeCost(newIdx, currentNode.idx);
if (!neighbor.walkable || closed.TryGetValue(newIdx, out NodeCost _))
{
continue;
}
int newGCost = currentNode.gCost + NodeDistance(currentNode.idx, newIdx);
newCost.gCost = newGCost;
newCost.hCost = NodeDistance(newIdx, endNode);
int oldIdx = open.IndexOf(newCost);
if (oldIdx >= 0)
{
if (newGCost < open[oldIdx].gCost)
{
open.RemoveAt(oldIdx);
open.Add(newCost);
}
}
else
{
if (open.Count < open.Capacity)
{
open.Add(newCost);
}
else
{
return;
}
}
}
}
}
} //while end
while (!math.all(currentNode.idx == currentNode.origin))
{
result.Add(currentNode.idx);
if (!closed.TryGetValue(currentNode.origin, out NodeCost next))
{
return;
}
currentNode = next;
}
} //execute end
