最短路径A*算法原理及java代码实现（看不懂是我的失败）

算法仅仅要懂原理了，代码都是小问题，先看以下理论，尤其是红色标注的（要源代码请留下邮箱，有測试用例，直接执行就可以）

A*算法

百度上的解释：

A*^[1]（A-Star)算法是一种静态路网中求解最短路最有效的直接搜索方法。

公式表示为： f(n)=g(n)+h(n),

当中 f(n) 是从初始点经由节点n到目标点的估价函数，

g(n) 是在状态空间中从初始节点到n节点的实际代价，

h(n) 是从n到目标节点最佳路径的预计代价。

保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取：

估价值h(n)<= n到目标节点的距离实际值，这样的情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。而且假设h(n)=d(n)，即距离预计h(n)等于最短距离，那么搜索将严格沿着最短路径进行， 此时的搜索效率是最高的。

假设 估价值>实际值,搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

1.2 Dijkstra算法与最佳优先搜索

　　Dijkstra算法从物体所在的初始点開始，訪问图中的结点。它迭代检查待检查结点集中的结点，并把和该结点最靠近的尚未检查的结点增加待检查结点集。该结点集从初始结点向外扩展，直到到达目标结点。Dijkstra算法保证能找到一条从初始点到目标点的最短路径，仅仅要全部的边都有一个非负的代价值。（我说“最短路径”是由于常常会出现很多差点儿相同短的路径。）在下图中，粉红色的结点是初始结点，蓝色的是目标点，而类菱形的有色区域（注：原文是teal
areas）则是Dijkstra算法扫描过的区域。颜色最淡的区域是那些离初始点最远的，因而形成探測过程（exploration）的边境（frontier）：

下图同样颜色的格子代表起点到达这些格子的代价是一样的，颜色越浅代表到达目标所须要的代价越大，Dijkstra算法均衡的向四面八方扩张，被扩张的每个格子都会记住它前一个消耗最少的那个格子，直到扩张区域包括目标点

　　最佳优先搜索（BFS）算法依照类似的流程执行，不同的是它可以评估（称为启示式的）随意结点到目标点的代价。与选择离初始结点近期的结点不同的是，它选择离目标近期的结点。BFS不能保证找到一条最短路径。然而，它比Dijkstra算法快的多，由于它用了一个启示式函数（heuristic
function）高速地导向目标结点。比如，假设目标位于出发点的南方，BFS将趋向于导向南方的路径。在以下的图中，越黄的结点代表越高的启示式值（移动到目标的代价高），而越黑的结点代表越低的启示式值（移动到目标的代价低）。这表明了与Dijkstra 算法相比，BFS执行得更快。

贪心算法：颜色同样的格子代表这些格子在理想状态下（没有障碍物的情况下）直线到达目标点的代价是一样的，从起点不停的像终点扩张，扩张的时候会记住前一个最小理想代价的格子假设碰到障碍物它会又一次选择新的理想代价最少的那一个格子直到到达目标格子

　　然而，这两个样例都不过最简单的情况——地图中没有障碍物，最短路径是直线的。如今我们来考虑前边描写叙述的凹型障碍物。Dijkstra算法执行得较慢，但确实能保证找到一条最短路径：

　　还有一方面，BFS执行得较快，可是它找到的路径明显不是一条好的路径：

　　问题在于BFS是基于贪心策略的，它试图向目标移动虽然这不是正确的路径。因为它只考虑到达目标的代价，而忽略了当前已花费的代价，于是虽然路径变得非常长，它仍然继续走下去。

　　结合两者的长处不是更好吗？1968年发明的A*算法就是把启示式方法（heuristic
approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。有点不同的是，类似BFS的启示式方法常常给出一个近似解而不是保证最佳解。然而，虽然A*基于无法保证最佳解的启示式方法，A*却能保证找到一条最短路径。

1.3 A*算法

　　我将集中讨论A*算法。A*是路径搜索中最受欢迎的选择，由于它相当灵活，而且能用于多种多样的情形之中。

　　和其他的图搜索算法一样，A*潜在地搜索图中一个非常大的区域。和Dijkstra一样，A*能用于搜索最短路径。和BFS一样，A*能用启示式函数（注：原文为heuristic）引导它自己。在简单的情况中，它和BFS一样快。

