protected virtual void Awake() { if (data == null) { data = new NavNodeData(); } data.position = transform.position; data.node = this; }
private List <NavNodeData> GetPathWorker(NavNodeData start, NavNodeData end) { AStarData startData = new AStarData { cameFrom = null, fScore = 0, gScore = 0, hScore = 0 }; Dictionary <NavNodeData, NodeDataPair> openList = new Dictionary <NavNodeData, NodeDataPair>(); Dictionary <NavNodeData, NodeDataPair> closedList = new Dictionary <NavNodeData, 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 <NavNodeData> pathFound = new List <NavNodeData>(); ReconstructPath(current, ref pathFound); pathFound.Reverse(); //Path path = new Path(startNode, endNode); //FindGoingTo(endNode, null); //ReconstructPath(startNode, ref path); //path.Smooth(); return(pathFound); } foreach (NavNodeConnection connection in current.n.connections) { if (closedList.ContainsKey(connection.connectedNode.data)) { continue; } NodeDataPair pairAlreadyInOpenlist = null; if (openList.ContainsKey(connection.connectedNode.data)) { pairAlreadyInOpenlist = openList[connection.connectedNode.data]; } if (pairAlreadyInOpenlist == null) { //if the neighbor doesn't exist AStarData nextNodeData = new AStarData(); nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connection.connectedNode.data.position); nextNodeData.hScore = CalculateHeuristic(connection.connectedNode.data.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(connection.connectedNode.data, new NodeDataPair { n = connection.connectedNode.data, d = nextNodeData }); } else if (current.d.gScore + Vector3.Distance(current.n.position, connection.connectedNode.data.position) < pairAlreadyInOpenlist.d.gScore) { //if the neighbor exists and has bigger g score pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connection.connectedNode.data.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); }