A*/AStar算法（Java实现）

MyNode实体类

package lydia.astar;

public class MyNode implements Comparable<MyNode> {
	private int x;
	private int y;
 	private int g;
	private int h;
	private MyNode parent;
	private boolean reachable;
	private boolean visitable;
	public MyNode(int x,int y) {
		this.x=x;
		this.y=y;
 		this.g=0;
		this.h=0;
		this.reachable=true;
		this.visitable=false;
	}
	public int getF() {
		return g+h;
	}
 
	public int getG() {
		return g;
	}
	public void setG(int g) {
		this.g = g;
	}
	public int getH() {
		return h;
	}
	public void setH(int h) {
		this.h = h;
	}
	public MyNode getParent() {
		return parent;
	}
	public void setParent(MyNode parent) {
		this.parent = parent;
	}
	public boolean isReachable() {
		return reachable;
	}
	public void setReachable(boolean reachable) {
		this.reachable = reachable;
	}

	 
	public int getX() {
		return x;
	}
	public void setX(int x) {
		this.x = x;
	}
	public int getY() {
		return y;
	}
	public void setY(int y) {
		this.y = y;
	}
	@Override
	public int compareTo(MyNode other) {
		return this.getF()-other.getF();
	}
}

 MyMap类

package lydia.astar;

public class MyMap {
private MyNode[][] myMap;
private MyNode start;
private MyNode end;
public   MyMap(int[][] map) {	
	myMap=new MyNode[map.length][map[0].length];
	//地图 -1 代表墙壁， 1代表起点，2代表终点
	for(int i=0;i<map.length;i++) {
		for(int j=0;j<map[i].length;j++) {
			myMap[i][j]=new MyNode(i,j);
			if(map[i][j]==1) {
				start=myMap[i][j];
			}else if(map[i][j]==2) {
				end=myMap[i][j];
			}else if(map[i][j]==-1) {
				 myMap[i][j].setReachable(false);;
			}
		}
	}
}

public MyNode GetNeighbor(MyNode curr, int i) {
	int[][] neighborIdx= {{-1,0},{0,-1},       {0,1},{1,0},{-1,-1},{-1,1},
			
			{1,-1},{1,1}
			};
	
	int nx=curr.getX()+neighborIdx[i][0];
	int ny=curr.getY()+neighborIdx[i][1];
	
	if(nx<0||nx>=myMap.length || ny<0|| ny>=myMap[nx].length) {
		return null;
	}
	return myMap[nx][ny];
}
public MyNode GetMapCell(int x,int y) {
	return myMap[x][y];
}
public MyNode getStart() {
	return start;
}
public MyNode getEnd() {
	return end;
}

}

MyAStar类

package lydia.astar;

import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;

public class MyAStar {
	
private boolean IsValid(MyNode node) {
	if(node==null || !node.isReachable()) {
		return false;
	}
	return true;
}
private int CalDist(MyNode node1,MyNode node2) {
	return Math.abs(node1.getX()-node2.getX())+Math.abs(node1.getY()+node2.getY());
}
private int Walk(MyNode node) {
	return node.getG()+1;
}
public MyNode Search(int[][] src) {
	//初始化地图
	MyMap myMap=new MyMap(src);
	
	//open列表
	PriorityQueue<MyNode> openList=new PriorityQueue<MyNode>();
	List<MyNode> closeList=new LinkedList<MyNode>();
	//添加首节点
	MyNode start=myMap.getStart();
	MyNode end=myMap.getEnd();
	start.setG(0);
	start.setH(CalDist(start,end));
 	openList.add(start);
	while(!openList.isEmpty()) {
		MyNode curr=openList.poll();
		closeList.add(curr);
		
		for(int i=0;i<4;i++) {
			MyNode neighbor=myMap.GetNeighbor(curr, i);
			if(neighbor==end) {
				neighbor.setParent(curr);
				neighbor.setG(Walk(curr   ));
				neighbor.setH(CalDist(  neighbor,end));
				return neighbor;
			}
			if(!IsValid(neighbor)) {
				continue;
			}
			//节点在close里面
			if(closeList.contains(neighbor)) {
				continue;
			}
			//节点在open里面
			if(openList.contains(neighbor)) {
				if( Walk(curr   )<neighbor.getG() ) {
					neighbor.setParent(curr);
					neighbor.setG(Walk(curr   ) );
					neighbor.setH(CalDist(  neighbor,end));
				}
			}else {
				neighbor.setParent(curr);
				neighbor.setG(Walk(curr   ) );
				neighbor.setH(CalDist(  neighbor,end));
				openList.add(neighbor);
			}
			
		}
		
	}
	return null;
}


}

主函数

package lydia.astar;

public class Main {
	private static void FindPath(MyNode node,int[][] src) {
		while(node!=null) {
			if(src[node.getX()][node.getY()]!=1&&src[node.getX()][node.getY()]!=2) {
			src[node.getX()][node.getY()]=11;}
			node=node.getParent();
		}
		
	}
	private static  void PrintPah(int[][] map) {
		 for (int i = 0; i < 10; i++)
	     {
	         for (int j = 0; j < 10; j++)
	         {
	        	 if(map[i][j]==-1) {
		             System.out.printf(" * " );
	        	 }else if(map[i][j]==11) {
		             System.out.printf(" + " );
	        	 }else if(map[i][j]==1) {
		             System.out.printf(" S " );
	        	 }else if(map[i][j]==2) {
		             System.out.printf(" E " );
	        	 }else {
		             System.out.printf(" 0 " );
	        	 }
	             
	         }
	         System.out.println();
	     }
	}
	public static void main(String[] args) {
		// TODO Auto-generated method stub
		int[][] map = {
                {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
                {-1,  0,  0,  0,  0,  0,  0,  0,  0, -1},
                {-1,  0,  0,  0,  0, -1,  0,  0,  0, -1},
                {-1,  0,  0,  0, -1,  0,  0,  0,  0, -1},
                {-1,  0,  1,  0, -1,  0,  0,  2,  0, -1},
                {-1,  0,  0,  0,  0, -1,  0,  0,  0, -1},
                {-1,  0,  0,  0, -1,  0,  0,  0,  0, -1},
                {-1,  0,  0,  0,  0, -1,  0,  0,  0, -1},
                {-1,  0,  0,  0,  0,  0,  0,  0,  0, -1},
                {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
        };
		System.out.println("*******************Init*****************");
		PrintPah(map);
		System.out.println("*******************Search*****************");
		MyAStar astar=new MyAStar();
		MyNode path= astar.Search(map);
		
		FindPath(path,map);
		PrintPah(map);
	}

}