Java室内最短路径搜索(支持多楼层)

修改了上次的代码，现在支持室内的多楼层情况下的最短路径搜索，还是使用A*算法，把在GraphAdjList中VNode没有利用起来的data字段作为我们存储楼层属性的位置。

实际上是我偷懒了，正常情况下VNode里应该再加一个int level属性，而data还是作为绑定用户想添加任意类型的数据的一个位置来使用，这样例如当用户想对任意节点添加String类型的描述时，声明GraphAdjList<String>即可，但现在我们的GraphAdjList只能声明为GraphAdjList<Integer>，因为我们把data作为楼层属性来使用，名存实亡的模板类hh。

用户添加节点时使用GraphAdjList.insertVex(E v,int index,int x,int y)，v 楼层，index 节点的唯一序列号(从1开始，符合生活习惯)，x,y是点坐标，添加边的操作与上次不变。

需要注意的是，在我们的A*代码中，f=g+h,启发函数h设置的可能不是很理想，我们仍旧沿用了x,y的曼哈顿距离，未考虑楼梯口位置和层数等因素的影响，如果遇到起点终点都在所属层的中心位置，楼梯电梯在每层的边缘，搜索方向会先往中心扩展，直到没有结果时，才会往边缘扩展，随后扩展到楼上或楼下，在这种情况下效率可能不佳，但仍旧能获得最短路径。简而言之，就是在我们现在设计的h下，起点会优先向终点在起点所属层的垂足点扩展。至于h的设计，可以去查阅一下相关文献。

我们依旧使用TestContinuous作为我们的测试类，自己画了一个简单的数据案例 ，想要查询从一楼的节点4到三楼的节点14的最短路径。

 

其中一二楼之间只有通道1-6，3-8，分别代价是3，4。二三楼之间只有通道10-11，代价是5。

最后获得结果是4-5-1-6-10-11-13-14，花费总代价是33。

话不多说仍旧上代码。

代码结构如下：

AStar:

package astar3D;

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 astar3D.GraphAdjList.ENode;
import astar3D.GraphAdjList.VNode;

