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

主要参考链接如下：https://blog.csdn.net/h348592532/article/details/44421753

https://blog.csdn.net/qq_38410730/article/details/79587747

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

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

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