示例#1
0
文件: Dijkstras.cpp 项目: Mikulas/Ada
void Container::compute()
{
	while (nodes.size() > 0)
	{
		Node* smallest = ExtractSmallest(nodes);
		vector<Node*>* adjacentNodes = AdjacentRemainingNodes(smallest);

		for (unsigned int i = 0; i < adjacentNodes->size(); ++i)
		{
			Node* adjacent = adjacentNodes->at(i);
			float distance = Distance(smallest, adjacent) + smallest->distanceFromStart;
			
			if (distance < adjacent->distanceFromStart)
			{
				adjacent->distanceFromStart = distance;
				adjacent->previous = smallest;
			}
		}
		delete adjacentNodes;
	}
}
void Dijkstras()
{
	while (nodes.size() > 0)
	{
		Node* smallest = ExtractSmallest(nodes);
		vector<Node*>* adjacentNodes = 
			AdjacentRemainingNodes(smallest);

		const int size = adjacentNodes->size();
		for (int i=0; i<size; ++i)
		{
			Node* adjacent = adjacentNodes->at(i);
			int distance = Distance(smallest, adjacent) +
				smallest->distanceFromStart;
			
			if (distance < adjacent->distanceFromStart)
			{
				adjacent->distanceFromStart = distance;
				adjacent->previous = smallest;
			}
		}
		delete adjacentNodes;
	}
}
示例#3
0
void Dijkstras(map_loader::GraphMap gmap, position_tracker::Position init_pos, position_tracker::Position dst_pos, path_navigator::Waypoints *w)
{
	//vector<map_loader::Node> nodes = gmap.nodes;
	//vector<map_loader::Edge> edges = gmap.edges;

	if(nodes.size() == 0) return;	

	erased.clear();
	previous.clear();
	waypoints.clear();
	erased_num = 0;
	for(int i = 0; i < nodes.size(); i++)
	{
		nodes.at(i).distanceFromStart = 1000000;
		erased.push_back(0);
		//hold the previous node of all the nodes in the shortest path
		previous.push_back(-1);
	}


	
	//find node (init_node) in the graph map that is closest to the init_pos
	vector<map_loader::Node>::iterator itl;
	map_loader::Node init_node, dst_node;
	double min_dist = 10000;
	int min_ind = -1;
        for(itl = nodes.begin(); itl != nodes.end(); ++itl)
	{
		double dist = sqrt(pow(init_pos.x - (*itl).p.x, 2) + pow(init_pos.y - (*itl).p.y, 2));			
		if(dist < min_dist)
		{
			min_dist = dist;
			min_ind = itl - nodes.begin();
		} 
		init_node = nodes[min_ind];		
	}
	std::cout << "Init_pos: It's closest node is (" << init_node.p.x << ", " << init_node.p.y << ")" << std::endl;
	std::cout << "min_ind = " << min_ind << std::endl;
	nodes.at(min_ind).distanceFromStart = 0; // set start node
	

	//find node (dst_node) in the graph map that is closest to the dst_pos
	min_dist = 10000.0;
	min_ind = -1;
	for(itl = nodes.begin(); itl != nodes.end(); ++itl)
	{
		double dist = sqrt(pow(dst_pos.x - (*itl).p.x, 2) + pow(dst_pos.y - (*itl).p.y, 2));			
			if(dist < min_dist)
			{
				min_dist = dist;
				min_ind = itl - nodes.begin();
			} 
		
		dst_node = nodes[min_ind];
	}
	std::cout << "Dst_pos: It's closest node is (" << dst_node.p.x << ", " << dst_node.p.y << ")" << std::endl;
	int dst_min_ind = min_ind;

	//---------------------------------------------


	//while not all the nodes have been examined
	while (erased_num <= nodes.size())
	{

		cout << "sum of erased = " << accumulate(erased.begin(), erased.end(), 0) << endl;

		int smallest_id = ExtractSmallest(nodes);
		cout << "smallest_id = " << smallest_id << endl;
		if(smallest_id == -1) 
		{
			ROS_INFO("No map has been loaded");
			return;
		}

		vector<int> adjacentNodes = AdjacentRemainingNodes(nodes[smallest_id]);
		cout << "number of adjacent nodes = " << adjacentNodes.size() << endl;

		const int size = adjacentNodes.size();
		for (int i=0; i<size; ++i)
		{
			Node adjacent = nodes.at(adjacentNodes.at(i));
			int distance = Distance(nodes.at(smallest_id), adjacent) + nodes.at(smallest_id).distanceFromStart;
			cout << "distance " << distance << " with node " << adjacentNodes.at(i) << endl;
			if (distance < adjacent.distanceFromStart)
			{
				nodes.at(adjacentNodes.at(i)).distanceFromStart = distance;
				previous[adjacent.id] = smallest_id;
			}
		}
	}

	//---------------------------------------------
	PrintLoadShortestRouteTo(nodes[dst_min_ind]); //stores the path in the waypoints vector => 	
 	Node *tmp_dst_node = new Node();
	tmp_dst_node->p = dst_pos;
	waypoints.push_back(*tmp_dst_node); //optional: add also the destination position (assuming it doesn't exist already as a node in the map)
	w->waypoints = waypoints;
}