　　在凹型障碍物的样例中，A*找到一条和Dijkstra算法一样好的路径：

　　成功的秘决在于，它把Dijkstra算法（靠近初始点的结点）和BFS算法（靠近目标点的结点）的信息块结合起来。在讨论A*的标准术语中，g(n)表示从初始结点到随意结点n的代价，h(n)表示从结点n到目标点的启示式评估代价（heuristic
estimated cost）。在上图中，yellow(h)表示远离目标的结点而teal(g)表示远离初始点的结点。当从初始点向目标点移动时，A*权衡这两者。每次进行主循环时，它检查f(n)最小的结点n，当中f(n)
= g(n) + h(n)。

A*算法：起点不停的向周围总代价（总代价=实际代价+预计代价；实际代价=起点到该点最小代价；预计代价=该点到终点的在理想代价，相当于没有障碍物的，有各种函数，我们自己选择）最小的格子不停扩张，每一个格子都会记住前一个格子，前一个格子一般它周围总代价最小的能够使用的（当然不包含障碍物）格子，直到扩散的目标节点。

例如以下图，绿色代表起始点，黑色代表障碍物，红色代表终点，起点能够向前后左右斜着总共8个方向扩张，平移和上下移动一格消耗代价为10，斜着移动一格消耗代价14

第一次扩散，假设是像四周8个方向扩散，我为了简化图外面都算障碍物 总共扫描了5个格子，增加openMap,openMap表示能够移动到的格子的集合，closeMap表示已经移动过的格子。

上移动一格的信息例如以下：

坐标（1,1）上一坐标：（1,2）

实际代价：g(n)=向上一格=1*10=10

曼哈顿距离:h(n)=D * (abs(n.x-goal.x) + abs(n.y-goal.y))

预计代价：h(n)=终点坐标跟该点坐标的曼哈顿距离=(4,2)跟（1,1）的曼哈顿距离=（|4-1|+|2-1|）*10=40；

总估价:f(n)=g(n)+h(n)=10+40=50

按这样能够分别算出总代价： 50,44,34,44,50

比較大小，44最小，所以向左移动一格到（2,2），并把（2,2）关闭起来增加closeMap并从openMap移除出去，再有（2,2）四周点扩散，发现

(3,3)没增加openMap,然后把（3,3）增加openMap,在迭代openMap里面的元素看哪个最小

发现（2,1），（2,3）总估价都是一样44，随机选一个，我选了（2,1）并增加closeMap,并移除出openMap

每个格子都会记住它上一个格子,上一个格子是它格子周围中到达这个格子花费世界代价最小的那一个

所以坐标（3,3）的上一个格子是（2,2）而不是（2,3）；

坐标（2,3）的上一个格子是（1,2）而不是（2，2);

按这种方式不断的扩散，直到扩散到终点（4,2）;

然后不停的迭代终点的上一个是哪个格子，在迭代上一个的上一个

终于获取最佳路径（1,2）->（2,2）->（3,3）->（4,2）

终于获取最佳路径（1,2）->（2,2）->（3,3）->（4,2）

加了个功能：假设终点在障碍物里面它会找到离终点近期距离的那一点

图二

代码结构

package astar;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Set;

/**
 *
 * @author hjn
 * @version 1.0 2015-03-11
 *
 */
public class AStar implements IMove {

    public static final int MOVE_TETN = 10;
    public static final int MOVE_SIDE = 14;
    public static final int LENGHT = 10;
    /* 打开的列表 */
    Map<String, Point> openMap = new HashMap<String, Point>();
    /* 关闭的列表 */
    Map<String, Point> closeMap = new HashMap<String, Point>();
    /* 障碍物 */
    Set<Point> barrier;
    /* 起点 */
    Point startPoint;
    /* 终点 */
    Point endPoint;
    /* 当前使用节点 */
    Point currentPoint;
    /* 循环次数，为了防止目标不可到达 */
    int num = 0;
    
    Point lastPoint;

