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會繼續修改，這三個函數現在還是針對柵格數據進行處理的，功能主要是，當用戶選擇的點在障礙物內，則選取障礙物外距離用戶選擇點最近的一點。