k最短路径算法 matlab

以下是一个基于Yen算法的k最短路径matlab实现的例子：

function [shortestPaths] = kShortestPaths(Y, startNode, endNode, k)
% Y: 邻接矩阵
% startNode: 起点
% endNode: 终点
% k: k短路
% shortestPaths: k个最短路径

% 初始化
n = size(Y, 1);
dist = inf(n, 1);
prev = zeros(n, 1);
visited = false(n, 1);
dist(startNode) = 0;

% 运行Dijkstra算法
for i = 1:n
    % 找到未访问节点中距离最小的节点
    [~, u] = min(dist(~visited));
    if isempty(u)
        break;
    end
    visited(u) = true;
    % 更新与u相邻节点的距离
    for v = 1:n
        if Y(u, v) > 0 && ~visited(v)
            alt = dist(u) + Y(u, v);
            if alt < dist(v)
                dist(v) = alt;
                prev(v) = u;
            end
        end
    end
end

% 初始化
shortestPaths = cell(k, 1);
A = {};
heap = {};

% 将起点加入A
A{1} = startNode;
heap{1} = 0;

% 运行k次
for kk = 1:k
    % 找到当前路径
    currentNode = endNode;
    path = [];
    while currentNode ~= startNode
        path = [currentNode path];
        currentNode = prev(currentNode);
    end
    path = [startNode path];
    shortestPaths{kk} = path;
    % 如果已经找到k条路径，退出
    if kk == k
        break;
    end
    % 对当前路径进行处理
    for i = 1:length(path)-1
        spurNode = path(i);
        rootPath = path(1:i);
        % 从A中移除rootPath之后的节点
        for j = 1:length(A)
            if isequal(A{j}, rootPath)
                A = A(1:j-1);
                heap = heap(1:j-1);
                break;
            end
        end
        % 对每个rootPath的边进行处理
        for j = 1:length(rootPath)-1
            if rootPath(j) ~= spurNode
                continue;
            end
            spurPath = rootPath(j:end);
            totalPath = [spurPath path(i+1:end)];
            % 计算总距离
            totalDist = 0;
            for k = 1:length(totalPath)-1
                totalDist = totalDist + Y(totalPath(k), totalPath(k+1));
            end
            % 将总距离加入堆中
            heap{end+1} = totalDist;
            A{end+1} = totalPath;
        end
    end
    % 如果堆为空，退出
    if isempty(heap)
        break;
    end
    % 找到堆中距离最小的路径
    [~, idx] = min(cell2mat(heap));
    path = A{idx};
    % 将该路径加入A
    A = A([1:idx-1 idx+1:end]);
    heap = heap([1:idx-1 idx+1:end]);
    % 更新dist和prev
    for i = 1:n
        dist(i) = inf;
        prev(i) = 0;
    end
    for i = 1:length(A)
        totalDist = heap{i};
        path = A{i};
        for j = 1:length(path)-1
            u = path(j);
            v = path(j+1);
            alt = totalDist - Y(u, v) + Y(v, u);
            if alt < dist(v)
                dist(v) = alt;
                prev(v) = u;
            end
        end
    end
end