贪婪最佳优先算法是一种没有做任何长期计划就马上选择最佳答案的算法。因此贪婪优先算法得到的寻路路径是一种次优路径。算法流程如下: 下面来根据流程图完成代码的编写。
节点类的编写 节点主要用来存放当前地图中位置信息,包括当前位置的行号、列号、父节点、邻居节点集合和当前节点到目标节点的距离,代码如下:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 public class Node { //行号 public int Row{ get; set; } //列号 public int Col{ get; set; } //父节点 public Node parent; //邻居节点的集合 public List<Node> adjacent=new List<Node>(); //到目标节点的距离 public float h = 0; public void Clear() { parent=null; h=0; } }
其中到目标节点的距离为曼哈顿距离,即当前节点到目标节点的向量的x、y的绝对值的和。
地图的存储 有了节点之后便可以构成一张二维地图,地图中有可行的节点和不可行的节点(障碍物),这里我们用0表示可通过,1表示不可通过。代码如下:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 public class Map { //行数 public int Rows; //列数 public int Cols; //地图中的节点,行优先排列 public Node[] nodes; public Map(int[,] mapArray) { Rows = mapArray.GetLength(0); Cols = mapArray.GetLength(1); //分配地图节点 nodes=new Node[Rows * Cols]; for(int i = 0; i < nodes.Length; i++) { Node node = new Node(); node.Row = i / Cols; node.Col = i % Cols; nodes[i] = node; } int row , col; //分配邻居节点 for (int j = 0; j < nodes.Length; j++) { row = nodes[j].Row; col = nodes[j].Col; //1表示有障碍物,不能通过 //0表示无障碍物,可以通过 if(mapArray[row,col] != 1) { //相邻上方的一个节点 if (row > 0 && mapArray[row - 1, col] != 1) nodes[j].adjacent.Add(nodes[(row - 1) * cols + col]); //相邻右边的一个节点 if (col + 1 < cols && mapArray[row, col + 1] != 1) nodes[j].adjacent.Add(nodes[row * cols + col + 1]); //相邻下方的一个节点 if (row + 1 < rows && mapArray[row + 1, col] != 1) nodes[j].adjacent.Add(nodes[(row + 1) * cols + col]); //相邻左边的一个节点 if (col > 0 && mapArray[row, col - 1] != 1) nodes[j].adjacent.Add(nodes[row * cols + col - 1]); } } } }
地图生成完之后,就可以根据起始点和目标点来寻找一条”最优”的到达路径了。
寻路算法实现 在有了地图信息之后,我们还不能立马去搜索地图寻找”最优”路径,因为我们还需要两个集合来存放地图搜索过程中以确定的节点和未确定但以估算h(x)值的节点,分别是封闭集合和开放集合。这样我们可以根据当前节点的邻居节点信息,算出所有邻居节点到目标点的曼哈顿距离h(x),并将当前节点作为邻居节点的父节点。而在计算一个节点的h(x)之前,我们首先要判断此节点是否存在于封闭集合中,如果不存在则计算,否侧继续下一个节点的判断,并把此节点加入到开放集合中。 在处理完邻居节点之后,此时我们还需要查看开放集合是否为空,如果为空则证明路径查找失败,退出路径查找。否则从开放集合中找出曼哈顿最小的节点加入到封闭集合从并从开放集合中一处,然后将此节点作为当前节点继续下一轮的查找,直至当前节点为目标节点。这样我们就可以得到一个链表,通过parent从终点指想起点,我们需要将其逆置得到我们想要的从起点到终点的路径。完整代码如下:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 public class GreedBestPriorityAlgorithm { //地图 private Map map; //终点 private Node destNode; //开放集合 private List<Node> openSet = new List<Node>(); //封闭集合 private List<Node> closedSet = new List<Node>(); //初始化地图 public GreedyBestPriorityAlgorithm(Map map) { this.map = map } //计算曼哈顿距离 private void H(Node node) { return Mathf.Abs(node.Row - destNode.Row) + Mathf.Abs(node.Col - destNode.Col); } //查找开放式集合中h(x)值最小的点 private Node FindLowestH() { Node node = openSet[0]; for(int i = 0; i < opencSet.Count; i++) { if(openSet[i].h < node.h) { node = openSet[i]; } } return node; } //计算当前节点邻居节点的h(x) private void AddAdjacent(Node node) { for(int i = 0; i < node.adjacent.Count; i++) { if(closedSet.Contains(node.adjacent[i]) continue; else { node.adjacent[i].parent = node; if(!openSet.Contains(node.adjacent[i]) { node.adjacent[i].h = H(node.adjacent[i]); openSet.Add(node.adjacent[i]); } } } } //更新地图 public UpdateMap(Map map) { this.map = map; } //设置起点、终点、清楚地图节点存放的信息 public void Start(Node startNode, Node endNode) { openSet.Clear(); closedSet.Clear(); closedSet.Add(startNode); destNode = endNode; for(int i = 0; i < map.nodes.Length; i++) { map.nodes[i].Clear(); } } //寻找路径 public Stack<Node> Find() { Stack<Node> path = new Stack<Node>(); Node currNode = closedSet[0]; while(currNode != destNode) { AddAdjacent(currNode); if(openSet.Count == 0) break; currNode = FindLowestH(); closedSet.Add(currNode); openSet.Remove(currNode); } if(currNode == destNode) { Node node = destNode; while(node != null) { path.Push(node); node = node.parent; } } else return null; return path; } }
对于最后的路径使用栈逆置,如果不想使用也可以直接计算起点到终点的路径,这样可以减少一定的计算开销。对于此算法还可以进行如下优化:
对于从开放集合中查找最小h(x)值的节点,我们可以使用二叉树最为开放集合(左边是比根节点小的值,右边是比根节点大的值),这样查找速度就是O(1);
Unity实现效果
完整工程仓库地址:贪婪最佳优先算法 CSDND地址:基于Unity的贪婪最佳优先算法
参考 《游戏编程算法与技巧》寻路算法-贪婪最佳优先算法