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;
}
}
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;
}