    /**
     * 获取点1到点1的最佳路径
     */
    @Override
    public Point move(int x1, int y1, int x2, int y2, Set<Point> barrier) {
        num = 0;
        this.lastPoint=new Point(x2,y2);
        this.barrier = barrier;
        this.startPoint = new Point(x1, y1);
        Point endPoint=new Point(x2,y2);
        this.endPoint = this.getNearPoint(endPoint,endPoint);
        this.closeMap.put(startPoint.getKey(), startPoint);
        this.currentPoint = this.startPoint;
        this.toOpen(x1, y1);
        return endPoint;
    }

    

    /**
     * 求亮点间的估算代价，性能由高到低 启示函数一（曼哈顿距离）： (Math.abs(x1 - x2) + Math.abs(y1 - y2)) *
     * 单位长度 启示函数二（平方的欧几里得距离）：((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 -y1))*
     * 单位长度 启示函数三（欧几里得距离）：(int) Math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) *
     * (y2 -y1))* 单位长度 启示函数四（对角线距离）：Math.max(Math.abs(x1 - x2), Math.abs(y1 -
     * y2)) * 单位长度 不用启示函数：0
     *
     * @param x1
     *            点1x轴
     * @param y1
     *            点1y轴
     * @param x2
     *            点2x轴
     * @param y2
     *            点2y轴
     * @return
     */
    private int getGuessLength(int x1, int y1, int x2, int y2) {
        //return ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 -y1))* AStar.LENGHT;
        return (Math.abs(x1 - x2) + Math.abs(y1 - y2)) * AStar.LENGHT;
        // return Math.max(Math.abs(x1 - x2), Math.abs(y1 - y2)) * AStar.LENGHT;
        // return 0;
    }

    /**
     * 把该节点相邻点增加打开的列表
     *
     * @param x
     * @param y
     */
    private void toOpen(int x, int y) {
        this.addOpenPoint(new Point(x - 1, y), AStar.MOVE_TETN);
        this.addOpenPoint(new Point(x + 1, y), AStar.MOVE_TETN);
        this.addOpenPoint(new Point(x, y - 1), AStar.MOVE_TETN);
        this.addOpenPoint(new Point(x, y + 1), AStar.MOVE_TETN);
        this.addOpenPoint(new Point(x - 1, y - 1), AStar.MOVE_SIDE);
        this.addOpenPoint(new Point(x - 1, y + 1), AStar.MOVE_SIDE);
        this.addOpenPoint(new Point(x + 1, y - 1), AStar.MOVE_SIDE);
        this.addOpenPoint(new Point(x + 1, y + 1), AStar.MOVE_SIDE);
        num++;
        if (num <= 4000) {
            this.toClose(x, y);
        }

    }

    /**
     * 把该节点相邻点增加关闭的列表
     *
     * @param x
     * @param y
     */
    private void toClose(int x, int y) {
        List<Point> list = new ArrayList<Point>(openMap.values());
        Collections.sort(list, new Comparator<Point>() {
            @Override
            public int compare(Point o1, Point o2) {
                if (o1.fTotal > o2.fTotal) {
                    return 1;
                } else if (o1.fTotal < o2.fTotal) {
                    return -1;
                } else {
                    return 0;
                }
            }
        });
        if (list.size() > 0) {
            this.currentPoint = list.get(0);
            closeMap.put(this.currentPoint.getKey(), this.currentPoint);
            openMap.remove(this.currentPoint.getKey());
            if (!currentPoint.equals(endPoint)) {
                this.toOpen(this.currentPoint.x, this.currentPoint.y);
            } else {
                    endPoint = this.currentPoint;
            }
        }
    }

    /**
     * 增加开放的点
     *
     * @param point
     *            点
     * @param gCost
     *            当前点到该点的消耗
     * @return
     */
    private void addOpenPoint(Point point, int gCost) {
        if (point.x < 0 || point.y < 0) {
            return;
        }
        String key = point.getKey();
        if (!barrier.contains(point) && !point.equals(this.currentPoint)) {
            int hEstimate = this.getGuessLength(point.x, point.y,
                    this.endPoint.x, this.endPoint.y);
            int totalGCost = this.currentPoint.gCost + gCost;
            int fTotal = totalGCost + hEstimate;
            if (!closeMap.containsKey(key)) {
                point.hEstimate = hEstimate;
                point.gCost = totalGCost;
                point.fTotal = fTotal;
                Point oldPoint = openMap.get(key);
                if (oldPoint != null) {
                    if (oldPoint.gCost > totalGCost) {
                        oldPoint.fTotal = fTotal;
                        oldPoint.prev = this.currentPoint;
                        openMap.put(key, oldPoint);
                    }
                } else {
                    point.prev = this.currentPoint;
                    openMap.put(key, point);
                }
            } else {
                Point oldPoint = closeMap.get(key);
                if (oldPoint != null) {
                    if ((oldPoint.gCost + gCost) < this.currentPoint.gCost) {
                        if (this.currentPoint.prev != oldPoint) {
                            this.currentPoint.fTotal = oldPoint.fTotal + gCost;
                            this.currentPoint.gCost = oldPoint.gCost + gCost;
                            this.currentPoint.prev = oldPoint;
                        }
                    }
                }
            }
        }
    }
    
    

