寻路算法之A星算法

  1 package cn.liushaofeng.algorithm;
  2 
  3 import java.util.ArrayList;
  4 import java.util.List;
  5 
  6 /**
  7  * A Star Algorithm
  8  * @author liushaofeng
  9  * @date 2015-8-24 下午11:05:48
 10  * @version 1.0.0
 11  */
 12 public class AstarAlgorithm
 13 {
 14     private List<Node> openList = null;
 15     private List<Node> closeList = null;
 16     private int[][] map;
 17 
 18     /**
 19      * default constructor
 20      * @param map data map
 21      */
 22     public AstarAlgorithm(int[][] map)
 23     {
 24         this.map = map;
 25         this.openList = new ArrayList<Node>();
 26         this.closeList = new ArrayList<Node>();
 27     }
 28 
 29     /**
 30      * find path
 31      * @param srcNode source node
 32      * @param desNode destination node
 33      * @return node path
 34      */
 35     public Node 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<Node> aroundPoint = getAroundPoint(srcNode, node, desNode);
 47             openList.addAll(aroundPoint);
 48             closeList.add(node);
 49             openList.remove(node);
 50 
 51         } while (!findDes(desNode));
 52 
 53         return findNodePath(desNode);
 54     }
 55 
 56     private Node findNodePath(Node desNode)
 57     {
 58         for (Node node : openList)
 59         {
 60             if (node.getX() == desNode.getX() && node.getY() == desNode.getY())
 61             {
 62                 return node;
 63             }
 64         }
 65         return null;
 66     }
 67 
 68     private boolean findDes(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 void init(Node srcNode)
 81     {
 82         openList.add(srcNode);
 83     }
 84 
 85     // top bottom left and right, four points
 86     private List<Node> getAroundPoint(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 < map.length)
 98         {
 99             xData[1] = x + 1;
100         }
101 
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<Node> tmpList = new ArrayList<Node>();
112 
113         for (int i : xData)
114         {
115             Node node = new Node(i, y, srcNode);
116             if (!isObstacle(node) && !inClosetList(node))
117             {
118                 calcWeight(srcNode, node, desNode);
119                 tmpList.add(node);
120             }
121         }
122 
123         for (int i : yData)
124         {
125             Node node = new Node(x, i, srcNode);
126             if (!isObstacle(node) && !inClosetList(node))
127             {
128                 calcWeight(srcNode, node, desNode);
129                 tmpList.add(node);
130             }
131         }
132 
133         return tmpList;
134     }
135 
136     private void calcWeight(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 boolean inClosetList(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 boolean isObstacle(Node nextNode)
156     {
157         return map[nextNode.getX()][nextNode.getY()] == 1;
158     }
159 
160     public static void main(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 = new AstarAlgorithm(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 }

  1 package cn.liushaofeng.algorithm;
  2 
  3 import java.util.ArrayList;
  4 import java.util.List;
  5 
  6 /**
  7  * Node
  8  * @author liushaofeng
  9  * @date 2015-8-24 下午09:48:53
 10  * @version 1.0.0
 11  */
 12 public class Node
 13 {
 14     private Node parentNode;
 15     private int x;
 16     private int y;
 17 
 18     private int weight;
 19     private int g;
 20 
 21     /**
 22      * default constructor
 23      * @param x x point
 24      * @param y y point
 25      * @param parentNode parent node
 26      */
 27     public Node(int x, int y, Node parentNode)
 28     {
 29         this.x = x;
 30         this.y = y;
 31         this.parentNode = parentNode;
 32     }
 33 
 34     public int getG()
 35     {
 36         return g;
 37     }
 38 
 39     public void setG(int g)
 40     {
 41         this.g = g;
 42     }
 43 
 44     public Node getParentNode()
 45     {
 46         return parentNode;
 47     }
 48 
 49     public void setParentNode(Node parentNode)
 50     {
 51         this.parentNode = parentNode;
 52     }
 53 
 54     public int getX()
 55     {
 56         return x;
 57     }
 58 
 59     public void setX(int x)
 60     {
 61         this.x = x;
 62     }
 63 
 64     public int getY()
 65     {
 66         return y;
 67     }
 68 
 69     public void setY(int y)
 70     {
 71         this.y = y;
 72     }
 73 
 74     public int getWeight()
 75     {
 76         return weight;
 77     }
 78 
 79     public void setWeight(int weight)
 80     {
 81         this.weight = weight;
 82     }
 83 
 84     @Override
 85     public String toString()
 86     {
 87         return getPath();
 88     }
 89 
 90     private String getPath()
 91     {
 92         List<Node> dataList = new ArrayList<Node>();
 93         Node node = this;
 94         while (node != null)
 95         {
 96             dataList.add(node);
 97             node = node.getParentNode();
 98         }
 99 
100         StringBuffer sb = new StringBuffer();
101         for (int i = dataList.size() - 1; i >= 0; i--)
102         {
103             if (i == 0)
104             {
105                 sb.append("(" + dataList.get(i).getX() + "," + dataList.get(i).getY() + ")");
106             } else
107             {
108                 sb.append("(" + dataList.get(i).getX() + "," + dataList.get(i).getY() + ")->");
109             }
110         }
111         return sb.toString();
112     }
113 }