java实现A*算法

以下引自 http://hi.baidu.com/%BA%DA%B5%C4%B7%A2%D7%CF/blog/item/60e3483dce5bb8c29e3d62e0.html

 

我们将以下图作为地图来进行讲解,图中对每一个方格都进行了编号,其中绿色的方格代表起点,红色的方格代表终点,蓝色的方格代表障碍,我们将用A星算法来寻找一条从起点到终点最优路径,为了方便讲解,本地图规定只能走上下左右4个方向,当你理解了A星算法,8个方向也自然明白

 

在地图中,每一个方格最基本也要具有两个属性值,一个是方格是通畅的还是障碍,另一个就是指向他父亲方格的指针(相当于双向链表结构中的父结点指针),我们假设方格值为0时为通畅,值为1时为障碍

A星算法中,有2个相当重要的元素,第一个就是指向父亲结点的指针,第二个就是一个OPEN表,第三个就是CLOSE表,这两张表的具体作用我们在后面边用边介绍,第四个就是每个结点的F值(F值相当于图结构中的权值)

而F = H + G;其中H值为从网格上当前方格移动到终点的预估移动耗费。这经常被称为启发式的，可能会让你有点迷惑。这样叫的原因是因为它只是个猜测。我们没办法事先知道路径的长度，因为路上可能存在各种障碍(墙，水，等等)。虽然本文只提供了一种计算H的方法，但是你可以在网上找到很多其他的方法,我们定义H值为     终点所在行减去当前格所在行的绝对值   与   终点所在列减去当前格所在列的绝对值 之和,而G值为从当前格的父亲格移动到当前格的预估移动耗费,在这里我们设定一个基数10,每个H和G都要乘以10,这样方便观察

好了,我们开始对地图进行搜索

首先,我们将起点的父亲结点设置为NULL,然后将起点的G值设置为0,再装进open表里面,然后将起点作为父亲结点的周围4个点20,28,30,38(因为我们地图只能走4个方向,如果是8方向,则要加个点进去)都加进open列表里面,并算去每个结点的H值,然后再将起点从open列表删除,放进close表中,我们将放进close表的所有方格都用浅蓝色线条进行框边处理,所以这次搜索以后,图片变为如下格式,其中箭头代表的是其父结点

 

其中每个格子的左下方为G值,右下方为H值,左上方为H值,我们拿28号格子为例来讲解一写F值的算法,首先因为终点33在4行7列,而28在4行2列,则行数相差为0,列数相差为5,总和为5,再乘以我们先前定的基数10,所以H值为50,又因为从28的父结点29移动到28,长度为1格,而29号为起点，G值为0,所以在父亲结点29的基础上移动到28所消耗的G值为(0 + 1) *10 = 10,0为父亲结点的G值,1为从29到28的消耗

当前OPEN表中的值: 20,28,30,38     当前CLOSE表中的值: 29

现在我们开始寻找OPEN列表中F值最低的,得出结点30的F值最低,且为40,然后将结点30从OPEN表中删除,然后再加入到CLOSE表中,然后在判断结点30周围4个结点,因为结点31为障碍,结点29存在于CLOSE表中,我们将不处理这两点,只将21和39号结点加入OPEN表中,添加完后地图变为下图样式

当前OPEN表中的值: 20,28,38,21,39   当前CLOSE表中的值: 29,30

接着我们重复上面的过程,寻找OPEN表中F值为低的值,我们发现OPEN表中所有结点的F值都为60,我们随即取一个结点,这里我们直接取最后添加进OPEN表中的结点,这样方便访问(因为存在这样的情况,所有从一个点到另外一个点的最短路径可能不只一条),我们取结点39,将他从OPEN表中删除,并添加进CLOSE表中,然后观察39号结点周围的4个结点,因为40号结点为障碍,所以我们不管它,而30号结点已经存在与OPEN表中了,所以我们要比较下假设39号结点为30号结点的父结点,30号结点的G值会不会更小,如果更小的话我们将30结点的父结点改为39号,这里我们以39号结点为父结点,得出30号结点的新G值为20,而30号结点原来的G值为10,并不比原来的小,所以我们不对30号进行任何操作,同样的对38号结点进行上述操作后我们也不对它进行任何操作,接着我们把48号结点添加进OPEN表中,添加完后地图变为下图样式

