Java A*算法搜索无向图最短路径

网上看了很多别人写的a*算法，都是针对栅格数据进行处理，每次向外扩展都是直接八方向或者四方向，这样利于理解。每次移动当前点，gcost也可以直接设置成横向10斜向14。

但是当我想处理一个连续的数据集，比如一个网络状的图，难道我还要先把这个数据图切分成网格，计算节点落在网格中的位置，再进行操作吗？在现实世界中，也会有很多使用矢量数据比栅格数据更为简便的情况。

显然我们可以自己动手，借助别人的代码进行重构，让a*也能对图使用。

代码结构如下：

 

其中astar是a*算法的核心类，graphadjlist是我们存储数据的邻接表(因为我们的无向图如果用邻接矩阵存储，往往是稀疏矩阵，会浪费内存空间)，point是节点的具体属性，testcontinuous是我们写的一个简单测试类

话不多说上代码吧。

astar:

package astarenhanced;

import java.util.arraylist;
import java.util.collections;
import java.util.comparator;
import java.util.hashmap;
import java.util.hashset;
import java.util.list;
import java.util.map;
import java.util.set;

import astarenhanced.graphadjlist.enode;
 
/**
 * 
 * @author yh
 * @version 2.0 
 * 
 */
public class astar implements imove {
 
    /* 打开的列表 */
    map<string, point> openmap = new hashmap<string, point>();
    /* 关闭的列表 */
    map<string, point> closemap = new hashmap<string, point>();
    /* 障碍物 */
    set<point> barrier;
    /* 起点 */
    point startpoint;
    /* 终点 */
    point endpoint;
    /* 当前使用节点 */
    point currentpoint;
    /* 循环次数，为了防止目标不可到达 */
    int num = 0;   
    //存储的数据结构
    public graphadjlist<integer> graphadjlist;
 
    /**
     * 初始化并开始计算最佳路径
     * @param x1 出发点x
     * @param y1 出发点y
     * @param x2 终止点x
     * @param y2 终止点y
     */
    @override
    public point move(int x1, int y1, int x2, int y2, set<point> barrier) {
       num = 0;
       this.barrier = barrier;
       this.startpoint = findnearpoint(x1, y1);
       this.endpoint = findnearpoint(x2, y2);
       
       //预留位置，准备解决点在障碍物里的情况
       //point endpoint=new point(x2,y2);
       //this.endpoint = this.getnearpoint(endpoint,endpoint);
       
       this.closemap.put(startpoint.getkey(), startpoint);
       this.currentpoint = this.startpoint;
       this.toopen(startpoint);
       return endpoint;
    }
 
     /**
     * 求两点间的估算代价， 启发函数一（曼哈顿距离）： (math.abs(x1 - x2) + math.abs(y1 - y2))
     * 启发函数二（平方的欧几里得距离）：((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 -y1))
     * 启发函数三（欧几里得距离）：(int) math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) *(y2 -y1))
     * 启发函数四（对角线距离）：math.max(math.abs(x1 - x2), math.abs(y1 -y2)) 
     * 不用启发函数：0
     */
    private int getguesslength(int x1, int y1, int x2, int y2) {
        //return ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 -y1));
        return (math.abs(x1 - x2) + math.abs(y1 - y2)) ;
        // return math.max(math.abs(x1 - x2), math.abs(y1 - y2));
        // return 0;
        }
    
    /**
     * 对用户输入的点坐标，寻找旁边最近的出发点  
     * @param x1
     * @param y1
     * @return  最近的出发结点
     */
    private point findnearpoint(int x1,int y1){
        int numofvexs = graphadjlist.getnumofvertex();      
        if(numofvexs > 0){
            int min = getguesslength(x1, y1, graphadjlist.vexs[1].x, graphadjlist.vexs[1].y);
            int index = 1;
            int tempmin;
            for(int i = 2; i < numofvexs + 1; i++){          
                tempmin = getguesslength(x1, y1, graphadjlist.vexs[i].x, graphadjlist.vexs[i].y);
                if(tempmin < min ){
                    min = tempmin;
                    index = i;
                }
            }
            point nearpoint = new point( graphadjlist.vexs[index].x, graphadjlist.vexs[index].y, index);
            return nearpoint;
        }
            return new point(x1, y1, 0);        
    }
 
