dijkstra 路径搜索算法的c++简单实现

参考

基于知乎文章https://zhuanlan.zhihu.com/p/51112799

图

codes

#include <iostream>
#include <deque>
#include <vector>

#include <limits.h>

using namespace std;



int main(int argc, char *args[])
{
	#define COL_SIZE 6
	#define ROW_SIZE 6
	int map[COL_SIZE][ROW_SIZE];

	for(int col=0; col<COL_SIZE; col++)
	{
		for(int row=0; row<ROW_SIZE; row++)
			map[col][row] = INT_MAX;
	}

	map[0][0] = 0;
	map[0][1] = INT_MAX;
	map[0][2] = 10;
	map[0][3] = INT_MAX;
	map[0][4] = 30;
	map[0][5] = 100;

	map[1][0] = INT_MAX;
	map[1][1] = 0;
	map[1][2] = 5;
	map[1][3] = INT_MAX;
	map[1][4] = INT_MAX;
	map[1][5] = INT_MAX;

	map[2][0] = INT_MAX;
	map[2][1] = INT_MAX;
	map[2][2] = 0;
	map[2][3] = 50;
	map[2][4] = INT_MAX;
	map[2][5] = INT_MAX;

	map[3][0] = INT_MAX;
	map[3][1] = INT_MAX;
	map[3][2] = INT_MAX;
	map[3][3] = 0;
	map[3][4] = INT_MAX;
	map[3][5] = 10;

	map[4][0] = INT_MAX;
	map[4][1] = INT_MAX;
	map[4][2] = INT_MAX;
	map[4][3] = 20;
	map[4][4] = 0;
	map[4][5] = 60;


	map[5][0] = INT_MAX;
	map[5][1] = INT_MAX;
	map[5][2] = INT_MAX;
	map[5][3] = INT_MAX;
	map[5][4] = INT_MAX;
	map[5][5] = 0;

	int start_index = 0; //from where

	int dist_min[ROW_SIZE]; //save dist min from source to others.
	for(int row=0; row<ROW_SIZE; row++)
	{
		dist_min[row] = map[start_index][row];
	}


	std::deque<std::vector<int> > route;
	for(int index=0; index<ROW_SIZE; index++ )
	{
		route.emplace_back();
		auto &back = route.back();
		back.push_back(start_index);
		back.push_back(index);
	}
	std::cout << "start route :" <<  std::endl;

	auto print_route = [&]()
	{
		std::cout <<"--------------------------------------------------" << std::endl;
		for(auto &r : route)
		{
			std::cout << "get: ";
			for(auto &s : r)
			{
				std::cout << " " << s;
			}
			std::cout << std::endl;
		}
		std::cout <<"--------------------------------------------------" << std::endl;
	};
	print_route();

	std::deque<int> t_set;

	t_set.push_back(start_index);
	std::cout << "start dist min array:" 
	<< " "  << dist_min[0]
	<< " "  << dist_min[1]
	<< " "  << dist_min[2]
	<< " "  << dist_min[3]
	<< " "  << dist_min[4]
	<< " "  << dist_min[5]
	<< std::endl;
	while(t_set.size() < ROW_SIZE)
	{
		//find_left_min_value
		int next_t_node = -1;
		for(int index=0; index<ROW_SIZE; index++)
		{
			bool find_loop=false;
			for(auto &val : t_set)
			{
				if(val == index)
				{
					find_loop=true;
					break;
				}
			}
			if(find_loop == false)
			{
				if(next_t_node == -1)
				{
					next_t_node = index;
				}
				else
				{
					//find current min index
					if(dist_min[next_t_node] > dist_min[index])
					{
						next_t_node = index;
					}
				}
			}
		}
		if(next_t_node == -1)
		{
			std::cout << "cannot find next t node " << std::endl;
			break;
		}

		std::cout << "find next t node index=" << next_t_node << std::endl;

		t_set.push_back(next_t_node);
		for(auto &t:t_set)
			std::cout << " " << t ;
		std::cout << std::endl;

		auto &one_route_node = route.at(next_t_node);
		//one_route_node.push_back(next_t_node);
		print_route();

		int prev_weight = 0;
		auto iter = one_route_node.begin();
		auto iter_end = one_route_node.end();
		for(;iter!=iter_end; )
		{
			auto iter_next = iter+1;
			if(iter_next == iter_end)
				break;
			if(map[*iter][*iter_next] == INT_MAX) //not reach
			{
				prev_weight = INT_MAX;
				break;
			}
			else
			prev_weight += map[*iter][*iter_next];

			iter++;
		}
		if(prev_weight == INT_MAX)
		{
			std::cout << "cur path not reach, continue " << std::endl; 
			continue;
		}

		//update dist min array
		for(int row=0; row<ROW_SIZE; row++)
		{
			bool find_loop=false;
			for(auto &val : t_set)
			{
				if(val == row)
				{
					find_loop=true;
					break;
				}
			}
			if(find_loop == false)
			{
				if((map[next_t_node][row] != INT_MAX ) 
				&& (prev_weight + map[next_t_node][row]) < dist_min[row])
				{
					dist_min[row] = prev_weight + map[next_t_node][row];
					auto &target_one_route_node = route.at(row);
					target_one_route_node = route.at(next_t_node);
					target_one_route_node.push_back(row);
					std::cout << "update dist min index=" << row << " to new value=" << dist_min[row] << std::endl;
				}

			}
		}
		std::cout << "dist min array:" 
		<< " "  << dist_min[0]
		<< " "  << dist_min[1]
		<< " "  << dist_min[2]
		<< " "  << dist_min[3]
		<< " "  << dist_min[4]
		<< " "  << dist_min[5]
		<< std::endl;
		print_route();
	}
	std::cout << "last dist min array:" 
	<< " "  << dist_min[0]
	<< " "  << dist_min[1]
	<< " "  << dist_min[2]
	<< " "  << dist_min[3]
	<< " "  << dist_min[4]
	<< " "  << dist_min[5]
	<< std::endl;
}