如何完善Dijkstra算法，而不是找到最佳路径

这是我对众所周知的Dijkstra算法的实现：

std::vector<unsigned> RouteFinder::findPathBetweenIntersections(unsigned intersect_id_start, unsigned intersect_id_end) {
    //Get number of intersections and reference graph from streetGraph class
    StreetGraph* streetGraph = StreetGraph::getGraphPointer();
    unsigned intersectionCount = streetGraph->getIntersectioncount();
    std::vector<attachedSegments> referenceGraph = streetGraph->getStreetGraph();
    /*Initialize:
     * min_distance: Min Distance to get to index node
     * active_vertices: The nodes to check next
     * cameAlong: Street Segments used to get to the node
     * cameFrom: Intersections taken to get to a node
     */
    vector<double> min_distance( intersectionCount, DBL_MAX );
    min_distance[ intersect_id_start ] = 0.0;
    set< pair<double,unsigned> > active_vertices;
    active_vertices.insert( {0.0,intersect_id_start} );
    vector<unsigned> cameAlong(intersectionCount,UINT_MAX);
    vector<unsigned> cameFrom(intersectionCount,0);
    //For each node in active_vertices
    while (!active_vertices.empty()) {
        unsigned currentNode = active_vertices.begin()->second;
        if (currentNode == intersect_id_end) return buildPath(cameFrom, cameAlong, currentNode, intersect_id_start);
        active_vertices.erase( active_vertices.begin() );
        for (auto edge : referenceGraph[currentNode].streetSegments) 
            if (min_distance[get<2>(edge)] > min_distance[currentNode] + get<0>(edge)) {
                //If the new distance is better than the one that is there
                //Remove the previous data
                active_vertices.erase( { min_distance[get<2>(edge)], get<2>(edge) } );
                //Calculate the better distance and replace it
                min_distance[get<2>(edge)] = min_distance[currentNode] + get<0>(edge);
                //Add 15 seconds if the street has changed
                if ((cameAlong[currentNode] != UINT_MAX
                        && getStreetSegmentInfo(cameAlong[currentNode]).streetID != getStreetSegmentInfo(get<1>(edge)).streetID)
                        ) {
                    min_distance[get<2>(edge)] = min_distance[get<2>(edge)] + .25;
                }
                active_vertices.insert( { min_distance[get<2>(edge)], get<2>(edge) } );
                //Record where you came from
                cameAlong[get<2>(edge)] = get<1>(edge);
                cameFrom[get<2>(edge)] = currentNode;
            }
    }
    //Return nothing if nothing found
    vector<unsigned> nothing;
    return nothing;
}

我的图是一个名为"intersectionNode"的结构向量。每个"交叉节点"都有一个tuple<double weight,int streetSegment,int nextIntersection>的向量（以及其他有用的信息）。

我根据我在网上和朋友那里找到的示例调整了我的实现，它非常快。但它似乎并没有返回最快的路径。有什么错误跳出来给你的，有什么调试提示吗？

此外，我还加入了 .25 分钟（15 秒）的跑步惩罚。

感谢您的帮助！