    /**
     * 把该节点相邻点加入计算
     * @param point
     */
    private void toopen(point point) {
        set<integer> adjpoint = new hashset<integer>();
        if(graphadjlist.vexs[point.serial].firstadj == null){
            return;
        }else{
            enode current;
            current = graphadjlist.vexs[point.serial].firstadj;
            while(current != null){
                adjpoint.add(current.adjvex);
                current = current.nextadj;
            }            
        }
        
        for (int serial : adjpoint) {
            this.addopenpoint(new point(graphadjlist.vexs[serial].x, graphadjlist.vexs[serial].y, serial), graphadjlist.getedge(currentpoint.serial, serial));          
      }      
        num++;
        if (num <= 4000) {
           this.toclose();
        }
     
    }
 
    /**
     * 把该节点相邻点加入关闭的列表
     * 
     * @param x
     * @param y
     */
    private void toclose() {
       list<point> list = new arraylist<point>(openmap.values());
       collections.sort(list, new comparator<point>() {
          @override
          //按升序排序，之后取出第一个元素即可
          public int compare(point o1, point o2) {
             if (o1.ftotal > o2.ftotal) {
                return 1;
             } else if (o1.ftotal < o2.ftotal) {
                return -1;
             } else {
             return 0;
             }
          }
       });
       if (list.size() > 0) {
          this.currentpoint = list.get(0);
          closemap.put(this.currentpoint.getkey(), this.currentpoint);
          openmap.remove(this.currentpoint.getkey());
          if (!currentpoint.equals(endpoint)) {
             this.toopen(this.currentpoint);
          } else {
             endpoint = this.currentpoint;
          }
       }
    }
 
    /**
     * a*核心处理函数
     * 
     * @param point currentpoint连接的点
     * @param gcost 当前点到该点的消耗
     * @return
     */
    private void addopenpoint(point point,int gcost) {
       if (point.x < 0 || point.y < 0) {
          return;
       }
       string key = point.getkey();
       if (!barrier.contains(point) && !point.equals(this.currentpoint)) {
          int hestimate = this.getguesslength(point.x, point.y, this.endpoint.x, this.endpoint.y);
          int totalgcost = this.currentpoint.gcost + gcost;
          int ftotal = totalgcost + hestimate;
          //当前point没有加入到closemap中，则放入openmap中，为toclose函数比较ftotal,并挑选出最佳点做准备
          if (!closemap.containskey(key)) {
             point.hestimate = hestimate;
             point.gcost = totalgcost;
             point.ftotal = ftotal;
             point oldpoint = openmap.get(key);
             //如果之前此点已经加入到openmap，看其是否需要更新为最小值
             if (oldpoint != null) {
                if (oldpoint.gcost > totalgcost) {
                oldpoint.ftotal = ftotal;               
                oldpoint.gcost=totalgcost;
                oldpoint.prev = this.currentpoint;
                //当key一样时，后面put的会把前面的覆盖
                openmap.put(key, oldpoint);
                }   
             } else {
                point.prev = this.currentpoint;
                openmap.put(key, point);
             } 
          } else {
             point oldpoint = closemap.get(key);
             if (oldpoint != null) {
                if ((oldpoint.gcost + gcost) < this.currentpoint.gcost) {
                   if (this.currentpoint.prev != oldpoint) {
                      this.currentpoint.ftotal = oldpoint.ftotal + gcost;
                      this.currentpoint.gcost = oldpoint.gcost + gcost;
                      this.currentpoint.prev = oldpoint;
                   }
                }
             }
          }
       }
    }

    //如果用户选择的点在障碍物内，则选出障碍物外距离endpoint最近的一点作为endpoint
    map<string, point> nearoutmap;

