PathSet* ObjectManager::getPathSet(){
if (pathSet_ == NULL) {
std::cout << "Creating path set" << std::endl;
PathApp app = getPathAlgoApp();
AONAssignment* aon = NULL;
if (getIfAdditive()) {
aon = new AONUsual(*getODMatrix(), getLabelCorrectingAlgo());
} else {
aon = new AONNonAdditive(*getODMatrix(), new NonAddShortestPathForAON(getNonAdditivePC(),
getNet()->getNbNodes(),
getOneSourceBSP(), getODMatrix()));
}
if (app == APP3) {
PathAlgoType algo = getPathAlgoType();
if (algo == PE ) {
pathSet_ = PathSet::createSetPEAPP3(getNet(), getODMatrix(), getShPath(),
getPathCost(),
getFloatValue("ZERO_FLOW"), getPathBasedFlowMove(),
aon);
} else if (algo == GP) {
pathSet_ = PathSet::createSetGPAPP3(getNet(), getODMatrix(), getShPath(),
getPathCost(),
getFloatValue("ZERO_FLOW"), getPathBasedFlowMoveGP(),
aon);
} else {
throw Error("Approach 3 is not implemented for this algorithm");
}
} else {
pathSet_ = PathSet::createSetWithStep(getNet(), getODMatrix(), getShPath(),
getPathCost(),
getFloatValue("ZERO_FLOW"), getPathBasedFlowMoveWithStep(),
aon);
}
std::cout << "Path set created" << std::endl;
}
return pathSet_;
};
void displayMinPath(Graph &graph, Map<coordT> &m, string &s1, string &s2) {
cout << "\nFinding shortest path using Dijkstra... " << endl;
int dequeueCount = 0;
Vector<Arc *> path = graph.findShortestPath(s1, s2, dequeueCount);
string prev = s1, curr;
for (int i = 1; i < path.size(); i++) {
curr = path[i]->start->name;
DrawLineBetween(m[prev], m[curr], "Red");
DrawFilledCircleWithLabel(m[curr], "Red", curr);
prev = curr;
}
DrawLineBetween(m[prev], m[s2], "Red");
double cost = getPathCost(path);
displayMinResult(s1, s2, cost, dequeueCount);
prev = s1;
for (int i = 0; i < path.size(); i++) {
curr = path[i]->finish->name;
DrawLineBetween(m[prev], m[curr], "Black");
DrawFilledCircleWithLabel(m[prev], "Black", prev);
prev = curr;
}
DrawFilledCircleWithLabel(m[prev], "Black", prev);
}
bool MSP3D::step(){
// std::cout << "Calculate Graph" << std::endl;
reducedGraph();
// std::set<kshortestpaths::BaseVertex*> res;
// std::map<int,int> neighboor_counts;
// for(int i=0;i<m_nodes.size();++i){
// res.clear();
// m_graph.get_adjacent_vertices(m_graph.get_vertex(i), res);
// if(neighboor_counts.count(res.size())==0){
// neighboor_counts[res.size()]=1;
// }else{
// neighboor_counts[res.size()]=neighboor_counts[res.size()]+1;
// }
//
// //check graph
//// std::cout << std::endl << "vertex " << i << " ID " << m_graph.get_vertex(i)->getID() <<std::endl;
//// for(std::set<kshortestpaths::BaseVertex*>::iterator it=res.begin(),end=res.end();it!=end;++it){
//// //kshortestpaths::BaseVertex* k=*it;
//// std::cout << "neighboor with " << (*it)->getID() <<std::endl;
//// res.clear();
//// }
// }
// std::cout << std::endl << "neighboor count check" << std::endl;
// std::cout << "nb neighboors => nb_nodes" << std::endl;
// for (std::map<int,int>::iterator it=neighboor_counts.begin(); it!=neighboor_counts.end(); ++it){
// std::cout << it->first << " => " << it->second << std::endl;
// }
// shortest path
kshortestpaths::YenTopKShortestPathsAlg yenAlg(m_graph, m_graph.get_vertex(m_start_index),m_graph.get_vertex(m_end_index));
bool got_next=false;
// std::cout << "Solve shortest path" << std::endl;
if(m_visited.count(m_current_coord)==0){
// std::cout << "First visit" << std::endl;
if(yenAlg.has_next()){
// std::cout << "Existing path" << std::endl;
got_next=true;
m_visited[m_current_coord]=0;
}
}else{
for(int i=0;i<m_visited[m_current_coord];++i){
if(yenAlg.has_next()){
yenAlg.next();
}
}
if(yenAlg.has_next()){
got_next=true;
}
}
// std::cout << "End of step update" << std::endl;
//TODO: check if the path contains obstacles (cost > M) if yes change get next to false
//if solution
if(got_next){
//go forward // if goal return false;
kshortestpaths::BasePath* result =yenAlg.next();
// std::cout << "Cost: " << result->Weight() << " Length: " << result->length() << std::endl;
// std::stringstream it_name;
// it_name << "iteration" << m_nb_step << ".ot";
// visu(std::string(it_name.str()),result);
if(result->Weight()>=m_M){
//no path without obstacles from current to finish
// std::cout << "shortest path with obstacles" << std::endl;
got_next=false;
}else{
// std::cout << "shortest path found" << std::endl;
// for(int i=0;i<result->length();++i)
// {
// std::cout << m_nodes[result->GetVertex(i)->getID()].first;
// std::cout << "->";
// }
// std::cout << std::endl << "*********************************************" << std::endl;
int next_point_id=result->GetVertex(1)->getID();
//do stuff to prepare next iteration
m_visited[m_current_coord]= m_visited[m_current_coord]+1;
// std::cout<<"before adding element"<< std::endl;
// for(std::deque<octomap::point3d>::iterator it=m_current_path.begin(),end=m_current_path.end();it!=end;++it){
// std::cout<< (*it) << std::endl;
// }
m_current_path.push_back(m_nodes[next_point_id].first);
// std::cout<<"after adding element"<< std::endl;
// for(std::deque<octomap::point3d>::iterator it=m_current_path.begin(),end=m_current_path.end();it!=end;++it){
// std::cout<< (*it) << std::endl;
// }
if(getPathCost()>=m_M){
//no path without obstacles from start to current
return false;
}
//m_current_point;
m_current_coord=m_nodes[next_point_id].first;
if(next_point_id==m_end_index){
std::cout << "goal reached" << std::endl;
m_path_found=true;
return false;
}else{
return true;
}
}
}
if(!got_next){
std::cout << "shortest path not found" << std::endl;
//go back // if path empty => no solution return false
m_visited[m_current_coord]=0;
m_current_path.pop_back();
if(m_current_path.size()==0){
//no possible path
return false;
}else{
m_current_coord=m_current_path.back();
return true;
}
}
}
