java a星寻路算法_算法之A星算法（寻路）

1 packagecn.liushaofeng.algorithm;2

3 importjava.util.ArrayList;4 importjava.util.List;5

6 /**

7 * A Star Algorithm8 *@authorliushaofeng9 * @date 2015-8-24 下午11:05:4810 *@version1.0.011 */

12 public classAstarAlgorithm13 {14 private List openList = null;15 private List closeList = null;16 private int[][] map;17

18 /**

19 * default constructor20 *@parammap data map21 */

22 public AstarAlgorithm(int[][] map)23 {24 this.map =map;25 this.openList = new ArrayList();26 this.closeList = new ArrayList();27 }28

29 /**

30 * find path31 *@paramsrcNode source node32 *@paramdesNode destination node33 *@returnnode path34 */

35 publicNode findPath(Node srcNode, Node desNode)36 {37 init(srcNode);38 do

39 {40 if(openList.isEmpty())41 {42 break;43 }44

45 Node node = openList.get(0);46 List aroundPoint =getAroundPoint(srcNode, node, desNode);47 openList.addAll(aroundPoint);48 closeList.add(node);49 openList.remove(node);50

51 } while (!findDes(desNode));52

53 returnfindNodePath(desNode);54 }55

56 privateNode findNodePath(Node desNode)57 {58 for(Node node : openList)59 {60 if (node.getX() == desNode.getX() && node.getY() ==desNode.getY())61 {62 returnnode;63 }64 }65 return null;66 }67

68 private booleanfindDes(Node desNode)69 {70 for(Node node : openList)71 {72 if (node.getX() == desNode.getX() && node.getY() ==desNode.getY())73 {74 return true;75 }76 }77 return false;78 }79

80 private voidinit(Node srcNode)81 {82 openList.add(srcNode);83 }84

85 //top bottom left and right, four points

86 private ListgetAroundPoint(Node srcNode, Node nextNode, Node desNode)87 {88 int x =srcNode.getX();89 int y =srcNode.getY();90

91 int[] xData = new int[2];92 int[] yData = new int[2];93 if (x - 1 >= 0)94 {95 xData[0] = x - 1;96 }97 if (x + 1

102 if (y - 1 >= 0)103 {104 yData[0] = y - 1;105 }106 if (y + 1 < map[0].length)107 {108 yData[1] = y + 1;109 }110

111 List tmpList = new ArrayList();112

113 for (inti : xData)114 {115 Node node = newNode(i, y, srcNode);116 if (!isObstacle(node) && !inClosetList(node))117 {118 calcWeight(srcNode, node, desNode);119 tmpList.add(node);120 }121 }122

123 for (inti : yData)124 {125 Node node = newNode(x, i, srcNode);126 if (!isObstacle(node) && !inClosetList(node))127 {128 calcWeight(srcNode, node, desNode);129 tmpList.add(node);130 }131 }132

133 returntmpList;134 }135

136 private voidcalcWeight(Node parentNode, Node node, Node desNode)137 {138 node.setG(parentNode.getG() + 10);139 int h = Math.abs(node.getX() - desNode.getX()) + Math.abs(node.getY() -desNode.getY());140 node.setWeight(node.getG() + h * 10);141 }142

143 private booleaninClosetList(Node nextNode)144 {145 for(Node node : closeList)146 {147 if (node.getX() == nextNode.getX() && node.getY() ==nextNode.getY())148 {149 return true;150 }151 }152 return false;153 }154

155 private booleanisObstacle(Node nextNode)156 {157 return map[nextNode.getX()][nextNode.getY()] == 1;158 }159

160 public static voidmain(String[] args)161 {162 int[][] map =

163 {164 { 0, 0, 0, 0, 0, 0, 0},165 { 0, 0, 0, 0, 0, 0, 0},166 { 0, 0, 0, 1, 0, 0, 0},167 { 0, 0, 0, 1, 0, 0, 0},168 { 0, 0, 0, 1, 0, 0, 0},169 { 0, 0, 0, 0, 0, 0, 0},170 { 0, 0, 0, 0, 0, 0, 0} };171

172 AstarAlgorithm astar = newAstarAlgorithm(map);173 Node pathNode = astar.findPath(new Node(3, 1, null), new Node(3, 5, null));174 System.out.println(pathNode == null ? "Can not find path!": pathNode.toString());175 }176 }