/**
 * 
 * @author yh
 * @version 3.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 point1 用户输入的起始点
     * @param point2 用户输入的终止点
     * @param barrier 无顺序的障碍列表
     */
    @Override
    public Point move(Point point1, Point point2, Set<Point> barrier) {
       num = 0;
       this.barrier = barrier;
       this.startPoint = findNearPoint(point1);
       this.endPoint = findNearPoint(point2);
       
       //预留位置，准备解决点在障碍物里的情况
       //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 point 用户输入的坐标点
     * @return  最近的出发结点
     */
    private Point findNearPoint(Point point){
        List<Integer> levelVertex = graphadjlist.getLevelVertex(point.level);      
        if(levelVertex.size() > 0){
            int index = levelVertex.get(0);
            int min = getGuessLength(point.x, point.y, graphadjlist.vexs[index].x, graphadjlist.vexs[index].y);  
            int tempmin;
            for(int i = 1; i < levelVertex.size(); i++){          
                tempmin = getGuessLength(point.x, point.y, graphadjlist.vexs[levelVertex.get(i)].x, graphadjlist.vexs[levelVertex.get(i)].y);
                if(tempmin < min ){
                    min = tempmin;
                    index = levelVertex.get(i);
                }
            }
            Point nearPoint = new Point( graphadjlist.vexs[index].x, graphadjlist.vexs[index].y, point.level, index);
            return nearPoint;
        }
            return new Point(point.x, point.y, 0, 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) {
            VNode<Integer> currentNode = graphadjlist.vexs[serial];
            //暂时模板类GraphAdjList只支持int类型，受限于Point不是模板类，楼层变量类型为int(currentNode.data是int)
            this.addOpenPoint(new Point(currentNode.x, currentNode.y, currentNode.data, serial), graphadjlist.getEdge(currentPoint.serial, serial));          
      }      
        num++;
        if (num <= 4000) {
           this.toClose();
        }
     
    }
 
    /**
     * 把该节点相邻点加入关闭的列表
     */
    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 astar3D;

import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.List;

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 int getNumOfLevelVertex(E v){
        int numOfLevelVexs = 0;
        for(int i = 1; i < numOfVexs + 1; i++){
            if(vexs[i].data.equals(v)){
                numOfLevelVexs++;
            }
        }
        return numOfLevelVexs;
    }
    
    //得到一层楼的顶点序列号列表
    public List<Integer> getLevelVertex(E v){
        List<Integer> levelVertex = new ArrayList<Integer>();
        for(int i = 1; i < numOfVexs + 1; i++){
            if(vexs[i].data.equals(v)){
                levelVertex.add(i);
            }
        }
        return levelVertex;
    }
    
    // 获取指定位置节点的楼层
    public E getLevel(int index) {
        if (index < 0 || index >  numOfVexs)
            return null;
        return vexs[index].data;
    }
 
    // 插入顶点,如果再次插入一个index一样的节点则覆盖
    public boolean insertVex(E v,int index,int x,int y) {
        if (numOfVexs >= maxNumOfVexs || index > 1000)
            return false;
        if (vexs[index] == null ){
            numOfVexs++;            
        }
        VNode<E> vex = new VNode<E>();
        vex.data = v;
        vex.x = x;
        vex.y = y;
        vexs[index] = vex;
        return true;
    }
 
    // 删除顶点
    public boolean deleteVex(int index) {        
        if (index > 0 && index < numOfVexs + 1) {
            //删除vexs中的相关记录
            for (int i = index; i < numOfVexs; i++) {
                vexs[i] = vexs[i + 1];
            }
            vexs[numOfVexs] = null;
            numOfVexs--;
            ENode current;
            ENode previous;
            //删除ENode中的
            for (int i = 1; i < numOfVexs + 1; i++) {
                if (vexs[i].firstadj == null)
                    continue;
                if (vexs[i].firstadj.adjvex == index && vexs[i].firstadj.nextadj == null) {
                    vexs[i].firstadj = null;
                    continue;
                }
                current = vexs[i].firstadj;
                while (current != null) {
                    previous = current;
                    current = current.nextadj;
                    if (current != null && current.adjvex == index) {
                        previous.nextadj = current.nextadj;
                        break;
                    }
                }
            }
            //对每一个ENode中的adjvex进行修改
            for (int i = 1; i < numOfVexs + 1; i++) {
                current = vexs[i].firstadj;
                while (current != null) {
                    if (current.adjvex > index)
                        current.adjvex--;
                    current = current.nextadj;
                }
            }
            return true;
        }    
        return false;
    }
 
    // 插入边
    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 astar3D;

import java.util.List;

public interface IGraph<E> {
     public int getNumOfVertex();//获取顶点的个数
     public int getNumOfLevelVertex(E v);//得到某一层楼的顶点数目
     public List<Integer> getLevelVertex(E v);//得到一层楼的顶点序列号列表
     public E getLevel(int index);//获取指定位置节点的楼层
     boolean insertVex(E v, int index, int x, int y);//插入顶点
     boolean deleteVex(int index);//删除顶点
     boolean insertEdge(int v1, int v2,int weight);//插入边
     boolean deleteEdge(int v1, int v2);//删除边
     int getEdge(int v1,int v2);//查找边

}

IMove:

package astar3D;

import java.util.Set;

/**
 * 
 * @author yh
 * @version 3.0 
 *
 */
public interface IMove {
    /**
     * 求点1到点2的合适路线
     * @param point1  用户输入的起始点
     * @param point2 用户输入的终止点 
     * @param barrier 无顺序的障碍列表
     * @return
     */
    Point move(Point point1, Point point2, Set<Point> barrier);
    
}

Point:

package astar3D;

public class Point {
    int x;
    int y;
    int gCost;
    int hEstimate;
    int fTotal;
    Point prev;
    //点所在的楼层
    int level;
    //点的序列号
    int serial;
    
    public String getKey(){
        return level + "_" + x + "_" + y;
    }
    public Point(int x, int y) {
        super();
        this.x = x;
        this.y = y;
    }
    
    /**
     * 
     * @param x
     * @param y
     * @param level 楼层
     */
    public Point(int x, int y, int level){
        super();
        this.x = x;
        this.y = y;
        this.level = level;
        
    }
    
    /**
     * 用户不是输入整数时，转换成整数再处理
     * @param x
     * @param y
     * @param level
     */
    public Point(double x, double y, int level){
        super();
        this.x = (int) x;
        this.y = (int) y;
        this.level = level;
    }
    
    /**
     * 
     * @param x
     * @param y
     * @param level 楼层
     * @param serialNumber 点的序号(唯一值)
     */
    public Point(int x, int y, int level,int serialNumber) {
        super();
        this.x = x;
        this.y = y;        
        this.level = level;
        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;
        if (level != other.level)
            return false;
        return true;
    }
 
}

TestContinuous:

package astar3D;

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(1, 2, 2, 1);
        graphadjlist.insertVex(1, 3, 3, 2);
        graphadjlist.insertVex(1, 4, 2, 3);
        graphadjlist.insertVex(1, 5, 1, 3);
        graphadjlist.insertVex(2, 6, 1, 2);
        graphadjlist.insertVex(2, 7, 3, 2);
        graphadjlist.insertVex(2, 8, 3, 4);
        graphadjlist.insertVex(2, 9, 1, 4);
        graphadjlist.insertVex(2, 10, 2, 3);
        graphadjlist.insertVex(3, 11, 2, 2);
        graphadjlist.insertVex(3, 12, 1, 2);
        graphadjlist.insertVex(3, 13, 3, 2);
        graphadjlist.insertVex(3, 14, 2, 1);
            
        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);
        graphadjlist.insertEdge(1, 6, 3);
        graphadjlist.insertEdge(3, 8, 4);
        graphadjlist.insertEdge(6, 9, 6);
        graphadjlist.insertEdge(9, 8, 4);
        graphadjlist.insertEdge(8, 7, 5);
        graphadjlist.insertEdge(7, 6, 2);
        graphadjlist.insertEdge(6, 10, 3);
        graphadjlist.insertEdge(9, 10, 15);
        graphadjlist.insertEdge(7, 10, 1);
        graphadjlist.insertEdge(10, 11, 5);
        graphadjlist.insertEdge(11, 12, 5);
        graphadjlist.insertEdge(12, 14, 8);
        graphadjlist.insertEdge(11, 13, 9);
        graphadjlist.insertEdge(13, 14, 2);
                
        Set<Point> barrier = new HashSet<Point>();
        //barrier.add(new Point(1, 3, 1));
                
        AStar aStar = new AStar();
        aStar.graphadjlist = graphadjlist;
        Point startPoint = new Point(2.2, 3.1, 1);
        Point endPoint = new Point(2, 1, 3); 
        endPoint = aStar.move(startPoint, endPoint, barrier);    
        
        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;
        }    
                    
}

如果想实现类似每个点都是(x,y,z)格式的最短三维路径搜索，可以对二维情况下代码进行修改(上次写的那篇)。

贴出我们的主要参考：

 

https://blog.csdn.net/h348592532/article/details/44421753

 

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

 

来源：oschina

链接：https://my.oschina.net/u/4267539/blog/4282983