    public point getnearpoint(point point,point point2) {
       if(this.barrier.contains(point)){
          nearoutmap = new hashmap<string, point>();
          this.endpoint=point;
          this.tonearpoint(point,point2);
          list<point> nearlist = new arraylist<point>(nearoutmap.values());
          collections.sort(nearlist, new comparator<point>() {
             @override
             public int compare(point o1, point o2) {
                if (o1.gcost > o2.gcost) {
                   return 1;
                } else if (o1.gcost < o2.gcost) {
                   return -1;
                } else {
                   return 0;
                }
             }
          });
          //刚才使用了这两个变量，现在障碍物外的最邻近点已经找到，初始化准备a*
          this.openmap=new hashmap<string,point>();
          this.closemap=new hashmap<string,point>();
          if (nearlist.size() > 0) {
             return nearlist.get(0);
          }else{
             return point;
          }
       }else{
       return point;
       }

   }

    public void tonearpoint(point point,point point2) {
       int x = point.x;
       int y = point.y;
       this.addnearopenpoint(new point(x - 1, y),point2);
       this.addnearopenpoint(new point(x + 1, y),point2);
       this.addnearopenpoint(new point(x, y - 1),point2);
       this.addnearopenpoint(new point(x, y + 1),point2);
       this.addnearopenpoint(new point(x - 1, y - 1),point2);
       this.addnearopenpoint(new point(x - 1, y + 1),point2);
       this.addnearopenpoint(new point(x + 1, y - 1),point2);
       this.addnearopenpoint(new point(x + 1, y + 1),point2);
 
       if(this.nearoutmap.size()==0){
          list<point> list = new arraylist<point>(openmap.values());
          //按照升序排序，最小的在list的最前面
          collections.sort(list, new comparator<point>() {
             @override
             public int compare(point o1, point o2) {
                int l1 = o1.gcost;
                int l2 = o2.gcost;
                if (l1 > l2) {
                   return 1;
                } else if (l1 < l2) {
                   return -1;
                } else {
                   return 0;
                }
             }
          });
          if (list.size() > 0) {
             point p = list.get(0);
             this.closemap.put(p.getkey(), p);
             this.openmap.remove(p.getkey());
             this.tonearpoint(list.get(0),point2);
          }
       }
    }

    private void addnearopenpoint(point point,point point2) {
       string key = point.getkey();
       int gcost = this.getguesslength(point.x, point.y, point2.x,point2.y);
       point.gcost = gcost;
       if (this.barrier.contains(point)) {
          if (!this.openmap.containskey(key) && !this.closemap.containskey(key)) {
             this.openmap.put(key, point);
          }
       } else {
          this.nearoutmap.put(key, point);
       }

    }
    
    public map<string, point> getopenmap() {
       return openmap;
    }

    public void setopenmap(map<string, point> openmap) {
       this.openmap = openmap;
    }

    public map<string, point> getclosemap() {
       return closemap;
    }

    public void setclosemap(map<string, point> closemap) {
       this.closemap = closemap;
    }

    public set<point> getbarrier() {
       return barrier;
    }

    public void setbarrier(set<point> barrier) {
       this.barrier = barrier;
    }

    public point getendpoint() {
       return endpoint;
    }

    public void setendpoint(point endpoint) {
       this.endpoint = endpoint;
    }

     public point getstartpoint() {
       return startpoint;
     }

    public void setstartpoint(point startpoint) {
       this.startpoint = startpoint;
    }

}

graphadjlist:

package astarenhanced;

import java.lang.reflect.array;

public class graphadjlist<e> implements igraph<e> {
    // 邻接表中表对应的链表的顶点
    public static class enode {
        int adjvex; // 邻接顶点序号
        int weight;// 存储边或弧相关的信息，如权值
        enode nextadj; // 下一个邻接表结点
 
        public enode(int adjvex, int weight) {
            this.adjvex = adjvex;
            this.weight = weight;
        }
    }
 
    // 邻接表中表的顶点
    public static class vnode<e> {
        e data; // 顶点信息
        int x;
        int y;
        enode firstadj; // //邻接表的第1个结点
    };
 
    public vnode<e>[] vexs; // 顶点数组
    private int numofvexs;// 顶点的实际数量
    private int maxnumofvexs;// 顶点的最大数量
    //private boolean[] visited;// 判断顶点是否被访问过
 
    @suppresswarnings("unchecked")
    public graphadjlist(int maxnumofvexs) {
        this.maxnumofvexs = maxnumofvexs;
        vexs = (vnode<e>[]) array.newinstance(vnode.class, maxnumofvexs);
    }
 
