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));
}
