CODE:

import java.util.*;

public class Main {
	static int map[][] = {
			{0,0,0,0,0},
			{0,1,0,1,0},
			{0,1,0,1,0},
			{0,1,0,1,0},
			{0,0,0,0,0},
	};
	public static void main(String[] args) {
		Vector start = new Vector(0,0);
		Vector goal = new Vector(2,2);
		Main m = new Main(map, start, goal);
		System.out.println(m.getRoute());
	}

	private int mapData[][];
	private int map_h;
	private int map_w;

	private ArrayList open;
	private ArrayList close;
	private ArrayList result;

	public Main(int[][] map, Vector start, Vector goal){
		mapData = map;
		open = new ArrayList();
		close = new ArrayList();
		result = new ArrayList();
		map_h = mapData.length-1;
		map_w = mapData[0].length-1;

		open.add(start);
		explore(start, goal);

		Vector v = open.get(indexof(open, goal));
		while(v != null){
			result.add(v);
			v = v.mom;
		}
	}

	public List getRoute(){
		return result;
	}

	public boolean explore(Vector start, Vector goal){
		if(open.size() == 0) return false;
		int[] fn = new int[open.size()];

		for(int i=0; i fn[i]) min = i;
		}
		Vector n = open.get(min);

		if(n.x == goal.x && n.y == goal.y) return true;
		else{
			open.remove(n);
			close.add(n);
		}

		ArrayList ms = new ArrayList();
		if(n.x != 0 && mapData[n.y][n.x-1] == 0) 	ms.add(new Vector(n.x-1, n.y));
		if(n.x != map_w && mapData[n.y][n.x+1] == 0) ms.add(new Vector(n.x+1, n.y));
		if(n.y != 0 && mapData[n.y-1][n.x] == 0) 	ms.add(new Vector(n.x, n.y-1));
		if(n.y != map_h && mapData[n.y+1][n.x] == 0) ms.add(new Vector(n.x, n.y+1));

		for(Vector m : ms){
			int fdn = getG(n,start) + getH(m,goal) + 1;

			if(contains(open, m)){
				if(fdn  list, Vector target){
		for(Vector v : list){
			if(v.x == target.x && v.y == target.y) return true;
		}
		return false;
	}

	private int indexof(List list, Vector target){
		for(int i=0; i<list.size(); i++){
			Vector v = list.get(i);
			if(v.x == target.x && v.y == target.y) return i;
		}
		return -1;
	}

	private int getF(Vector now, Vector start, Vector goal){
		int gn = getG(now, start);
		int hn = getH(now, goal);
		return gn + hn;
	}

	private int getG(Vector now, Vector start){
		return (int)Math.sqrt((now.x - start.x)*(now.x - start.x)+(now.y - start.y)-(now.y - start.y));
	}

	private int getH(Vector now, Vector goal){
		return (int)Math.sqrt((goal.x - now.x)*(goal.x - now.x)+(goal.y - now.y)*(goal.y - now.y));
	}
}

CODE:

public class Vector{
	public int x;
	public int y;
	public Vector mom = null;

	public Vector(int x, int y){
		this.x = x;
		this.y = y;
	}
	public Vector(int x, int y, Vector m){
		this.x = x;
		this.y = y;
		this.mom = m;
	}

	public void setMom(Vector m){
		this.mom = m;
	}

	public String toString(){
		return "[" + x + "," + y + "]";
	}
}