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