private void ReconstructPath(NodeDataPair currentPair, ref List <NavMesh2DNode> path) { path.Add(currentPair.n); if (currentPair.d.cameFrom != null) { ReconstructPath(currentPair.d.cameFrom, ref path); } }
private List <NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end) { AStarData startData = new AStarData { cameFrom = null, fScore = 0, gScore = 0, hScore = 0 }; Dictionary <NavMesh2DNode, NodeDataPair> openList = new Dictionary <NavMesh2DNode, NodeDataPair>(); Dictionary <NavMesh2DNode, NodeDataPair> closedList = new Dictionary <NavMesh2DNode, NodeDataPair>(); openList.Add(start, new NodeDataPair { n = start, d = startData }); while (openList.Count != 0) { NodeDataPair current = null; { float lowestFscore = float.MaxValue; foreach (NodeDataPair n in openList.Values) { if (n.d.fScore < lowestFscore) { lowestFscore = n.d.fScore; current = n; } } } openList.Remove(current.n); closedList.Add(current.n, current); if (closedList.ContainsKey(end)) { //found a path List <NavMesh2DNode> pathFound = new List <NavMesh2DNode>(); ReconstructPath(current, ref pathFound); pathFound.Reverse(); //Path path = new Path(startNode, endNode); //FindGoingTo(endNode, null); //ReconstructPath(startNode, ref path); //path.Smooth(); return(pathFound); } foreach (NavMesh2DConnection connection in current.n.connections) { NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex); if (closedList.ContainsKey(connectedMesh2DNode)) { continue; } NodeDataPair pairAlreadyInOpenlist = null; if (openList.ContainsKey(connectedMesh2DNode)) { pairAlreadyInOpenlist = openList[connectedMesh2DNode]; } if (pairAlreadyInOpenlist == null) { //if the neighbor doesn't exist AStarData nextNodeData = new AStarData(); nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position); nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find //paths that are less dangerous, etc. danger being the heucistic. nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore; nextNodeData.cameFrom = current; openList.Add(connectedMesh2DNode, new NodeDataPair { n = connectedMesh2DNode, d = nextNodeData }); } else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore) { //if the neighbor exists and has bigger g score pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position); pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position); //connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position); pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore; pairAlreadyInOpenlist.d.cameFrom = current; } } } return(null); }
private void ReconstructPath(NodeDataPair currentPair, ref List<NavMesh2DNode> path) { path.Add(currentPair.n); if (currentPair.d.cameFrom != null) ReconstructPath(currentPair.d.cameFrom, ref path); }