偶的Java版A*算法源程序

vic_st

/***********************************************\
*
*
*                A*算法的Java版本
*
*
*                作者：Vic
*                QQ:307042976
*
*
*
\***********************************************/
//好不容易玩了，谨奉此。
import java.util.*;
import java.awt.*;
import java.io.*;

class point extends Point{
        int h;                //到本点的步数
        point father;        //前节点
        public point(int nx,int ny,int nh,point p){
                super(nx,ny);
                h=nh;
                father=p;
        }
        public point(point p){
                x=p.x;
                y=p.y;
                h=p.h;
                father=p.father;
        }
        public Point getPoint(){
                return new Point(x,y);
        }
}

class Astar {
        BitSet                        map;
        int                                width;
        int                                height;
       
        Point                        s;
        Point                        e;
       
        Astar(){
        }
       
        //The G function of A*.
        int                                f(point p)        {
                return  (Math.abs(p.x-e.x)+Math.abs(p.y-e.y)+p.h);
        }
        //Final step,Get Way.
        Stack                        getWay(point p){
                Stack way=new Stack();
                while(p.father!=null){
                        way.push(p.getPoint());
                        p=p.father;
                }
                way.push(p.getPoint());
                return way;
        }
        //Juge point.
        boolean                        isOK(Point p)        {
                if(p.x>=width || p.x<0)
                        return false;
                if(p.y>=height || p.y<0)
                        return false;
                if(map.get(p.y*width+p.x))
                        return false;
                return true;
        }
        //Insert at the right position.
        void                        InsertP2V(point p,Vector v){
                if(v.isEmpty()){
                        v.addElement(p);
                        return;
                }
                if(v.size()==1){
                        if(f((point)(v.elementAt(0)))<f(p))
                                v.addElement(p);
                        else
                                v.insertElementAt(p,0);
                        return;
                }
                for(int i=0;i<v.size()-1;i++){
                        //Find the right positon to insert.
                        if(f((point)(v.elementAt(i)))<=f(p) && f(p)<=f((point)(v.elementAt(i+1)))){
                                v.insertElementAt(p,i+1);
                                return;
                        }
                        v.addElement(p);
                }
        }
        //The main function of A*.
        Stack                        findWay()
        {
                Vector open=new Vector(100);
                Vector close=new Vector(100);
                open.addElement(new point(s.x,s.y,0,null));
               
                while(!open.isEmpty()){
                        point p1=new point((point)open.elementAt(0));
                        open.removeElementAt(0);
                       
                        if(p1.equals(e)){
                                return getWay(p1);
                        }
                       
                        int nh=p1.h+1;
                        point[] p2=new point[4];
                        p2[0]=new point(p1.x-1,p1.y,nh,p1);
                        p2[1]=new point(p1.x+1,p1.y,nh,p1);
                        p2[2]=new point(p1.x,p1.y-1,nh,p1);
                        p2[3]=new point(p1.x,p1.y+1,nh,p1);
                       
                        for(int i=0;i<4;i++){
                                if(!isOK(p2)){
                                        continue;
                                }
                                int I_O=open.indexOf(p2);
                                int I_C=close.indexOf(p2);
                                int f=f(p2);
                               
                                if(I_O<0 && I_C<0){//If haven't visited:
                                        InsertP2V(p2,open);
                                }
                                else if(I_O>=0){//If it's in the open:
                                        if(nh<((point)open.elementAt(I_O)).h){
                                                open.removeElementAt(I_O);
                                                InsertP2V(p2,open);
                                        }
                                }
                                else{//If it's in the Close:
                                        if(nh<((point)close.elementAt(I_C)).h){
                                                close.removeElementAt(I_C);
                                                InsertP2V(p2,open);
                                        }
                                }
                        }
                        close.addElement(p1);
                }
                return null;
        }
        //Main Test:
        public static void main(String[]args) throws IOException{
                BufferedReader br=new BufferedReader(new InputStreamReader(System.in));
                System.out.println("Width & Height");
                int w=Integer.parseInt(br.readLine());
                int h=Integer.parseInt(br.readLine());
                BitSet bs=new BitSet(w*h);
                System.out.println("Map Data:");
                for(int i=0;i<w*h;i++){
                        if(Integer.parseInt(br.readLine())!=0)
                                bs.set(i);
                }
                System.out.println("SXSYEXEY");
                int sx=Integer.parseInt(br.readLine()),
                        sy=Integer.parseInt(br.readLine()),
                        ex=Integer.parseInt(br.readLine()),
                        ey=Integer.parseInt(br.readLine());
                Point start=new Point(sx,sy);
                Point end  =new Point(ex,ey);
               
                Astar as=new Astar();
                as.width=w;
                as.height=h;
                as.map=bs;
                as.s=start;
                as.e=end;
                System.out.println(as.findWay());
        }
}

[em16]