• GeoTools的AStar算法实现，自定义Node及Edge

    1 核心依赖包

    开发语言java

    <dependency>   
    <groupId>org.geotoolsgroupId>    
    <artifactId>gt-graphartifactId>   
    <version>18.0version>
dependency>

    • 1
    • 2
    • 3
    • 4
    • 5

    Geotools提供了一个Graph的扩展包，这个扩展包不只是对A*算法进行了实现，也对算法Dijkstra进行了实现，两种算法在Geotools中的用法基本一致。

    2 基本概念介绍

    img

    2.1 Graph（图）

    可以理解为是一个将要建模的对象的容器，图里面存放的是很多的点（node）和边（edge）。

    2.2 Node（点）

    Node（点）是将要建模的对象，它可以指的是实际地图中的一个坐标点，也可以指的是任意坐标系中的一个点，也可以指的是拓扑图里面的一个节点。构建好的Node全部都存放在Graph中。

    2.3 Edge（边）

    边是指两个Node（点）的关联，一个边下面会有两个点，在图中的边是认为两个点之间可到达，边下面两个点分别表示NodeA和NodeB，节点方向为NodeA->NodeB,

    3 在地图上标注POI点，并采集每个点的坐标

    如图，合肥紫云山公园（随便选的公园），随意标注13个POI点（随便标注的点），采集这13个点的坐标。

    image-20220809144414623

    坐标采集可以通过百度、或者高德的坐标采集器实现。

    如高德坐标采集器地址：高德地图获取地图坐标（GCJ-02坐标） - openGPS.cn

    定位到该公园：117.329088,31.739236

    获取到13个点的坐标。

    4 标注出13个POI的拓扑关系，即到达关系

    如图，标注出每个POI的拓扑关系（随便标注的）

    image-20220809144803665

    5 生成Node节点以及Edge边，并标注POI点与Node节点对应关系

    Node生成出来是无序的，如图所示（需根据生成的Node节点和POI点进行对应）。

    //构造点
