bool Dlite::ComputeShortestPath(LocalMap &map)
{
while (OPEN.top_key_less_than(CalculateKey(*start)) || start->rhs != start->g) {
++number_of_steps;
Node* current = OPEN.get();\
Key old_key = current->key;
Key new_key = CalculateKey(*current);
if (old_key < new_key) {
current->key = new_key;
OPEN.put(current);
} else if (current->g > current->rhs) {
current->g = current->rhs;
OPEN.pop();
for (auto elem : GetSuccessors(current, map)) {
if (elem->point != map.goal && elem->rhs > current->g + GetCost(elem->point, current->point, map)) {
elem->parent = current;
elem->rhs = current->g + GetCost(elem->point, current->point, map);
}
UpdateVertex(elem); // !!!<-apparently OK
}
} else {
//double old_g = current->g;
current->g = std::numeric_limits<double>::infinity();
std::vector<Node* > succ = GetSuccessors(current, map);
succ.push_back(current);
for (auto elem : succ) {
if (elem->point != map.goal && elem->parent->point == current->point) {
Node min_val = GetMinPredecessor(elem, map);
elem->rhs = min_val.rhs;
if(min_val.parent)
elem->parent = min_val.parent;
}
UpdateVertex(elem);
}
}
/*if(current->parent) {
std::cout << current->point << "g " << current->g << " rhs" << current->rhs <<
current->parent->point << std::endl;
} */ //std::cout << OPEN.top_key().k1 << std::endl;
//OPEN.print_elements();
}
if (start->rhs != std::numeric_limits<double>::infinity()) {
current_result.pathfound = true;
current_result.numberofsteps = number_of_steps;
current_result.nodescreated = NODES.size();
//std::cout << start->rhs << std::endl;
//MakePrimaryPath(start);
//current_result.lppath = &path;
//map.PrintPath(curpath);
//for (auto elem : curpath) path.push_back(elem);
return true;
} else {
current_result.pathfound = false;
current_result.pathlength = 0;
return false;
}
return false;
}
bool GetPath(void *data, xyLoc s, xyLoc g, std::vector<xyLoc> &path){
/* clear open list */
#ifdef _BUCKET_LIST_H
open_list.Reset();
#elif HEAP2_H
open_list.reset();
open_list.reserve(3000);
#else
open_list.clear();
#endif
/* initialize closed list */
closed_list.resize(map.size());
for( int i=0; i<map.size(); i++){
closed_list[i] = false;
}
/* start location */
start = s;
/* goal location */
goal = g;
m_currentIteration++;
/* start state */
State* start = (State*)malloc(sizeof(State));//new State;
#ifdef _SPEEDY_H
start->set_values(distance(s,g),s);
#endif
#ifdef _WASTAR_H
start->set_values(0,distance(s,g),s);
#endif
start->set_parent(NULL);
start->m_iteration = m_currentIteration;
/* push start state */
#ifdef _BUCKET_LIST_H
open_list.Push(start);
#elif HEAP2_H
open_list.add(start);
#else
open_list.push_back(start);
std::push_heap(open_list.begin(),open_list.end(),StateComparator());
#endif
/* Speedy */
#ifdef _BUCKET_LIST_H
while( !open_list.Empty() ){
#else
while( !open_list.empty() ){
#endif
#ifdef _BUCKET_LIST_H
State* curr = open_list.Pop();
#elif HEAP2_H
State* curr = open_list.remove();
#else
std::pop_heap (open_list.begin(),open_list.end(),StateComparator());
State* curr = open_list.back();
open_list.pop_back();
#endif
if( FindGoal(curr) ){
return_path(curr, path);
#ifdef DEBUG
display_search();
#endif
break;
}
GetSuccessors(curr->pos(), succ);
for( int i=0; i<succ.size(); i++){
State* nb = (State*)malloc(sizeof(State));//new State;
xyLoc pos = succ[i];
#ifdef _SPEEDY_H
/* estimate total number of moves to go */
nb->set_values(distance(pos,g),pos);
#endif
#ifdef _WASTAR_H
/* octile distance heuristic */
if( succ[i].x == curr->pos().x || succ[i].y == curr->pos().y ){
nb->set_values(curr->g()+1.0f,distance(pos,g),pos);
}
else{
nb->set_values(curr->g()+sqrt(2),distance(pos,g),pos);
}
#endif
nb->set_parent(curr);
nb->m_iteration = m_currentIteration;
int index = GetIndex(succ[i]);
if( !closed_list[index] && map[index] ){
closed_list[GetIndex(pos)] = true; //add state to closed list
#ifdef _BUCKET_LIST_H
open_list.Push(nb);
#elif HEAP2_H
open_list.add(nb);
#else
open_list.push_back(nb);
std::push_heap(open_list.begin(),open_list.end(),StateComparator());
#endif
}
}
}
return true;
}
SearchResult Dlite::FindThePath(LocalMap &map, const Map& const_map, EnvironmentOptions options)
{
opt = options;
std::chrono::time_point<std::chrono::system_clock> tstart, end;
tstart = std::chrono::system_clock::now();
number_of_steps = 0;
current_result.pathlength = 0;
Initialize(map);
last = start;
if(!ComputeShortestPath(map))
std::cout << "OOOPS\n";
else
std::cout << "Done\n";
std::cout << current_result.pathlength <<std::endl;
while(start->point != goal->point) {
Cell jump = start->point;
Node min_val = GetMinPredecessor(start, map);
path.push_back(*start);
if (!min_val.parent) {
OPEN.remove_if(start);
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
} else {
current_result.pathlength += GetCost(start->point, min_val.parent->point, map);
start = min_val.parent;
}
min_val = GetMinPredecessor(start, map);
while (opt.allowjump && euclid_dist(jump, min_val.parent->point) < radius && start->point != goal->point) {
path.push_back(*start);
if (!min_val.parent) {
OPEN.remove_if(start);
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
} else {
current_result.pathlength += GetCost(start->point, min_val.parent->point, map);
start = min_val.parent;
}
min_val = GetMinPredecessor(start, map);
}
UpdateVertex(start);
Changes changes = map.UpdateMap(const_map, start->point, radius);
if (!changes.cleared.empty() && !changes.occupied.empty()) {
Km += heuristic(last->point, start->point, opt.metrictype);
last = start;
}
for (auto dam : changes.occupied) {
if (NODES.count(vertex(dam, map.height))) {
Node* d = &(NODES.find(vertex(dam, map.height))->second);
OPEN.remove_if(d);
for (auto neighbor: GetSurroundings(d, map)) {
//std::cout << "n: " << neighbor->point << std::endl;
if (neighbor->point != map.goal && (neighbor->parent->point == dam || CutOrSqueeze(neighbor, d))) {
Node min_val = GetMinPredecessor(neighbor, map);
if (!min_val.parent) {
OPEN.remove_if(neighbor);
if(neighbor->point == start->point) {
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
}
} else {
neighbor->rhs = min_val.rhs;
neighbor->parent = min_val.parent;
// std::cout << "changed: "
// << neighbor->point << ' ' << neighbor->parent->point << std::endl;
UpdateVertex(neighbor);
}
}
}
}
}
for (auto cleared : changes.cleared) {
Node new_node(cleared);
new_node.rhs = std::numeric_limits<double>::infinity();
new_node.g = std::numeric_limits<double>::infinity();
NODES[vertex(cleared, map.height)] = new_node;
Node * cl = &(NODES.find(vertex(cleared, map.height))->second);
Node min_val = GetMinPredecessor(cl, map);
if (min_val.parent) {
cl->rhs = min_val.rhs;
cl->parent = min_val.parent;
cl->g = min_val.parent->g + GetCost(cl->point, min_val.parent->point, map);
UpdateVertex(cl);
} else {
break;
}
for (auto neighbor : GetSuccessors(cl, map)) {
if (neighbor->rhs > cl->g + GetCost(neighbor->point, cl->point, map)) {
neighbor->parent = cl;
neighbor->rhs = cl->g + GetCost(neighbor->point, cl->point, map);
UpdateVertex(neighbor);
}
if (neighbor->point.x == cl->point.x || neighbor->point.y == cl->point.y) {
Node min_val = GetMinPredecessor(neighbor, map);
if (!min_val.parent) {
OPEN.remove_if(neighbor);
if(neighbor->point == start->point) {
current_result.pathfound = false;
current_result.pathlength = 0;
return current_result;
}
} else {
neighbor->rhs = min_val.rhs;
neighbor->parent = min_val.parent;
//std::cout << "changed: "
// << neighbor->point << ' ' << neighbor->parent->point << std::endl;
UpdateVertex(neighbor);
}
}
}
}
if(ComputeShortestPath(map)){
//std::cout << "ALL OK\n";
continue;
} else {
std::cout << "NOT OK" << std::endl;
if (OPEN.top_key() == Key(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())) {
current_result.pathfound = false;
current_result.pathlength = 0;
break;
}
}
}
end = std::chrono::system_clock::now();
current_result.time = static_cast<double>(std::chrono::duration_cast<std::chrono::nanoseconds>(end - tstart).count()) / 1000000000;
//map.PrintPath(path);
current_result.lppath = &path;
if (current_result.pathfound) {
makeSecondaryPath();
current_result.hppath = &hpath;
}
//for (auto elem: path) std::cout << elem->point << " ";
//std::cout << std::endl;
return current_result;
}