A*算法之最短路径（java代码）

图解

 

代码

比较接口

package com.hyh.java_algorithm;

public interface MyCompare {

    public boolean isLarger(MyCompare m2);

    public boolean isSmaller(MyCompare m2);

    public boolean isEqual(MyCompare m2);
}

最小堆

package com.hyh.java_algorithm;

/**
 * 最小堆
 * @author hyh
 * 2015-5-7上午11:08:20
 */
public class MinHeap<E extends MyCompare> {
    private int size;
    private Object[] element;

    public MinHeap(int maxSize){
        size = 0;
        element = new Object[maxSize];
    }
    public MinHeap(){
        this(10);
    }

    /**
     * 元素入堆
     * @param e
     */
    public void append(E e){
        ensureCapacity(size+1);
        element[size++] = e;///put the element to the end of the heap

        adjustUp(); //adjust the heap to minHeap
    }
    /**
     * 取出堆顶元素（最小元素）
     * @return
     */
    @SuppressWarnings("unchecked")
    public E poll(){
        if(isEmpty()){
            return null;
        }

        E min = (E) element[0];
        element[0] = element[size-1];///replace the min element with the last element
        element[size-1] = null ;///let gc do its work
        size--;

        adjustDown();///adjust the heap to minHeap

        return min;
    }
    /**
     * 查看堆顶元素（最小元素）
     * @return
     */
    @SuppressWarnings("unchecked")
    public E  peek(){
        if(isEmpty()){
            return null;
        }
        return (E) element[0];
    }
    /**
     * 是否为空堆
     * @return
     */
    public boolean isEmpty(){
        return size == 0 ;
    }

    /**
     * 确保容量空间足够
     * @param minCapacity
     */
    private void ensureCapacity(int minCapacity){
        int oldCapacity = element.length;
        if(minCapacity > oldCapacity){
            int newCapacity = (oldCapacity*3)/2+1;///每次扩容至1.5倍
            Object[] copy = new Object[newCapacity];
            ///调用本地C方法进行数组复制
            System.arraycopy(element, 0, copy, 0, element.length);

            element = copy;
        }
    }

    /**
     * 向上调整为堆，将小值往上调
     */
    @SuppressWarnings("unchecked")
    private void adjustUp(){

        E temp = (E) element[size-1]; ///get the last element
        int parent = size - 1;
        while(parent>0&&((E)element[(size - 1)/2]).isLarger(temp)){
            ///if smaller than it parent
            element[parent] = element[(parent - 1)/2];
            parent = (parent - 1)/2;
        }
        element[parent] = temp;
    }

    /**
     * 向下调整为堆
     */
    @SuppressWarnings("unchecked")
    private void adjustDown(){
        E temp = (E) element[0]; ///get the first element
        int child = 1;
        while(child<size){
            E left = (E) element[child];
            E right = (E) element[child+1];///这里的child+1不会越界（想想为什么）
            if(right!=null&&left.isLarger(right)){
                child++;
            }

            if(temp.isSmaller((E)element[child])){
                break; 如果比两个孩子中较小者都还小，则结束
            }

            element[(child-1)/2] = element[child]; ///assign the smaller to its parent
            child = child*2 + 1;
        }

        element[(child-1)/2] = temp;
    }
}

比较对象（格子）

package com.hyh.java_algorithm;

/**
 * pojo ，格子
 * <pre> F = G + H
 * G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动,斜方向的代价为对角线长度)
 * H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动).</pre>
 * @author hyh
 * 
 */
public class Grid implements MyCompare{

    private double F;
    private double H;
    private double G;

    private int i ;
    private int j;

    private Grid parent; ///该格子的父格子

    /**
     * pojo ，格子
     * @param F F = G + H
     * @param G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动,斜方向的代价为对角线长度)
     * @param H	表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动).
     * @param i 纵坐标i
     * @param j 横坐标j
     * @param parent  父结点
     */
    public Grid(double F,double G,double H,int i,int j,Grid parent){
        this.F = F;
        this.G = G;
        this.H = H;
        this.i = i;
        this.j = j;
        this.parent = parent;
    }
    public Grid(){}



    public Grid getParent() {
        return parent;
    }
    public void setParent(Grid parent) {
        this.parent = parent;
    }

    public int getI() {
        return i;
    }
    public int getJ() {
        return j;
    }
    public void setI(int i) {
        this.i = i;
    }
    public void setJ(int j) {
        this.j = j;
    }


    /**
     * 经过当前点到终点B的总耗费  期望值
     * @return
     */
    public double getF() {
        return F;
    }

    /**
     * H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动)
     * @return
     */
    public double getH() {
        return H;
    }
    /**
     * 表示从起点 A 移动到当前网格上的移动耗费 (可沿斜方向移动,斜方向的代价为对角线长度)
     * @return
     */
    public double getG() {
        return G;
    }



