迷宫算法

/**
* Created by aaron on 16-3-16.
*
* •    F = G+H
* •	G＝从起点A沿着已生成的路径到一个给定方格的移动开销。
* •    H＝从给定方格到目的方格的估计移动开销。这种方式常叫做试探，有点困惑人吧。其实之所以叫做试探法是因为这只是一个猜测。在找到路径之前我们实际上并不知道实际的距离，因为任何东西都有可能出现在半路上（墙啊，水啊什么的）。本文中给出了一种计算H值的方法，网上还有很多其他文章介绍的不同方法。
*
*/
public class RobotTest {

static Piece start;
static Piece end;
static Piece[][] pieces;

public static Piece[][] init(){
Piece[][] pieces = new Piece[5][7];

start = pieces[2][1] = new Piece(2,1,PieceType.START);
end = pieces[2][5] = new Piece(2,5,PieceType.END);

pieces[2][1].setG(0);
pieces[2][1].setH(calcH(pieces[2][1],pieces[2][5]));
pieces[2][1].setF(pieces[2][1].getH());

for(int i=0;i<pieces.length;i++){
for(int j=0;j<pieces[i].length;j++){
if(i == 2 && j == 1){

}else if(i == 2 && j == 5){

}else if(j == 3 && (i==1||i==2||i==3)){
pieces[i][j] = new Piece(i,j,PieceType.WALL);
}else{
pieces[i][j] = new Piece(i,j,PieceType.EMPTY);
}
}
}
return pieces;
}

public static void test(Piece[][] pieces){
//        System.out.println(calcH(pieces[3][0],pieces[2][5]));
//        System.out.println(calcH(pieces[3][2],pieces[2][5]));
//        System.out.println(calcH(pieces[1][2],pieces[2][5]));

System.out.println(calcG(pieces[0][1],pieces[0][2]));
System.out.println(calcG(pieces[0][1],pieces[1][1]));
System.out.println(calcG(pieces[0][1],pieces[1][2]));
//        System.out.println(calcG(pieces[0][1],pieces[0][3]));
//        System.out.println(calcG(pieces[0][1],pieces[2][1]));
//        System.out.println(calcG(pieces[0][1],pieces[2][2]));
}

public static int calcF(Piece start,Piece end){
return calcG(start,end)+calcH(start,end);
}

public static void main(String[] args) {
pieces = init();
display(pieces);
//test(pieces);
process(pieces);
}

public static void process(Piece[][] pieces){
Piece start = pieces[2][1];
Piece end = pieces[2][5];

List<Piece> openList = new ArrayList<Piece>(Arrays.asList(start));
List<Piece> closeList = new ArrayList<Piece>();

process(pieces,openList,closeList,end);
}

public static int indexOfMinF(List<Piece> pieces){
int index = 0;
int minF = pieces.get(0).getF();
int size = pieces.size();
for(int i=0;i<size;i++){
if(pieces.get(i).getF() < minF){
minF = pieces.get(i).getF();
index = i;
}
}
return index;
}

public static void process(Piece[][] pieces,List<Piece> openList,List<Piece> closeList,Piece end){
//        System.out.println(openList);
//        System.out.println(closeList);
//        System.out.println("******************************************************");
//        displayDetail(pieces);
//        System.out.println("******************************************************");

if(!CollectionUtils.isEmpty(openList) && !openList.contains(end)){
Piece currentPiece = openList.remove(indexOfMinF(openList));
processAround(pieces,currentPiece,openList,closeList);
closeList.add(currentPiece);

process(pieces,openList,closeList,end);
}else if(openList.contains(end)){
// Found
System.out.println("Found");
displayDetail(pieces);
Piece next = end;
while(next != null){
System.out.print(next);
next = next.getParent();
if(next != null){
System.out.print("\t--->\t");
}
}
}else{
// No route
System.out.println("No route found");
}
}

public static void processAround(Piece[][] pieces,Piece currentPiece,List<Piece> openList,List<Piece> closeList){
int currentX = currentPiece.getX();
int currentY = currentPiece.getY();
int width = pieces[currentX].length;
int height = pieces.length;

Piece tmp = null;

//        if((currentPiece.getY()==4 && (currentPiece.getX()==1||currentPiece.getX()==2||currentPiece.getX()==3))||(currentPiece.getY()==5&&(currentPiece.getX()==1||currentPiece.getX()==3))){
//            System.out.println("processing...");
//            System.out.println("...");
//        }

if(currentX - 1 >= 0){
tmp = pieces[currentX-1][currentY];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
if(currentY - 1 >= 0){
tmp = pieces[currentX-1][currentY-1];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
}
if(currentY + 1 <= width - 1){
tmp = pieces[currentX-1][currentY+1];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
}
}

if(currentX+1 <= height-1){
tmp = pieces[currentX+1][currentY];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
if(currentY-1>=0){
tmp = pieces[currentX+1][currentY-1];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
}
if(currentY+1<=width-1){
tmp = pieces[currentX+1][currentY+1];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
}
}

if(currentY-1>=0){
tmp = pieces[currentX][currentY-1];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
}

if(currentY+1<=width-1){
tmp = pieces[currentX][currentY+1];
if(canAdd(currentPiece,tmp,closeList,openList)){
tmp.setParent(currentPiece);
openList.add(tmp);
}
}
}

private static boolean aroundObstacle(Piece one,Piece another, Piece[][] pieces){
if(Math.abs(one.getX()-another.getX()) == 1 && Math.abs(one.getY()-another.getY()) == 1) {
// 对角线
int oneX = one.getX();
int oneY = one.getY();
int anotherX = another.getX();
int anotherY = another.getY();

if(pieces[oneX][anotherY].isObstacle()||pieces[anotherX][oneY].isObstacle()){
return true;
}
}
return false;
}

public static boolean canAdd(Piece currentPiece, Piece piece, List<Piece> closeList, List<Piece> openList){
if(piece.isObstacle() || closeList.contains(piece) || aroundObstacle(currentPiece,piece,pieces)){
return false;
}

if(openList.contains(piece)){
if(currentPiece.getG()+calcG(currentPiece,piece) < piece.getG()){
// better,then update
System.out.print("Better route of {"+piece.getX()+","+piece.getY()+"}:["+piece.getF()+","+piece.getG()+","+piece.getH()+"]\t");
piece.setG(currentPiece.getG()+calcG(currentPiece,piece));
piece.setF(piece.getG()+piece.getH());
System.out.println("["+piece.getF()+","+piece.getG()+","+piece.getH()+"]\t");
}
return false;
}else{
piece.setG(currentPiece.getG()+calcG(currentPiece,piece));
piece.setH(calcH(end,piece));
piece.setF(piece.getH()+piece.getG());
return true;
}
}

public static void sleep(long time){
try {
Thread.currentThread().sleep(time);
} catch (InterruptedException e) {
e.printStackTrace();
}
}

public static int calcH(Piece start,Piece end){
return 10 * (Math.abs(start.getX()-end.getX()) + Math.abs(start.getY()-end.getY()));
}

public static int calcG(Piece start,Piece end){
if(Math.abs(start.getX()-end.getX()) > 1 || Math.abs(start.getY()-end.getY()) > 1){
throw new RuntimeException("Only one step is allowed.Start {"+start.toString()+"} End {"+end.toString()+"}");
}

if(Math.abs(start.getX()-end.getX()) == 1 && Math.abs(start.getY()-end.getY()) == 1){
return 14;
}else{
return 10;
}
}

public static void display(Piece[][] pieces){
for(int i=0;i<pieces.length;i++){
for(int j=0;j<pieces[i].length;j++){
if(pieces[i][j].pieceType == PieceType.WALL){
System.err.print("* ");
sleep(100);
}else if(pieces[i][j].pieceType == PieceType.START){
System.err.print("o ");
sleep(100);
}else if(pieces[i][j].pieceType == PieceType.END){
System.err.print("O ");
sleep(100);
}else{
System.out.print("* ");
sleep(100);
}
}
System.out.println();
sleep(100);
}
}

public static void displayDetail(Piece[][] pieces){
for(int i=0;i<pieces.length*2;i++){
boolean firstLine = i%2==0;
for(int j=0;j<pieces[i/2].length;j++){
if(firstLine){
System.out.print(pieces[i/2][j].getF()+"\t[]\t\t");
}else{
System.out.print(pieces[i/2][j].getG()+"\t"+pieces[i/2][j].getH()+"\t\t");
}
}
System.out.println();
if(!firstLine){
System.out.println();
}
}
}

}

enum PieceType{
EMPTY(0),
WALL(1),
START(2),
END(3);

private int value;
private PieceType(int value){
this.value = value;
}

public int value(){
return this.value;
}
}

class Piece{
int x;
int y;

int f;
int g;
int h;

Piece parent;

PieceType pieceType;

public Piece(int x,int y, PieceType pieceType){
this.x = x;
this.y = y;
this.pieceType = pieceType;
}

// 是否是障碍
public boolean isObstacle(){
return this.pieceType != PieceType.EMPTY && this.pieceType != PieceType.START && this.pieceType != PieceType.END;
}

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;
}

public int getF() {
return f;
}

public void setF(int f) {
this.f = f;
}

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 Piece getParent() {
return parent;
}

public void setParent(Piece parent) {
this.parent = parent;
}

@Override
public String toString() {
//        return ToStringBuilder.reflectionToString(this);
return "["+x+","+y+","+f+","+g+","+h+"]";
}
}