java求两个gps点夹角

转载

编程小达人 2024-10-02 23:13:43

文章标签 java求两个gps点夹角 System 最短距离最短路径 文章分类 Java 后端开发

一、问题

求下图中节点0到节点5之间的最短距离

java求两个gps点夹角_java求两个gps点夹角

二、方法

Dijkstra最短路由算法。本文不再赘述，直接上代码，如果不懂，可以参考文章

三、代码

package dijkstra;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;

public class MyDijkstra {

    public static void main(String[] args) {
        int[][] nums = {{0, 6, 3, -1, -1, -1},
                {6, 0, 2, 5, -1, -1},
                {3, 2, 0, 3, 4, -1},
                {-1, 5, 3, 0, 2, 3},
                {-1, -1, 4, 2, 0, 5},
                {-1, -1, -1, 3, 5, 0}};
        getShortestPaths(nums, 0, 5);

    }

    /**
     * Dijkstra算法，求任意两点之间的最短路径
     *
     * @param nums：二维矩阵，代表两个节点之间的距离，其中-1代表两点之间无法直接相连
     * @param src：源节点
     * @param des：目的节点
     * @return 源节点到目的节点之间的最短距离
     */

    public static int getShortestPaths(int[][] nums, int src, int des) {
        int[] result = new int[nums.length]; // 用于存放节点src到其它节点的最短距离
        int[] preNodeIndex = new int[6]; // preNodeIndex[i]表示节点i前一个节点的编号
        boolean[] visited = new boolean[nums.length]; // 用于判断节点是否被遍历
        ArrayList<Integer> path = new ArrayList<Integer>(); // 用于存放(src, des)之间的最短路径

        // step1：初始化，将src节点设置为已经访问，并且初始化该节点到其他所有节点之间的距离
        visited[src] = true;
        path.add(src);
        System.out.println("源节点是: " + src + ", 目的节点是: " + des + "\n");

        for (int i = 0; i < nums.length; i++) {
            result[i] = nums[src][i];
        }

        while (true) {
            // step2：找到剩余未访问的节点中，距离src最近的一个节点，作为curNode
            int curNode = getNearestNode(nums, visited, result);
            System.out.println("下一个距离源节点" + src + " 最近的节点是: " + curNode);

            // step3：更新所有节点距离src节点的最短距离（需要考虑curNode）
            update(nums, curNode, visited, result, path, preNodeIndex);
            System.out.println("更新后的result为: " + Arrays.toString(result));

            if (curNode == des) {
                break;
            }
        }

        System.out.println("\n每个节点的上一个节点是: " + Arrays.toString(preNodeIndex));

        // step4：根据preNodeIndex从目的节点倒退经过的最短路由节点
        String shortestPath = getShortestPath(preNodeIndex, src, des);
        System.out.println("源节点" + src + "到目的节点" + des + "经过的最短路径是: " + shortestPath);
        System.out.println("源节点" + src + "到目的节点" + des + "之间的最短距离是: " + result[des]);
        return result[des];
    }

    // 找到剩余未访问的节点中，距离源节点最近的一个节点编号
    public static int getNearestNode(int[][] nums, boolean[] visited, int[] result) {
        int minIndex = -1;
        int minLength = Integer.MAX_VALUE;
        for (int i = 0; i < nums.length; i++) {
            if (!visited[i] && result[i] != -1 && result[i] < minLength) {
                minIndex = i;
                minLength = result[i];
            }
        }

        return minIndex;
    }

    // 更新与curNode节点直接相连接的未被访问的节点距离，专业术语叫 “松弛”
    public static void update(int[][] nums, int curNode, boolean[] visited, int[] result, ArrayList<Integer> path, int[] preNodeIndex) {
        visited[curNode] = true;
        path.add(curNode);

        for (int j = 0; j < nums.length; j++) {
            // 节点j未访问过 && 节点curNode与节点j之间需要直接相连
            if (!visited[j] && nums[curNode][j] != -1) {
                // 更新result[j]
                if (result[j] > result[curNode] + nums[curNode][j] || result[j] == -1) {
                    result[j] = result[curNode] + nums[curNode][j];
                    preNodeIndex[j] = curNode; // 当前节点j的前一个节点选择了curNode
                }
            }
        }
    }

    // 根据preNodeIndex从目的节点倒退经过的最短路由节点
    public static String getShortestPath(int[] preNodeIndex, int src, int des) {
        ArrayList<String> path = new ArrayList<>();

        int index = des;
        path.add(String.valueOf(index));

        while (index != src) {
            path.add(String.valueOf(preNodeIndex[index]));
            index = preNodeIndex[index];
        }
        Collections.reverse(path);
        StringBuilder sb = new StringBuilder();
        for (String item : path) {
            sb.append(item).append("->");
        }

        return sb.substring(0, sb.length() - 2);
    }
}