使用Java编写A*路径规划算法

在Test中调用AstarPathPlan构造方法，Map类的editObstacle方法，可以编辑地图；

AstarPathPlan的pathPlanning方法执行A*算法，结果保存至堆栈；

AstarPathPlan的printResult用于打印出路径。

请各位大大提宝贵意见

package com.bit.pathplanning;
/**
 * 地图网格节点类
 * @author Hao
 *
 */
public class MapNode {
	private double x;
	private double y;
	private double G;
	private double F;
	private double H;
	private MapNode fatherNode;
	private boolean reachable;
	public MapNode(){
	}
	
	public MapNode(double x,double y){
		this.x = x;
		this.y = y;
		G = 0;
		F = 0;
		H = 0;
		fatherNode = null;
		reachable = true;
	}

	@Override
	public String toString() {
		return "MapNode [x=" + x + ", y=" + y + ", reachable=" + reachable + "]";
	}

	public double getX() {
		return x;
	}

	public double getY() {
		return y;
	}

	public boolean isReachable() {
		return reachable;
	}

	public void setReachable(boolean reachable) {
		this.reachable = reachable;
	}

	public MapNode getFatherNode() {
		return fatherNode;
	}

	public void setFatherNode(MapNode fatherNode) {
		this.fatherNode = fatherNode;
		this.F = fatherNode.F + this.distanceTo(fatherNode);
		this.G = this.F + this.H;
	}
	
	/**
	 * 计算H值
	 */
	public double calH(MapNode fatherNode){
		double distance = 0.0;
		if(fatherNode.x == this.x)
			distance = Math.abs(fatherNode.y - this.y);
		else if(fatherNode.y == this.y)
			distance = Math.abs(fatherNode.x - this.x);
		else
			distance = 1.414*Math.abs(fatherNode.y - this.y);
		return fatherNode.H + distance;
	}