    // 得到顶点的数目
    public int getnumofvertex() {
        return numofvexs;
    }
 
    // 插入顶点
    public boolean insertvex(e v,int index,int x,int y) {
        if (numofvexs >= maxnumofvexs)
            return false;
        vnode<e> vex = new vnode<e>();
        vex.data = v;
        vex.x = x;
        vex.y = y;
        vexs[index] = vex;
        numofvexs++;
        return true;
    }
 
    // 删除顶点
    public boolean deletevex(e v) {
        for (int i = 1; i < numofvexs + 1; i++) {
            if (vexs[i].data.equals(v)) {
                //删除vexs中的相关记录
                for (int j = i; j < numofvexs; j++) {
                    vexs[j] = vexs[j + 1];
                }
                vexs[numofvexs] = null;
                numofvexs--;
                enode current;
                enode previous;
                //删除enode中的
                for (int j = 1; j < numofvexs + 1; j++) {
                    if (vexs[j].firstadj == null)
                        continue;
                    if (vexs[j].firstadj.adjvex == i) {
                        vexs[j].firstadj = null;
                        continue;
                    }
                    current = vexs[j].firstadj;
                    while (current != null) {
                        previous = current;
                        current = current.nextadj;
                        if (current != null && current.adjvex == i) {
                            previous.nextadj = current.nextadj;
                            break;
                        }
                    }
                }
                //对每一个enode中的adjvex进行修改
                for (int j = 1; j < numofvexs + 1; j++) {
                    current = vexs[j].firstadj;
                    while (current != null) {
                        if (current.adjvex > i)
                            current.adjvex--;
                        current = current.nextadj;
                    }
                }
                return true;
            }
        }
        return false;
    }
 
    // 定位顶点的位置
    public int indexofvex(e v) {
        for (int i = 1; i < numofvexs + 1; i++) {
            if (vexs[i].data.equals(v)) {
                return i;
            }
        }
        return -1;
    }
 
    // 定位指定位置的顶点
    public e valueofvex(int v) {
        if (v < 0 || v >  numofvexs)
            return null;
        return vexs[v].data;
    }
 
    // 插入边
    public boolean insertedge(int v1, int v2, int weight) {
        if (v1 < 0 || v2 < 0 || v1 > numofvexs || v2 > numofvexs)
            throw new arrayindexoutofboundsexception();
        enode vex1 = new enode(v2, weight);
 
        // 索引为index1的顶点没有邻接顶点
        if (vexs[v1].firstadj == null) {
            vexs[v1].firstadj = vex1;
        }
        // 索引为index1的顶点有邻接顶点
        else {
            vex1.nextadj = vexs[v1].firstadj;
            vexs[v1].firstadj = vex1;
        }
        enode vex2 = new enode(v1, weight);
        // 索引为index2的顶点没有邻接顶点
        if (vexs[v2].firstadj == null) {
            vexs[v2].firstadj = vex2;
        }
        // 索引为index1的顶点有邻接顶点
        else {
            vex2.nextadj = vexs[v2].firstadj;
            vexs[v2].firstadj = vex2;
        }
        return true;
    }
 
    // 删除边
    public boolean deleteedge(int v1, int v2) {
        if (v1 < 0 || v2 < 0 || v1 > numofvexs || v2 > numofvexs)
            throw new arrayindexoutofboundsexception();
        // 删除索引为index1的顶点与索引为index2的顶点之间的边
        enode current = vexs[v1].firstadj;
        enode previous = null;
        while (current != null && current.adjvex != v2) {
            previous = current;
            current = current.nextadj;
        }
        if (current != null)
            previous.nextadj = current.nextadj;
        // 删除索引为index2的顶点与索引为index1的顶点之间的边
        current = vexs[v2].firstadj;
        while (current != null && current.adjvex != v1) {
            previous = current;
            current = current.nextadj;
        }
        if (current != null)
            previous.nextadj = current.nextadj;
        return true;
    }
 
    // 得到边
    public int getedge(int v1, int v2) {
        if (v1 < 0 || v2 < 0 || v1 > numofvexs || v2 > numofvexs)
            throw new arrayindexoutofboundsexception();
        enode current = vexs[v1].firstadj;
        while (current != null) {
            if (current.adjvex == v2) {
                return current.weight;
            }
            current = current.nextadj;
        }
        return 0;
    }
 
}