    public void setF(double f) {
        F = f;
    }
    public void setH(double h) {
        H = h;
    }
    public void setG(double g) {
        G = g;
    }

    @Override
    public boolean isLarger(MyCompare m2) {
        // TODO Auto-generated method stub
        return this.F>((Grid)m2).getF();
    }
    @Override
    public boolean isSmaller(MyCompare m2) {
        // TODO Auto-generated method stub
        return this.F<((Grid)m2).getF();
    }
    @Override
    public boolean isEqual(MyCompare m2) {
        // TODO Auto-generated method stub
        return this.F==((Grid)m2).getF();
    }

}

A*寻路算法

package com.hyh.java_algorithm;

/**
 * A*寻路算法
 * <pre>
 * 思路：	每次取期望值最小的位置作为下一步要走的位置，F = G + H
 *  	G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动,斜方向的代价为对角线长度).
 *  	H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动).
 *
 *  	此处用一个最小堆来记录开启列表中的格子，每个格子有一个指向父格子的指针，以此记录路劲 </pre>
 * @author hyh
 * 
 */
public class AStar {

    private static MinHeap<Grid> open ;//= new MinHeap<Grid>();
    //	private static MTree close ;//= new MTree();
    private Grid last; //记录最后一个格子

    private final String obstacle = "1";//障碍物标记值
    private String end = "e";	目标标记值
    private String start = "s";开始标记值
    //目标坐标
    private int end_i = -1;
    private int end_j = -1;
    //开始目标
    private int start_i = -1;
    private int start_j = -1;

    /**
     * 初始化操作
     * @param boxs
     */
    public void init(String[][] boxs){
        for(int i=0;i<boxs.length;i++){
            for(int j=0;j<boxs[0].length;j++){
                if(boxs[i][j].equals(start)){
                    start_i = i;
                    start_j = j;
                }
                if(boxs[i][j].equals(end)){
                    end_i = i;
                    end_j = j;
                }
            }
        }

        Grid sGrid = new Grid(0, 0, 0, start_i, start_j, null);
        open = new MinHeap<Grid>();
        open.append(sGrid);///、将开始位置加入开集
    }
    /**
     * 开始搜索
     */
    public void search(String[][] boxs){
        int height = boxs.length;
        int width = boxs[0].length;
        while(open.peek()!=null){//对开集进行遍历，直到找到目标或者找不到通路
            Grid g = open.poll();
            int i = g.getI();
            int j = g.getJ();
            double pre_G = g.getG();///已耗费
            for(int h=-1;h<=1;h++){
                for(int w=-1;w<=1;w++){

                    int next_i = i + h;	///下一个将加入open 集的格子的i
                    int next_j = j + w;///下一个将加入open 集的格子的j

                    if(next_i>=0 && next_i<=height-1 && next_j>=0 && next_j<=width-1){
                        数组不越界，则进行计算
                        if(boxs[next_i][next_j].equals(obstacle) || boxs[next_i][next_j].equals("-1") ||(h==0&&w==0)){
                            //如果该格子是障碍，或者格子本身，跳过
                            continue;
                        }
                        计算该点到终点的最短路劲
                        double H =  Math.abs(end_i - next_i) + Math.abs(end_j - next_j) ;
                        if(H<1){
                            ///找到目标,记录并结束
                            last = new Grid(0, pre_G, 0, next_i, next_j,g); ;
                            return ;
                        }
                        如果是对角线则加1.4，否则加1
                        double G = Math.sqrt((next_i-i)*(next_i-i)+(next_j-j)*(next_j-j))>1 	? 	pre_G+1.4 	: 	pre_G+1;
                        //生成新格子
                        Grid temp = new Grid(H+G, G, H, next_i, next_j,g);
                        加入open集
                        open.append(temp);
                        boxs[i][j] = "-1";///表示此处已经计算过了
                    }
                }
            }

            last = g;
        }


    }

    /**
     * 打印路劲
     */
    public void printPath(){

        if(end_i!=last.getI()||end_j!=last.getJ()){
            System.out.println("无法到达终点！");
            return ;
        }

        System.out.println("路劲逆序为:");
        while(true){
            System.out.print("("+last.getI()+","+last.getJ()+")");
            last = last.getParent();
            if(last==null){
                break;
            }
            System.out.print(" <———");
        }
    }

    /**
     * @param args
     */
    public static void main(String[] args) {
        // TODO Auto-generated method stub

        String[][] boxs = {
                {"0","0","1","0","0"},
                {"0","0","1","e","0"},
                {"0","0","1","1","0"},
                {"0","0","0","1","0"},
                {"s","0","1","0","0"},
        };

        AStar star = new AStar();
        star.init(boxs);
        star.search(boxs);
        star.printPath();
    }

}