BasicGraphGenerator basicGraphGenerator = new BasicGraphGenerator();
BasicGraphBuilder basicGraphBuilder = new BasicGraphBuilder();
Coordinate[] coordinates = new Coordinate[12];
coordinates[0] = new Coordinate(104.069649, 30.542246);
coordinates[1] = new Coordinate(104.071001, 30.542283);
coordinates[2] = new Coordinate(104.073458, 30.542329);
.....................
List<Node> nodeList = new ArrayList<>();
for (int i = 0; i < 12; i++) {
    Node node = basicGraphGenerator.getGraphBuilder().buildNode();
    basicGraphGenerator.getGraphBuilder().addNode(node);
    node.setObject(coordinates[i]);
    node.setID(i);
    node.setCount(i);
    node.setVisited(true);
    basicGraphBuilder.addNode(node);
    nodeList.add(node);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19

    Node生成示例数据如下：V=[6, 9, 1, 7, 4, 2, 8, 10, 5, 11, 3, 0]

    image-20220809145009794

    Edge生成：

    public static Graph buildEdge(BasicGraphBuilder basicGraphBuilder, List<Node> nodeList) {
    BasicGraphGenerator basicGraphGenerator = new BasicGraphGenerator();
    Map<Integer, String> map = new HashMap<>();
    //维护Edge关系
    map.put(1, "2");
    map.put(2, "3,12");
    map.put(3, "4,10");
    map.put(4, "5");
    map.put(5, "6");
    map.put(6, "7,11");
    map.put(7, "8");
    map.put(8, "1");
    map.put(9, "4");
    map.put(10, "9,11");
    map.put(11, "12,7");
    map.put(12, "7");
    for (Map.Entry<Integer, String> entry : map.entrySet()) {
        if (entry.getValue().split(",").length > 1) {
            String[] strings = entry.getValue().split(",");
            for (String string : strings) {
                //判断nodeList数组越界情况
                if (Integer.parseInt(string) == 12) {
                    string = "11";
                }
                Edge edge = new BasicEdge(nodeList.get(entry.getKey()), nodeList.get(Integer.parseInt(string)));
                Long distance = getDistance(nodeList.get(entry.getKey()), nodeList.get(Integer.parseInt(string)));
                edge.setObject(distance);
                edge.setVisited(true);
                basicGraphBuilder.addEdge(edge);
            }
        } else {
            Integer key = entry.getKey();
            //判断nodeList数组越界情况
            if (Integer.parseInt(entry.getValue()) == 12) {
                entry.setValue("11");
            }
            //判断nodeList数组越界情况
            if (key == 12) {
                key = 11;
            }
            //构造边
            Edge edge = new BasicEdge(nodeList.get(key), nodeList.get(Integer.parseInt(entry.getValue())));
            Long distance = getDistance(nodeList.get(key), nodeList.get(Integer.parseInt(entry.getValue())));
            edge.setObject(distance);
            edge.setVisited(true);
            basicGraphBuilder.addEdge(edge);
        }
    }
    System.out.println(basicGraphBuilder.getNodes());
    System.out.println(basicGraphBuilder.getGraph());
    return basicGraphBuilder.getGraph();
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52

    Edge生成示例数据如下：

    E=[13 (2,3), 25 (10,11), 14 (2,11), 16 (3,10), 18 (5,6), 17 (4,5), 15 (3,4), 24 (10,9), 22 (8,1), 19 (6,7), 23 (9,4), 21 (7,8), 28 (11,7), 20 (6,11), 26 (11,11), 12 (1,2), 27 (11,7)]

    注意，Edge括号中，描述的为POI点的序号。

    标注Edge的关系，示例如图。

    image-20220809145416205

    6 使用AStar导航进行计算路径

    public static void AStarShortestPath(Graph graph, Node startNode, Node endNode) {
        AStarIterator.AStarFunctions aStarFunction = new AStarIterator.AStarFunctions(endNode) {
            @Override
            public double cost(AStarIterator.AStarNode aStarNode, AStarIterator.AStarNode aStarNode1) {
                Edge edge = aStarNode.getNode().getEdge(aStarNode1.getNode());
                System.out.println("edge=" + edge.getID() + ",Distance" + edge.getObject());
                /*SimpleFeature feature = (SimpleFeature) edge.getObject();
                Geometry geometry = (Geometry) feature.getDefaultGeometry();
                System.out.println(aStarNode.getH());
                return geometry.getLength();*/
//                return Integer.parseInt(String.valueOf(edge.getObject()));
                return 10;
            }

            @Override
            public double h(Node node) {
                return -10;
            }
        };
        Date start = new Date();
        AStarShortestPathFinder aStarPf = new AStarShortestPathFinder(graph, startNode, endNode, aStarFunction);
        try {
            aStarPf.calculate();
            Date end = new Date();
            System.out.println("AStar算法耗时：" + (end.getTime() - start.getTime()));
            aStarPf.getPath();
            System.out.println("AStar算法距离：" + getPathLength(aStarPf.getPath()));
/*            Iterator it = aStarPf.getPath().iterator();
            String result = "";
            while (it.hasNext()) {
                Node node = (Node) it.next();
                result = result + node.getObject().toString();
            }
            System.out.println("result:" + result);*/
        } catch (Exception e) {
            e.printStackTrace();
        }
    }

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38

    7 完整代码

    import org.geotools.data.DataStore;
import org.geotools.data.shapefile.ShapefileDataStore;
import org.geotools.data.simple.SimpleFeatureCollection;
import org.geotools.feature.FeatureIterator;
import org.geotools.graph.build.basic.BasicGraphBuilder;
import org.geotools.graph.build.basic.BasicGraphGenerator;
import org.geotools.graph.build.feature.FeatureGraphGenerator;
import org.geotools.graph.build.line.LineStringGraphGenerator;
import org.geotools.graph.path.AStarShortestPathFinder;
import org.geotools.graph.path.Path;
import org.geotools.graph.structure.Edge;
import org.geotools.graph.structure.Graph;
import org.geotools.graph.structure.Node;
import org.geotools.graph.structure.basic.BasicEdge;
import org.geotools.graph.traverse.standard.AStarIterator;
import org.geotools.referencing.GeodeticCalculator;
import org.geotools.referencing.crs.DefaultGeographicCRS;
import org.locationtech.jts.geom.Coordinate;
import org.opengis.feature.Feature;

import java.io.File;
import java.io.IOException;
import java.net.MalformedURLException;
import java.util.*;

public class NavigationUtil {
    
    /*public static void main(String[] args) throws IOException {

        File file = new File("E:\\公交路线.shp");
        //1.读取shp数据
        DataStore dataStore = readShapeFile(file);
        SimpleFeatureSource featureSource = dataStore.getFeatureSource(dataStore.getTypeNames()[0]);
        SimpleFeatureCollection simFeatureCollect = featureSource.getFeatures();
        System.out.println("shp文件原始线的个数：" + simFeatureCollect.size());
        //2.创建graph数据结构
        Graph graph = buildGraph(simFeatureCollect);
        Iterator iterator = graph.getNodes().iterator();
        List nodeArrayList = new ArrayList<>();
        while (iterator.hasNext()) {
            nodeArrayList.add(iterator.next());
        }
        System.out.println("节点：" + graph.getNodes());
        System.out.println("边：" + graph.getEdges());
        System.out.println(nodeArrayList.get(74));
        System.out.println(nodeArrayList.get(283));
        AStarShortestPath(graph, nodeArrayList.get(74), nodeArrayList.get(283));
    }*/

    public static void main(String[] args) throws IOException {
        buildNodeAndEdgeGraph();
    }

    public static void AStarShortestPath(Graph graph, Node startNode, Node endNode) {
        AStarIterator.AStarFunctions aStarFunction = new AStarIterator.AStarFunctions(endNode) {
            @Override
            public double cost(AStarIterator.AStarNode aStarNode, AStarIterator.AStarNode aStarNode1) {
                Edge edge = aStarNode.getNode().getEdge(aStarNode1.getNode());
                System.out.println("edge=" + edge.getID() + ",Distance" + edge.getObject());
                /*SimpleFeature feature = (SimpleFeature) edge.getObject();
                Geometry geometry = (Geometry) feature.getDefaultGeometry();
                System.out.println(aStarNode.getH());
                return geometry.getLength();*/
//                return Integer.parseInt(String.valueOf(edge.getObject()));
                return 10;
            }

            @Override
            public double h(Node node) {
                return -10;
            }
        };
        Date start = new Date();
        AStarShortestPathFinder aStarPf = new AStarShortestPathFinder(graph, startNode, endNode, aStarFunction);
        try {
            aStarPf.calculate();
            Date end = new Date();
            System.out.println("AStar算法耗时：" + (end.getTime() - start.getTime()));
            aStarPf.getPath();
            System.out.println("AStar算法距离：" + getPathLength(aStarPf.getPath()));
/*            Iterator it = aStarPf.getPath().iterator();
            String result = "";
            while (it.hasNext()) {
                Node node = (Node) it.next();
                result = result + node.getObject().toString();
            }
            System.out.println("result:" + result);*/
        } catch (Exception e) {
            e.printStackTrace();
        }
    }

    public static DataStore readShapeFile(File shpFile) throws MalformedURLException {
        ShapefileDataStore shapefileDataStore = new ShapefileDataStore(shpFile.toURI().toURL());
        return shapefileDataStore;
    }

    public static Graph buildNodeAndEdgeGraph() {
        //构造点
        BasicGraphGenerator basicGraphGenerator = new BasicGraphGenerator();
        BasicGraphBuilder basicGraphBuilder = new BasicGraphBuilder();
        Coordinate[] coordinates = new Coordinate[12];
        coordinates[0] = new Coordinate(104.069649, 30.542246);
        coordinates[1] = new Coordinate(104.071001, 30.542283);
        //此处省略好多点，麻烦自己手动采集
        List<Node> nodeList = new ArrayList<>();
        for (int i = 0; i < 12; i++) {
            Node node = basicGraphGenerator.getGraphBuilder().buildNode();
            basicGraphGenerator.getGraphBuilder().addNode(node);
            node.setObject(coordinates[i]);
            node.setID(i);
            node.setCount(i);
            node.setVisited(true);
            basicGraphBuilder.addNode(node);
            nodeList.add(node);
        }
        buildEdge(basicGraphBuilder, nodeList);
        System.out.println("nodeList:" + nodeList);
        AStarShortestPath(basicGraphBuilder.getGraph(), nodeList.get(1), nodeList.get(5));
        return basicGraphBuilder.getGraph();
    }

    public static Graph buildGraph(SimpleFeatureCollection simFeatureCollect) {
        //构造线
        LineStringGraphGenerator lineStringGen = new LineStringGraphGenerator();
        FeatureGraphGenerator featureGen = new FeatureGraphGenerator(lineStringGen);
        FeatureIterator iter = simFeatureCollect.features();
        while (iter.hasNext()) {
            Feature next = iter.next();
            featureGen.add(next);
        }
        iter.close();
        return featureGen.getGraph();
    }

    public static Graph buildEdge(BasicGraphBuilder basicGraphBuilder, List<Node> nodeList) {
        BasicGraphGenerator basicGraphGenerator = new BasicGraphGenerator();
        Map<Integer, String> map = new HashMap<>();
        map.put(1, "2");
        map.put(2, "3,12");
        map.put(3, "4,10");
        map.put(4, "5");
        map.put(5, "6");
        map.put(6, "7,11");
        map.put(7, "8");
        map.put(8, "1");
        map.put(9, "4");
        map.put(10, "9,11");
        map.put(11, "12,7");
        map.put(12, "7");
        for (Map.Entry<Integer, String> entry : map.entrySet()) {
            if (entry.getValue().split(",").length > 1) {
                String[] strings = entry.getValue().split(",");
                for (String string : strings) {
                    if (Integer.parseInt(string) == 12) {
                        string = "11";
                    }
                    Edge edge = new BasicEdge(nodeList.get(entry.getKey()), nodeList.get(Integer.parseInt(string)));
                    Long distance = getDistance(nodeList.get(entry.getKey()), nodeList.get(Integer.parseInt(string)));
                    edge.setObject(distance);
                    edge.setVisited(true);
                    basicGraphBuilder.addEdge(edge);
                }
            } else {
                Integer key = entry.getKey();
                if (Integer.parseInt(entry.getValue()) == 12) {
                    entry.setValue("11");
                }
                if (key == 12) {
                    key = 11;
                }
                //构造边
                Edge edge = new BasicEdge(nodeList.get(key), nodeList.get(Integer.parseInt(entry.getValue())));
                Long distance = getDistance(nodeList.get(key), nodeList.get(Integer.parseInt(entry.getValue())));
                edge.setObject(distance);
                edge.setVisited(true);
                basicGraphBuilder.addEdge(edge);
            }
        }
        System.out.println(basicGraphBuilder.getNodes());
        System.out.println(basicGraphBuilder.getGraph());
        return basicGraphBuilder.getGraph();
    }

    public static String getPathLength(Path path) {
        return path.toString();
    }

    //计算两个节点的经纬度距离
    public static Long getDistance(Node nodeA, Node nodeB) {
        // 84坐标系构造GeodeticCalculator
        GeodeticCalculator geodeticCalculator = new GeodeticCalculator(DefaultGeographicCRS.WGS84);
        Coordinate coordinate1 = (Coordinate) nodeA.getObject();
        Coordinate coordinate2 = (Coordinate) nodeB.getObject();
        // 起点经纬度
        geodeticCalculator.setStartingGeographicPoint(coordinate1.getX(), coordinate1.getY());
        // 末点经纬度
        geodeticCalculator.setDestinationGeographicPoint(coordinate2.getX(), coordinate2.getY());
        // 计算距离，单位：米
        double orthodromicDistance = geodeticCalculator.getOrthodromicDistance();
        return new Double(orthodromicDistance).longValue();
    }
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203

    8 参考文章

  • 相关阅读:
    Prometheus（三）node-exporter
    Ubuntu18.04遇到的nodejs的坑记录
    LeetCode 每日一题——1668. 最大重复子字符串
    【Azure 环境】Azure Resource Graph Explorer 中实现动态数组数据转换成多行记录模式 - mv-expand
    PostgreSQL（二）Procedural Language使用的最佳实践
    Leetcode 49.字母异位词分组
    MultiBank Group宣布在阿联酋和新加坡取得两项新牌照
    线程同步互斥机制
    什么耳机适合华为手机？适合华为手机的蓝牙耳机推荐
    PLC有几种编程语言以及它们的特点是什么
  • 原文地址：https://blog.csdn.net/An1090239782/article/details/126248144