igraph:

package astarenhanced;


public interface igraph<e> {
     public int getnumofvertex();//获取顶点的个数
     boolean insertvex(e v, int index, int x, int y);//插入顶点
     boolean deletevex(e v);//删除顶点
     int indexofvex(e v);//定位顶点的位置
     e valueofvex(int v);// 定位指定位置的顶点
     boolean insertedge(int v1, int v2,int weight);//插入边
     boolean deleteedge(int v1, int v2);//删除边
     int getedge(int v1,int v2);//查找边

}

imove:

import java.util.set;

/**
 * 
 * @author yh
 * @version 2.0 
 *
 */
public interface imove {
    /**
     * 求点1到点2的合适路线
     * @param x1 点1x坐标
     * @param y1 点1y坐标
     * @param x2 点2x坐标
     * @param y2 点2y坐标
     * @param barrier 无顺序的障碍列表
     * @return
     */
    point move(int x1,int y1,int x2,int y2,set<point> barrier);
    
}

point:

package astarenhanced;

public class point {
    int x;
    int y;
    int gcost;
    int hestimate;
    int ftotal;
    point prev;
    int level=1;
    int serial;
    
    public string getkey(){
        return x+"_"+y;
    }
    public point(int x, int y) {
        super();
        this.x = x;
        this.y = y;
    }
 
    
    public point(int x, int y, int serialnumber) {
        super();
        this.x = x;
        this.y = y;
        this.serial = serialnumber;
    } 
 
    @override
    public int hashcode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + x;
        result = prime * result + y;
        return result;
    }
 
    @override
    public boolean equals(object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getclass() != obj.getclass())
            return false;
        point other = (point) obj;
        if (x != other.x)
            return false;
        if (y != other.y)
            return false;
        return true;
    }
 
}

testcontinuous:

package astarenhanced;

import java.util.arraylist;
import java.util.hashset;
import java.util.list;
import java.util.set;

import org.junit.test;


public class testcontinuous {
    @test
    public void test2() {
        graphadjlist<integer> graphadjlist=new graphadjlist<integer>(1000); 
        
        graphadjlist.insertvex(1, 1, 1, 1);
        graphadjlist.insertvex(2, 2, 2, 1);
        graphadjlist.insertvex(3, 3, 3, 2);
        graphadjlist.insertvex(4, 4, 2, 3);
        graphadjlist.insertvex(5, 5, 1, 3);
            
        graphadjlist.insertedge(1, 2, 10);
        graphadjlist.insertedge(1, 5, 3);
        graphadjlist.insertedge(2, 3, 15);
        graphadjlist.insertedge(2, 4, 7);
        graphadjlist.insertedge(2, 5, 13);
        graphadjlist.insertedge(3, 4, 8);
        graphadjlist.insertedge(4, 5, 8);
        
        set<point> barrier = new hashset<point>();
        
        astar astar = new astar();
        astar.graphadjlist = graphadjlist;
        astar.move(1, 1, 3, 2, barrier);
        point endpoint = astar.getendpoint();
        list<point> list = new arraylist<point>();
        list = testcontinuous.get(endpoint, list);
        
        for (point point : list) {
            system.out.println(point.serial);
        }
        system.out.println(endpoint.ftotal);
        
    }
        
       //生成路径集合
        public static list<point> get(point p, list<point> list) {
            if (p != null) {
                list.add(p);
            }
            point pp = p.prev;
            if (pp != null) {
                testcontinuous.get(pp, list);
            } else {
                return list;
            }
            return list;
        }                    
}

主要参考链接如下：

 

我们自己进行了代码的重构和整合，并对astar中核心部分进行了相当一部分的修改以便满足我们需求。

之后我们还想让算法能支持室内路径规划，会添加关于楼层的处理。

同时对于astar.getnearpoint，astar.tonearpoint，astar.addnearopenpoint会继续修改，这三个函数现在还是针对栅格数据进行处理的，功能主要是，当用户选择的点在障碍物内，则选取障碍物外距离用户选择点最近的一点。