    Map<String, Point> nearOutMap;

    public Point getNearPoint(Point point,Point point2) {
        if(this.barrier.contains(point)){
            nearOutMap = new HashMap<String, Point>();
            this.endPoint=point;
            this.toNearPoint(point,point2);
            List<Point> nearList = new ArrayList<Point>(nearOutMap.values());
            Collections.sort(nearList, new Comparator<Point>() {
                @Override
                public int compare(Point o1, Point o2) {
                    if (o1.gCost > o2.gCost) {
                        return 1;
                    } else if (o1.gCost < o2.gCost) {
                        return -1;
                    } else {
                        return 0;
                    }
                }
            });
            this.openMap=new HashMap<String,Point>();
            this.closeMap=new HashMap<String,Point>();
            if (nearList.size() > 0) {
                return nearList.get(0);
            }else{
                return point;
            }
        }else{
            return point;
        }
        
    }

    public void toNearPoint(Point point,Point point2) {
        int x = point.x;
        int y = point.y;
        this.addNearOpenPoint(new Point(x - 1, y),point2);
        this.addNearOpenPoint(new Point(x + 1, y),point2);
        this.addNearOpenPoint(new Point(x, y - 1),point2);
        this.addNearOpenPoint(new Point(x, y + 1),point2);
        this.addNearOpenPoint(new Point(x - 1, y - 1),point2);
        this.addNearOpenPoint(new Point(x - 1, y + 1),point2);
        this.addNearOpenPoint(new Point(x + 1, y - 1),point2);
        this.addNearOpenPoint(new Point(x + 1, y + 1),point2);
        if(this.nearOutMap.size()==0){
            List<Point> list = new ArrayList<Point>(openMap.values());
            Collections.sort(list, new Comparator<Point>() {
                @Override
                public int compare(Point o1, Point o2) {
                    int l1 = o1.gCost;
                    int l2 = o2.gCost;
                    if (l1 > l2) {
                        return 1;
                    } else if (l1 < l2) {
                        return -1;
                    } else {
                        return 0;
                    }
                }
            });
            if (list.size() > 0) {
                Point p = list.get(0);
                this.closeMap.put(p.getKey(), p);
                this.openMap.remove(p.getKey());
                this.toNearPoint(list.get(0),point2);
            }
        }
    }

    private void addNearOpenPoint(Point point,Point point2) {
        String key = point.getKey();
        int gCost = this.getGuessLength(point.x, point.y, point2.x,
                point2.y);
        point.gCost = gCost;
        if (this.barrier.contains(point)) {
            if (!this.openMap.containsKey(key)
                    && !this.closeMap.containsKey(key)) {
                this.openMap.put(key, point);
            }
        } else {
            this.nearOutMap.put(key, point);
        }

    }

    public Map<String, Point> getOpenMap() {
        return openMap;
    }

    public void setOpenMap(Map<String, Point> openMap) {
        this.openMap = openMap;
    }

    public Map<String, Point> getCloseMap() {
        return closeMap;
    }

    public void setCloseMap(Map<String, Point> closeMap) {
        this.closeMap = closeMap;
    }

    public Set<Point> getBarrier() {
        return barrier;
    }

    public void setBarrier(Set<Point> barrier) {
        this.barrier = barrier;
    }

    public Point getEndPoint() {
        return endPoint;
    }

    public void setEndPoint(Point endPoint) {
        this.endPoint = endPoint;
    }

    public Point getStartPoint() {
        return startPoint;
    }

