public Path <IntPointNode> FindPath(IPathNode start, IPathNode goal, bool reset) { if (start == null || goal == null) { return(new Path <IntPointNode>(new IntPointNode[] { }, 0f, PathStatus.DESTINATION_UNREACHABLE, 0)); } if (start == goal) { return(new Path <IntPointNode>(new IntPointNode[] { }, 0f, PathStatus.ALREADY_THERE, 0)); } int testCount = 0; if (reset) { Reset(); } start.IsStartNode = true; goal.IsGoalNode = true; List <IntPointNode> resultNodeList = new List <IntPointNode>(); IPathNode currentNode = start; IPathNode goalNode = goal; 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) { IPathNode otherNode = l.GetOtherNode(currentNode); if (!otherNode.Visited) { TryQueueNewNode(otherNode, l, nodesToVisit, goalNode); } } if (nodesToVisit.Count == 0) { pathResult = PathStatus.DESTINATION_UNREACHABLE; } else { currentNode = nodesToVisit.Pop(); testCount++; 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 != start) { resultNodeList.Add((IntPointNode)currentNode); currentNode = currentNode.LinkLeadingHere.GetOtherNode(currentNode); } resultNodeList.Add((IntPointNode)currentNode); resultNodeList.Reverse(); } return(new Path <IntPointNode>(resultNodeList.ToArray(), tLength, pathResult, testCount)); }
public Path <PathNodeType> FindPath(IPathNode pStart, IPathNode pGoal, IPathNetwork <PathNodeType> pNetwork) { int testCount = 0; pNetwork.Reset(); pStart.isStartNode = true; pGoal.isGoalNode = true; List <PathNodeType> resultNodeList = new List <PathNodeType>(); if (pStart == pGoal) { return(new Path <PathNodeType>(resultNodeList.ToArray(), 0f, PathStatus.ALREADY_THERE, testCount)); } IPathNode currentNode = pStart; IPathNode goalNode = pGoal; if (currentNode == null || pGoal == null) { return(new Path <PathNodeType>(resultNodeList.ToArray(), 0f, PathStatus.DESTINATION_UNREACHABLE, testCount)); } 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) { IPathNode 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++; // Console.WriteLine("testing new node: " + (currentNode as TileNode).localPoint); 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((PathNodeType)currentNode); currentNode = currentNode.linkLeadingHere.GetOtherNode(currentNode); } resultNodeList.Add((PathNodeType)currentNode); resultNodeList.Reverse(); } return(new Path <PathNodeType>(resultNodeList.ToArray(), tLength, pathResult, testCount)); }