最短路径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;
	}
}