    public void setStartPoint(Point startPoint) {
        this.startPoint = startPoint;
    }

}

package astar;

import java.util.Set;

/**
 *
 * @author hjn
 * @version 1.0 2015-3-11
 *
 */
public interface IMove {
	/**
	 * 求点1到点2的合适路线
	 * @param x1 点1x坐标
	 * @param y1 点1y坐标
	 * @param x2 点2x坐标
	 * @param y2 点2y坐标
	 * @param barrier 有顺序的路线列表
	 * @return
	 */
	Point move(int x1,int y1,int x2,int y2,Set<Point> barrier);

}

package astar;

public class Point {
	int x;
	int y;
	int gCost;
	int hEstimate;
	int fTotal;
    Point prev;
    int level=1;

    public String getKey(){
    	return x+"_"+y;
    }
	public Point(int x, int y) {
		super();
		this.x = x;
		this.y = y;
	}

	public Point(int x, int y, int gCost) {
		super();
		this.x = x;
		this.y = y;
		this.gCost = gCost;
	}

	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + x;
		result = prime * result + y;
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		Point other = (Point) obj;
		if (x != other.x)
			return false;
		if (y != other.y)
			return false;
		return true;
	}

}

package astar;

import java.util.HashSet;
import java.util.Map;
import java.util.Set;

import org.junit.Test;

public class TestPoint {
	@Test
	public void test2() {
		AStar aStar = new AStar();
		Set<Point> barrier = new HashSet<Point>();
/*		for (int j = 30; j > 15; j--) {
			for (int i = 20; i < 50; i++) {
				barrier.add(new Point(j, i));

			}
		}*/

	for (int j = 30; j > 15; j--) {
			barrier.add(new Point(j, 20));
		}
	/*
		for (int j = 30; j > 15; j--) {
			barrier.add(new Point(j, 50));
		}
		*/
		for (int i = 20; i < 50; i++) {
			barrier.add(new Point(30, i));
		}

		for (int i = 20; i < 55; i++) {
			barrier.add(new Point(15, i));
		}
		long start = System.currentTimeMillis();
		for (int i = 0; i < 1; i++) {
			aStar = new AStar();
			aStar.move(10, 25, 28, 40, barrier);
		}
		long end = System.currentTimeMillis();
		System.out.println(end - start);
		Set<Point> set = new HashSet<Point>();
		Point endPoint = aStar.getEndPoint();
		Point startPoint = aStar.getStartPoint();
		Map<String, Point> openMap = aStar.getOpenMap();
		Map<String, Point> closeMap = aStar.getCloseMap();
		set = TestPoint.get(endPoint, set);
		/**
		 * 显示最佳路径
		 */
		System.out.println(aStar.getEndPoint().getKey());
		for (int i = 0; i < 70; i++) {
			for (int j = 0; j < 70; j++) {
				Point p = new Point(j, i);
				if (p.equals(aStar.getEndPoint())) {
					System.out.print("o");
				} else if (p.equals(startPoint)) {
					System.out.print("^");
				} else {
					if (set.contains(p)) {
						System.out.print("@");
					} else if (barrier.contains(p)) {
						System.out.print("#");
					} else {
						System.out.print("*");
					}

				}
				System.out.print(" ");
			}
			System.out.println();
		}

		System.out.println("--------------------------------------------------------------------------------------------------------");
		/**
		 * 扫描的范围
		 */
		for (int i = 0; i < 70; i++) {
			for (int j = 0; j < 70; j++) {
				Point p = new Point(j, i);
				if (p.equals(endPoint)) {
					System.out.print("o");
				} else if (p.equals(startPoint)) {
					System.out.print("^");
				} else {
					if (openMap.containsKey(p.getKey())) {
						System.out.print("%");
					} else if (closeMap.containsKey(p.getKey())) {
						System.out.print("@");
					} else if (barrier.contains(p)) {
						System.out.print("#");
					} else {
						System.out.print("*");
					}

				}
				System.out.print(" ");
			}
			System.out.println();
		}

	}

	public static Set<Point> get(Point p, Set<Point> set) {
		if (p != null) {
			set.add(p);
		}
		Point pp = p.prev;
		if (pp != null) {
			TestPoint.get(pp, set);
		} else {
			return set;
		}
		return set;
	}
}