public void InitMapInfo(int w, int h)
{
for (int i = 0; i < w; i++)
{
for (int j = 0; j < h; j++)
{
AStartNode node = new AStartNode(i, j, Random.Range(0, 100) < 20 ? NodeType.Stop : NodeType.Walk);
}
}
}
public List <AStartNode> FindPath(Vector2 startPos, Vector2 endPos)
{
// 判断范围 省略
// 判断阻挡 省略
AStartNode start = nodes[(int)startPos.x, (int)startPos.y];
AStartNode end = nodes[(int)endPos.x, (int)endPos.y];
// 清空上次寻路数据
closeList.Clear();
openList.Clear();
start.father = null;
start.f = 0;
start.g = 0;
start.h = 0;
closeList.Add(start);
while (true)
{
FindNearlyNodeToOpenList(start.x - 1, start.y - 1, 1.4f, start, end);
FindNearlyNodeToOpenList(start.x, start.y - 1, 1f, start, end);
FindNearlyNodeToOpenList(start.x + 1, start.y - 1, 1.4f, start, end);
FindNearlyNodeToOpenList(start.x - 1, start.y, 1.4f, start, end);
FindNearlyNodeToOpenList(start.x + 1, start.y, 1.4f, start, end);
FindNearlyNodeToOpenList(start.x - 1, start.y + 1, 1.4f, start, end);
FindNearlyNodeToOpenList(start.x, start.y + 1, 1f, start, end);
FindNearlyNodeToOpenList(start.x + 1, start.y + 1, 1.4f, start, end);
if (openList.Count == 0)
{
return(null);
}
openList.Sort(SortOpenList);
closeList.Add(openList[0]);
start = openList[0];
openList.RemoveAt(0);
if (start == end)
{
List <AStartNode> path = new List <AStartNode>();
path.Add(end);
while (end.father != null)
{
path.Add(end.father);
end = end.father;
}
path.Reverse();
return(path);
}
}
}
private int SortOpenList(AStartNode a, AStartNode b)
{
if (a.f >= b.f)
{
return(1);
}
else
{
return(-1);
}
}
//Find all nodes that surround a particiular node which are empty
//it also takes into account you can only consider diagnol nodes empty if there is
//no walls on either relevant side
List <AStartNode> GetSurroundingEmptyNodes(AStartNode fromNode)
{
var emptyNodes = new List <AStartNode>();
var surroundingPoints = GetSurroundingPoints(new Vector2(fromNode.Point.x, fromNode.Point.y));
foreach (var surroundingPoint in surroundingPoints)
{
int x = (int)surroundingPoint.x;
int y = (int)surroundingPoint.y;
if (x < 0 || x >= m_width || y < 0 || y >= m_height)
{
continue;
}
var node = m_aStartWorld[x, y];
if (!node.IsEmpty)
{
continue;
}
if (node.State == AStarState.Closed)
{
continue;
}
//Only to be added if their G-value is lower. This means that this route is lower for this node than the previously explored way that considered it
if (node.State == AStarState.Open)
{
var preG = fromNode.G + node.Cost;
if (preG < node.G)
{
node.Load(fromNode);
emptyNodes.Add(node);
}
continue;
}
else
{
//If untested, set the parent and flat it as Open for consideration
node.Load(fromNode);
node.State = AStarState.Open;
emptyNodes.Add(node);
}
}
return(emptyNodes);
}
//The method that encases all the logic of A*'s actual path finding.
//Returns true if a path can be found, false it not.
//The root parameter is the starting point of the search
bool Search(AStartNode root)
{
//All the branches hit of possible paths as the best path is found.
//The branches are organized by combined F values
var nodes = new SortedDictionary <float, Queue <AStartNode> >();
//A action to add a node to the relevant path
Action <AStartNode> AddNodeToNodes = (node) => {
if (!nodes.ContainsKey(node.F))
{
nodes.Add(node.F, new Queue <AStartNode>());
}
nodes[node.F].Enqueue(node);
};
//A action that finds the node with the lowset F and dequeues it from the list of nodes to try
Func <AStartNode> GetLowestFs = () => {
foreach (var key in nodes.Keys.ToList())
{
if (nodes[key].Count > 0)
{
return(nodes[key].Dequeue());
}
else
{
nodes.Remove(key);
}
}
return(null);
};
AddNodeToNodes(root);
AStartNode currentNode = GetLowestFs();
//The 'recusive' loop, written as a do:while
do
{
//Mark the current node to check as closed
currentNode.State = AStarState.Closed;
var surroundingEmptyNodes = GetSurroundingEmptyNodes(currentNode).OrderBy(x => x.F).ToList();
bool finish = false;
//For every surrounding empty node ordered by F values
foreach (var surroundingNode in surroundingEmptyNodes)
{
//If we found the end, mark it as finished and leave the loop
if (surroundingNode.Point == m_endPoint)
{
finish = true;
surroundingNode.Load(currentNode);
break;
//Otherwise, add the current node to the Nodes dictionary
}
else
{
surroundingNode.Load(currentNode);
AddNodeToNodes(surroundingNode);
}
}
//If we are finished, break out of the loop as we found the best path
if (finish)
{
break;
}
//Set the current node to check to be that with the lowest F value in the nodes we can check next
currentNode = GetLowestFs();
} while (currentNode != null);
//Return success or fail
return(currentNode != null);
}
