private static void TryQueueNewNode(IPathNode pNewNode, PathLink pLink, AStarStack pNodesToVisit, IPathNode pGoal) { IPathNode previousNode = pLink.GetOtherNode(pNewNode); float linkDistance = pLink.Distance; float newPathCost = previousNode.PathCostHere + pNewNode.CostMultiplier * linkDistance; if (pNewNode.LinkLeadingHere == null || (pNewNode.PathCostHere > newPathCost)) { pNewNode.DistanceToGoal = pNewNode.DistanceTo(pGoal) * 2f; pNewNode.PathCostHere = newPathCost; pNewNode.LinkLeadingHere = pLink; pNodesToVisit.Push(pNewNode); } }
private void TryQueueNewTile(IPathNode pNewNode, PathLink pLink, AStarStack pNodesToVisit, IPathNode pGoal) { IPathNode 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); } }
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)); }