void emMinesFileModel::OpenField(int x, int y, int z, bool saveFile) { int x1,y1,z1,x2,y2,z2,f; f=GetField(x,y,z); if ((f&2)!=0) return; f|=2; f&=~4; SetField(x,y,z,f); if (GetSurroundings(x,y,z)==0) { z1=z-1; if (z1<0) z1=0; z2=z+2; if (z2>SizeZ) z2=SizeZ; y1=y-1; if (y1<0) y1=0; y2=y+2; if (y2>SizeY) y2=SizeY; x1=x-1; if (x1<0) x1=0; x2=x+2; if (x2>SizeX) x2=SizeX; for (z=z1; z<z2; z++) { for (y=y1; y<y2; y++) { for (x=x1; x<x2; x++) { OpenField(x,y,z,false); } } } } if (saveFile) Save(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; }