public Path FindPath(PointTileNode pStart, PointTileNode pGoal, RoomRunner pNetwork, bool pReset) { #if DEBUG if(pNetwork == null) { throw new Exception("pNetwork is null"); } #endif if (pStart == null || pGoal == null) { return new Path(new PointTileNode[] {}, 0f, PathStatus.DESTINATION_UNREACHABLE, 0); } if (pStart == pGoal) { return new Path(new PointTileNode[] {}, 0f, PathStatus.ALREADY_THERE, 0); } int testCount = 0; if(pReset) { pNetwork.Reset(); } pStart.isStartNode = true; pGoal.isGoalNode = true; List<PointTileNode> resultNodeList = new List<PointTileNode>(); PointTileNode currentNode = pStart; PointTileNode goalNode = pGoal; currentNode.visited = true; currentNode.linkLeadingHere = null; AStarStack nodesToVisit = new AStarStack(); PathStatus pathResult = PathStatus.NOT_CALCULATED_YET; testCount = 1; while (pathResult == PathStatus.NOT_CALCULATED_YET) { foreach (PathLink l in currentNode.links) { PointTileNode otherNode = l.GetOtherNode(currentNode); if (!otherNode.visited) { TryQueueNewTile(otherNode, l, nodesToVisit, goalNode); } } if (nodesToVisit.Count == 0) { pathResult = PathStatus.DESTINATION_UNREACHABLE; } else { currentNode = nodesToVisit.Pop(); testCount++; #if LOG D.Log("testing new node: " + currentNode); #endif currentNode.visited = true; if (currentNode == goalNode) { pathResult = PathStatus.FOUND_GOAL; } } } // Path finished, collect float tLength = 0; if (pathResult == PathStatus.FOUND_GOAL) { tLength = currentNode.pathCostHere; while (currentNode != pStart) { resultNodeList.Add((PointTileNode)currentNode); currentNode = currentNode.linkLeadingHere.GetOtherNode(currentNode); } resultNodeList.Add((PointTileNode)currentNode); resultNodeList.Reverse(); } return new Path(resultNodeList.ToArray(), tLength, pathResult, testCount); }
private void TryQueueNewTile(PointTileNode pNewNode, PathLink pLink, AStarStack pNodesToVisit, PointTileNode pGoal) { PointTileNode previousNode = pLink.GetOtherNode(pNewNode); float linkDistance = pLink.distance; float newPathCost = previousNode.pathCostHere + pNewNode.baseCost + linkDistance; if (pNewNode.linkLeadingHere == null || (pNewNode.pathCostHere > newPathCost)) { pNewNode.distanceToGoal = pNewNode.DistanceTo(pGoal) * 2f; pNewNode.pathCostHere = newPathCost; pNewNode.linkLeadingHere = pLink; pNodesToVisit.Push(pNewNode); } }