A*算法

来源参考 http://blog.csdn.net/crayondeng/article/details/12342989

假设有人想从A点移动到一墙之隔的B点，如下图，绿色的是起点A，红色是终点B，蓝色方块是中间的墙。

选择路径中经过哪个方格的关键是下面这个等式：

F = G + H

这里：
    * G = 从起点A，沿着产生的路径，移动到网格上指定方格的移动耗费。
    * H = 从网格上那个方格移动到终点B的预估移动耗费。这经常被称为启发式的

参考java代码：

package com.java;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;

public class TestAStar {

    /**
     * A*方法总结
     * 
     * 好，现在你已经看完了整个说明，让我们把每一步的操作写在一起：
     * 
     * 1，把起始格添加到开启列表。 2，重复如下的工作： a) 寻找开启列表中F值最低的格子。我们称它为当前格。 b) 把它切换到关闭列表。 c)
     * 对相邻的8格中的每一个？ 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
     * 如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
     * 如果它已经在开启列表中，用G值为参考检查新的路径是否更好
     * 。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值
     * 。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
     * 
     * d) 停止，当你 把目标格添加进了关闭列表(注解)，这时候路径被找到，或者 没有找到目标格，开启列表已经空了。这时候，路径不存在。
     * 3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是你的路径 F = G + H
     */

    /**
     * @param args
     */
    public static void main(String[] args) {
        TestAStar ts = new TestAStar();
        ts.initBarrierList();
        ts.startAstarroad(new PointHolder(1, 2), new PointHolder(5, 2));

    }

    int sp = 10;
    int spA = 14;

    int hang = 5;
    int lie = 7;

    ArrayList<PointHolder> barrierList = new ArrayList<PointHolder>();

    public void initBarrierList() {
        PointHolder ph = new PointHolder(3, 1);
        barrierList.add(ph);
        ph = new PointHolder(3, 2);
        barrierList.add(ph);
        ph = new PointHolder(3, 3);
        barrierList.add(ph);
    }

    ArrayList<PointHolder> openList = new ArrayList<PointHolder>();
    ArrayList<PointHolder> closeList = new ArrayList<PointHolder>();

    public void startAstarroad(PointHolder srcph, PointHolder desph) {

        // 当前点
        PointHolder curph = null;
        // 加入open
        openList.add(srcph);

        while (true) {

            if (openList.isEmpty()) {
                System.out.println("not find the point");
                return;
            }

            if (closeList.indexOf(desph) > 0) {
                System.out.println("find");
                int ch = closeList.indexOf(desph);
                closeList.get(ch).printPath();
                return;
            }
            // 排序
            Collections.sort(openList, new ComparatorPh(desph));
            // 找最小的当前点
            if (openList.size() > 0) {
                curph = openList.get(0);
            }
            
            // 当前点加入close
            closeList.add(curph);
            openList.remove(0);
            // 打印
            curph.toStringPoint();
            // 找周围点
            ArrayList<PointHolder> shipphs = getCatShipPh(curph);
            for (PointHolder shipph : shipphs) {
                shipph.prePoint = curph;
            }
            // 周围点在openlist中，比较最优H 加入open list
            for (PointHolder shipph : shipphs) {
                int ch = openList.indexOf(shipph);
                if (ch >= 0) {
                    int shipph_H = shipph.getH(desph);
                    int openph_H = openList.get(ch).getH(desph);
                    if (shipph_H < openph_H) {
                        openList.get(ch).prePoint = shipph.prePoint;
                    }
                } else {
                    openList.add(shipph);
                }
            }

        }

    }

    // 得到可到达的点的集合
    ArrayList<PointHolder> getCatShipPh(PointHolder ph) {
        ArrayList<PointHolder> mList = new ArrayList<TestAStar.PointHolder>();
        int x = ph.x;
        int y = ph.y;
        boolean uEnable = false;// 上是否可用
        boolean dEnable = false;
        boolean lEnable = false;
        boolean rEnable = false;
        // 上
        PointHolder ph1 = new PointHolder(x, y - 1);
        if (validatePointHolder(ph1)) {
            uEnable = true;
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 下
        ph1 = new PointHolder(x, y + 1);
        if (validatePointHolder(ph1)) {
            dEnable = true;
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 左
        ph1 = new PointHolder(x - 1, y);
        if (validatePointHolder(ph1)) {
            lEnable = true;
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 右
        ph1 = new PointHolder(x + 1, y);
        if (validatePointHolder(ph1)) {
            rEnable = true;
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 左上
        ph1 = new PointHolder(x - 1, y - 1);
        if (validatePointHolder(ph1) && uEnable && lEnable) {
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 右上
        ph1 = new PointHolder(x + 1, y - 1);
        if (validatePointHolder(ph1) && uEnable && rEnable) {
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 左下
        ph1 = new PointHolder(x - 1, y + 1);
        if (validatePointHolder(ph1) && lEnable && dEnable) {
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        // 右下
        ph1 = new PointHolder(x + 1, y + 1);
        if (validatePointHolder(ph1) && rEnable && dEnable) {
            if (!closeList.contains(ph1))
                mList.add(ph1);
        }
        return mList;
    }

    /***
     * 校验 是否符合
     * 
     * @param ph
     * @return
     */
    boolean validatePointHolder(PointHolder ph) {
        if (ph == null)
            return false;
        // 是否范围内
        if (ph.x < 0 || ph.x >= 7 || ph.y < 0 || ph.y >= 5)
            return false;
        // 是否不可用
        if (barrierList.contains(ph))
            return false;
        return true;
    }

    public static class PointHolder {

        public PointHolder(int x, int y) {
            this.x = x;
            this.y = y;
        }

        public int x;
        public int y;
        // 前一个节点
        PointHolder prePoint;
        public int H;

        public int getH(PointHolder desPh) {

            return Math.abs(desPh.x - x) + Math.abs(desPh.y - y);
        }

        public int getG() {
            PointHolder ph = this;
            int g = 0;
            while (ph != null) {
                g++;
                ph = ph.prePoint;
            }
            return g;
        }

        @Override
        public boolean equals(Object obj) {
            PointHolder ph = (PointHolder) obj;
            if (ph.x == x && ph.y == y) {
                return true;
            } else {
                return false;
            }
        }

        public void toStringPoint() {
            System.out.println("P(" + x + "," + y + ")");
        }

        public void printPath() {
            ArrayList<PointHolder> mList = new ArrayList<TestAStar.PointHolder>();
            PointHolder ph = this;
            while (ph != null) {
                mList.add(ph);
                ph = ph.prePoint;
            }

            for (int i = 0; i < mList.size(); i++) {
                mList.get(i).toStringPoint();
            }

        }
    }

    public class ComparatorPh implements Comparator<PointHolder> {
        PointHolder desPh;

        public ComparatorPh(PointHolder desPh) {
            this.desPh = desPh;
        }

        @Override
        public int compare(PointHolder o1, PointHolder o2) {
            int f1 = o1.getG() + o1.getH(desPh);
            int f2 = o2.getG() + o1.getH(desPh);
            return f1 - f2;
        }

    }

}