AStar算法的學習

摘自：http://www.cnblogs.com/hxsyl/p/3994730.html

A*算法的java實現

import java.util.ArrayList;
import java.util.Collections;
import java.util.Stack;

/**
 * @author pang
 * 
 */
public class AStartPathFind {

	//前四個上下左右
	public final static int[] dx = { 0, -1, 0, 1, -1, -1, 1, 1 };
	public final static int[] dy = { -1, 0, 1, 0, 1, -1, -1, 1 };

	// 最外圈都是1表示不可經過
	final static public int[][] map = {
			{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 1, 1, 0, 1, 1, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
			{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 } };

	public static void main(String[] args) {
		// TODO Auto-generated method stub
		Point start = new Point(1, 1);
		Point end = new Point(10, 13);
		/*
		 * 第一個問題：起點FGH須要初始化嗎？ 看參考資料的圖片發現不須要
		 */
		Stack<Point> stack = printPath(start, end);
		if (null == stack) {
			System.out.println("不可達");
		} else {
			while (!stack.isEmpty()) {
				// 輸出(1,2)這樣的形勢須要重寫toString
				System.out.print(stack.pop() + " -> ");
			}
			System.out.println();
		}

	}

	public static Stack<Point> printPath(Point start, Point end) {

		/*
		 * 不用PriorityQueue是由於必須取出存在的元素
		 */
		ArrayList<Point> openTable = new ArrayList<Point>();
		ArrayList<Point> closeTable = new ArrayList<Point>();
		openTable.clear();
		closeTable.clear();
		Stack<Point> pathStack = new Stack<Point>();
		start.parent = null;
		// 該點起到轉換做用，就是當前擴展點
		Point currentPoint = new Point(start.x, start.y);
		// closeTable.add(currentPoint);
		boolean flag = true;

		while (flag) {
			for (int i = 0; i < 8; i++) {
				int fx = currentPoint.x + dx[i];
				int fy = currentPoint.y + dy[i];
				Point tempPoint = new Point(fx, fy);
				if (map[fx][fy] == 1) {
					// 因爲邊界都是1中間障礙物也是1，，這樣沒必要考慮越界和障礙點擴展問題
					// 若是不設置邊界那麼fx >=map.length &&fy>=map[0].length判斷越界問題
					continue;
				} else {
					if (end.equals(tempPoint)) {
						flag = false;
						// 不是tempPoint，他倆都同樣了此時
						end.parent = currentPoint;
						break;
					}
					if (i < 4) {
						tempPoint.G = currentPoint.G + 10;
					} else {
						tempPoint.G = currentPoint.G + 14;
					}
					tempPoint.H = Point.getDis(tempPoint, end);
					tempPoint.F = tempPoint.G + tempPoint.H;
					// 由於重寫了equals方法，因此這裏包含只是按equals相等包含
					// 這一點是使用java封裝好類的關鍵
					if (openTable.contains(tempPoint)) {
						int pos = openTable.indexOf(tempPoint);
						Point temp = openTable.get(pos);
						if (temp.F > tempPoint.F) {
							openTable.remove(pos);
							openTable.add(tempPoint);
							tempPoint.parent = currentPoint;
						}
					} else if (closeTable.contains(tempPoint)) {
						int pos = closeTable.indexOf(tempPoint);
						Point temp = closeTable.get(pos);
						if (temp.F > tempPoint.F) {
							closeTable.remove(pos);
							openTable.add(tempPoint);
							tempPoint.parent = currentPoint;
						}
					} else {
						openTable.add(tempPoint);
						tempPoint.parent = currentPoint;
					}

				}
			}// end for

			if (openTable.isEmpty()) {
				return null;
			}// 無路徑
			if (false == flag) {
				break;
			}// 找到路徑
			openTable.remove(currentPoint);
			closeTable.add(currentPoint);
			Collections.sort(openTable);
			currentPoint = openTable.get(0);

		}// end while
		Point node = end;
		while (node.parent != null) {
			pathStack.push(node);
			node = node.parent;
		}
		return pathStack;
	}
}

class Point implements Comparable<Point> {
	int x;
	int y;
	Point parent;
	int F, G, H;

	public Point(int x, int y) {
		super();
		this.x = x;
		this.y = y;
		this.F = 0;
		this.G = 0;
		this.H = 0;
	}

	@Override
	public int compareTo(Point o) {
		// TODO Auto-generated method stub
		return this.F - o.F;
	}

	@Override
	public boolean equals(Object obj) {
		Point point = (Point) obj;
		if (point.x == this.x && point.y == this.y)
			return true;
		return false;
	}

	public static int getDis(Point p1, Point p2) {
		int dis = Math.abs(p1.x - p2.x) * 10 + Math.abs(p1.y - p2.y) * 10;
		return dis;
	}

	@Override
	public String toString() {
		return "(" + this.x + "," + this.y + ")";
	}

}