A*算法在三维空间实现C++（2）——图的生成

接下来开始为生成一个图做准备。

1、根据一个顶点，构建其可行走的26个方向的剩余顶点

方法：遍历写好的directions集合，将顶点坐标与集合中的每一个方向进行计算，得到可能的26个方向的顶点坐标，保存在set集合中。

void build_directions(std::set<Point>& possible_directions, const Point& own_coords)
{
	// 根据自己的坐标生成所有26个可能的相邻坐标。
	for (const auto& vect : directions)
	{
		double d_x = std::get<0>(vect) + std::get<0>(own_coords);
		double d_y = std::get<1>(vect) + std::get<1>(own_coords);
		double d_z = std::get<2>(vect) + std::get<2>(own_coords);

		Point d_point(d_x, d_y, d_z);
		possible_directions.insert(d_point);
	}
}

2、构建一个三维空间的所有顶点

这里构建一个radius=3的立方体（3*3*3），那么直接三重循环将信息保存在graph的vertex_map中就好。

int id_count = 0;
	for (int i = 0; i < radius; i++)
	{
		for (int j = 0; j < radius; j++)
		{
			for (int k = 0; k < radius; k++)
			{
				graph.vertex_map.insert(std::make_pair(id_count, std::make_shared<GVertex>(id_count)));
				id_count++;
				// graph-vertex中id是从0开始的
			}
		}
	}

3、如何生成每个顶点与邻居节点之间的边？

生成edge比较复杂，首先第一个顶点直接生成26个方向的边即可。 那后面的顶点呢？生成的边可能会与前面的已经生成的边有重复，所以还需要进一步判断。

直接上代码；

template <typename T_Graph>
void map_nodes(T_Graph& graph, unsigned int& node_id, int& edge_ids_taken, int& node_ids_taken)
{
	// 创建包含节点node_id的所有邻接关系的邻接向量    <edge_id,neighbour_id>
	std::vector<std::pair<int, int>> adjacencies;

	// 设置所有可能的方向26个
	std::set<Point> possible_directions;

	// 如果id为0，则为第一个节点。所有其他节点都是围绕它构建的，所以已经有了坐标
	if (node_id == 0)
	{
		// 第一个节点的坐标  后面可以改
		graph.vertex_map.at(node_id).get()->coord = Point(0, 0, 0);
	}

	// 根据自己坐标求得可能的26个方向坐标
	build_directions(possible_directions, graph.vertex_map.at(node_id).get()->coord);

	// 对照邻接表，看是否已经被列为邻接点？
	for (const auto& elem : graph.adjacency_map)
	{
		bool neigh = false;
		//vector of edge_id, neighbour_id
		for (auto& ed_ne : elem.second)
		{
			//如果是邻接点
			if (ed_ne.second == node_id)
			{
				// elem是当前节点的邻居
				// adjacencies  <edge_id,neighbour_id>
				adjacencies.push_back(std::make_pair(ed_ne.first, elem.first));
				neigh = true;
			}
		}
	}



	std::set<int> adj_nodes;          //用于保存邻接点的id——集合
	std::set<int> to_add;

	// 填充adj_nodes集合
	for (const auto& elem : adjacencies)
	{
		// elem.second   =  neighbour_id
		adj_nodes.insert(elem.second);
	}

	// 对于所有邻接向量的节点  adjacencies <edge_id,neighbour_id>
	for (const auto& elem : adjacencies)
	{
		// elem.second=neighbour_id
		// graph.adjacency_map.at(elem.second) 获取到neighbour_id节点的邻接关系，边和它的邻居<edge_id,neighbour'的neigh_id>
		for (auto& ed_ne : graph.adjacency_map.at(elem.second))
		{
			
			if (possible_directions.count(graph.vertex_map.at(ed_ne.second).get()->coord))
			{
				
				// 检查是否已近被列为邻接点了？
				if (!adj_nodes.count(ed_ne.second))
				{
					
					to_add.insert(ed_ne.second);
				}
			}
		}
	}


	// 创建边
	for (const auto& id : to_add)
	{
		// adjacencies   <edge_id,neighbour_id>
		adjacencies.push_back(std::make_pair(edge_ids_taken, id));
		graph.edge_map.insert(std::make_pair(edge_ids_taken, std::make_shared<Edge>(edge_ids_taken)));
		edge_ids_taken++;
	}


	// 清除所有可能的方向
	for (const auto& coord : adjacencies)
	{
		possible_directions.erase(graph.vertex_map.at(coord.second).get()->coord);
	}

	if (!graph.vertex_map.count(node_ids_taken))
	{
		node_ids_taken = -1;
	}

	// 不存在空闲节点
	if (node_ids_taken != -1)
	{
		//对于每个可能的方向，在adj vector中添加自由节点，获取其坐标
		for (const auto& coord : possible_directions)
		{
			if (node_ids_taken != -1)
			{

				//推新边节点，增加计数器，设置节点坐标
				adjacencies.push_back(std::make_pair(edge_ids_taken, node_ids_taken));
				graph.edge_map.insert(std::make_pair(edge_ids_taken, std::make_shared<Edge>(edge_ids_taken)));


				graph.vertex_map.at(node_ids_taken).get()->coord = coord;

				node_ids_taken++;
				edge_ids_taken++;

				if (!graph.vertex_map.count(node_ids_taken))
				{
					node_ids_taken = -1;
				}
			}
		}
	}

	graph.adjacency_map.insert(std::make_pair(node_id, adjacencies));
}