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);
}
}
public PathLink AddPathLink(IntPointNode nodeA, IntPointNode nodeB)
{
if (nodeA == nodeB || nodeB.Position == nodeA.Position)
{
throw new ArgumentException();
}
PathLink link = nodeB.GetLinkTo(nodeA);
if (link == null)
{
link = new PathLink(nodeA, nodeB);
link.Distance = (nodeA.Position - nodeB.Position).Length();
nodeA.Links.Add(link);
nodeB.Links.Add(link);
}
return(link);
}
public PathLink AddPathLink(IntPointNode nodeA, IntPointNode nodeB)
{
if (nodeA == nodeB || nodeB.Position == nodeA.Position)
{
Debug.Assert(false, "nodeA and node B should not be the same position");
return(null);
}
PathLink link = nodeB.GetLinkTo(nodeA);
if (link == null)
{
link = new PathLink(nodeA, nodeB);
link.Distance = (nodeA.Position - nodeB.Position).Length();
nodeA.Links.Add(link);
nodeB.Links.Add(link);
}
return(link);
}
public void AddLink(PathLink pLink)
{
Links.Add(pLink);
}
public void RemoveLink(PathLink pLink)
{
Links.Remove(pLink);
}