private NodeComponent GetMinNode(List <NodeComponent> open) { NodeComponent candidate = null; HeuristicNodeInfo currNodeInfo = null; float minCost = float.MaxValue; foreach (var node in open) { currNodeInfo = node.NodeInfo as HeuristicNodeInfo; if (candidate == null || minCost > currNodeInfo.estimatedTotalCost) { candidate = node; minCost = currNodeInfo.estimatedTotalCost; } } Debug.Log(minCost); return(candidate); }
private void ProcessConnection(NodeComponent goal, List <NodeComponent> open, NodeComponent fromNode, Connection connection) { var toNode = connection.To; var toNodeInfo = toNode.NodeInfo as HeuristicNodeInfo; HeuristicNodeInfo newNodeInfo = new HeuristicNodeInfo( connection, Mathf.Infinity, fromNode.NodeInfo.costSoFar + connection.Cost); float endNodeHeuristic; if (toNodeInfo.category == Category.Closed) { //If the node is closed, we need to update its cost //only when it is greater then the one of the current path //we also need to add the node again on the open list, because we should //update its neighbors if (toNodeInfo.costSoFar > newNodeInfo.costSoFar) { newNodeInfo.category = Category.Open; open.Add(toNode); toNode.NodeInfo = GetUpdateNodeInfo(toNodeInfo, newNodeInfo); } } else if (toNodeInfo.category == Category.Open) { //If the node is opened, we should update its cost //only when it is greater then the one of the current path if (toNodeInfo.costSoFar > newNodeInfo.costSoFar) { toNode.NodeInfo = GetUpdateNodeInfo(toNodeInfo, newNodeInfo); } } else { //the node is unvisited: we calculate its heuristic and add it //to the open list endNodeHeuristic = heuristicFuncDict[heuristicType](toNode, goal); newNodeInfo.estimatedTotalCost = newNodeInfo.costSoFar + endNodeHeuristic; newNodeInfo.category = Category.Open; toNode.NodeInfo = newNodeInfo; open.Add(toNode); } }
private HeuristicNodeInfo GetUpdateNodeInfo(HeuristicNodeInfo oldNodeInfo, HeuristicNodeInfo newNodeInfo) { newNodeInfo.estimatedTotalCost = newNodeInfo.costSoFar + (oldNodeInfo.estimatedTotalCost - oldNodeInfo.costSoFar); //the heuristic is already calculated return(newNodeInfo); }