当前OPEN表中的值: 20,28,38,21,48   当前CLOSE表中的值: 29,30,39

 

 

以下为java代码实现

public class Point {

	public int x;
	public int y;
	public Point parent;
	public boolean equal(Point p){
		return x == p.x&&y==p.y;
	}
	public Point(int x,int y){
		this.x = x;
		this.y = y;
	}
	public String toString(){
		return "("+x+","+y+")";
	}
}

 

public class Map2D {

	private int points[][];
	public Map2D(int[][] p){
		points = p;
	}
	
	public int getPointData(int x,int y){
		return points[y][x];
	}
	public int getPointData(Point p){
		return points[p.y][p.x];
	}
	public int getXLength(){
		return points[0].length;
	}
	public int getYLength(){
		return points.length;
	}
	public void setPoints(int ps[][]){
		points = ps;
	}
}

 

public class AStart {

	private Point startLoca; //开始位置
	private Point dest;			//结束位置
	private Map2D map;			//地图
	private List openTable = new LinkedList(); //open表，用于存放候选位置
	private List closeTable = new LinkedList();	//close表，用于存放已经超过的位置
	private Point cur ;				//当前位置
	public AStart(Map2D map){
		this.map = map;
	}
	public static void main(String[] args){
		Map2D m = new Map2D(new int[][]{
				{0,0,0,0,0,0,0,0,0,0},
				{0,0,0,0,0,0,0,0,0,0},
				{0,0,1,1,1,1,0,0,0,0},
				{0,0,0,0,0,1,0,0,0,0},
				{0,0,0,0,0,1,0,0,0,0},
				{0,0,0,0,0,1,0,0,0,0},
				{0,0,1,1,1,1,0,0,0,0},
				{0,0,0,0,0,0,0,0,0,0},
				{0,0,0,0,0,0,0,0,0,0}
		});
		AStart as = new AStart(m);
		as.setStartLoca(new Point(2,4));
		as.setDest(new Point(8,4));
		System.out.println(as.findBestPath());
	}
	/**
	 * 
	 * 向l中加入p,当前点和当前点的父结点形成的链表组成了最短路径 
	 * 
	 * @param l
	 * @param p
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:27:21 PM
	 */
	public void addPath(List l,Point p){
		if(p.parent!= null){
			addPath(l,p.parent);
		}
		l.add(p);
	}
	/**
	 * 
	 * 找到最短路径 
	 * 
	 * @return
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:29:21 PM
	 */
	public List findBestPath(){
		cur = startLoca;
		closeTable.add(startLoca);
		while(!cur.equal(dest)){
			Point sur[] = this.getSurroundPoints(cur) ;
			if(sur.length==0){
				this.removePoint(openTable, cur.x, cur.y);
				cur = this.getBestPoint();
			}
			else{
				for(Point c:sur){
					//计算f_func;
					openTable.add(c);
					c.parent = cur;
				}
				openTable.remove(cur);
				closeTable.add(cur);
				cur = this.getBestPoint();
			}
		}
		List rt = new ArrayList();
		this.addPath(rt, cur);
		clearMemory();
		return rt;
	}
	/**
	 * 
	 * 清除内存空间 
	 * 
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:29:52 PM
	 */
	public void clearMemory(){
		for(Point p:openTable){
			p.parent = null;
		}
		openTable.clear();
		openTable = null;
		for(Point p:closeTable){
			p.parent = null;
		}
		closeTable.clear();
		closeTable = null;
	}
	/**
	 * 
	 * 在候选点表(open)中找到消耗最小的点 
	 * 
	 * @return
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:30:20 PM
	 */
	public Point getBestPoint(){
		Point rt = null;
		int k = 0;
		for(Point p:openTable){
			if(k == 0 &&f_func(p)>=0){
				rt = p;
				k++;
			}
			if(f_func(p)>= 0 &&map.getPointData(p) l,int x,int y){
		for(Point p:l){
			if(p.x == x&&p.y==y){
				p.parent = null;
				l.remove(p);
				return;
			}
		}
	}
	/**
	 * 
	 * 查询l中是否存在点(x,y) 
	 * 
	 * @param l
	 * @param x
	 * @param y
	 * @return
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:31:43 PM
	 */
	public boolean hasPoint(List l,int x,int y){
		for(Point p:l){
			if(p.x == x&&p.y==y){
				return true;
			}
		}
		return false;
	}
	/**
	 * 
	 * 在l中找到(x,y)点，并返回 
	 * 
	 * @param l
	 * @param x
	 * @param y
	 * @return
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:32:09 PM
	 */
	public Point getPoint(List l,int x,int y){
		for(Point p:l){
			if(p.x == x&&p.y==y){
				return p;
			}
		}
		return null;
	}
	/**
	 * 
	 * 得到loc点周围的可选点 
	 * 
	 * @param loc
	 * @return
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:32:31 PM
	 */
	public Point[] getSurroundPoints(Point loc){
		List l = new ArrayList();
		int lx = map.getXLength();
		int ly = map.getYLength();
		int tx = loc.x+1;
		int ty = loc.y;
		/**
		 * 只有上下左右四个方向的点
		 */
		if(tx=0){
			if(map.getPointData(tx, ty)==0){
				if(!this.hasPoint(openTable, tx, ty) && !this.hasPoint(closeTable, tx, ty)){ 
					l.add(new Point(tx,ty));
				}
			}
		}
		tx = loc.x;
		ty = loc.y+1;
		if(ty=0){
			if(!this.hasPoint(openTable, tx, ty) && map.getPointData(tx, ty)==0){
				if(!this.hasPoint(closeTable, tx, ty)){ 
					l.add(new Point(tx,ty));
				}
			}
		}
		Point[] rt = new Point[l.size()];
		return l.toArray(rt);
	}
	public Point getStartLoca() {
		return startLoca;
	}
	
	public int h_func(Point p){
		return h_func(p.x,p.y);
	}
	public int f_func(Point p){
		
		return h_func(p)+g_func(p);
	}
	public int g_func(Point p){
		return g_func(p.x,p.y);
	}
	public int h_func(int x,int y){
		return (int)Math.sqrt((x-dest.x)*(x-dest.x)+(y-dest.y)*(y-dest.y))*10;
		
	}
	/**
	 * 
	 * f函数 
	 * 
	 * @param x
	 * @param y
	 * @return
	 * @author      YitianC 
	 * @history 
	 *              YitianC Apr 24, 2012 3:33:04 PM
	 */
	public int f_func(int x,int y){
		return h_func(x,y)+g_func(x,y);
	}
	public int g_func(int x,int y){
		return (int)Math.sqrt((x-cur.x)*(x-cur.x)+(y-cur.y)*(y-cur.y))*10;
	}
	public void setStartLoca(Point startLoca) {
		this.startLoca = startLoca;
	}
	public Point getDest() {
		return dest;
	}
	public void setDest(Point dest) {
		this.dest = dest;
	}
	public Map2D getMap() {
		return map;
	}
	public void setMap(Map2D map) {
		this.map = map;
	}
}

 

输出

[(2,4), (1,4), (1,5), (1,6), (1,7), (2,7), (3,7), (4,7), (5,7), (6,7), (7,7), (8,7), (8,6), (8,5), (8,4)]