double compute_deltabeta() {
size_t i;
size_t k;
double val;
double deltabeta;
bool first = true;
double idcol[objc];
for(i = 0; i < objc; ++i) {
idcol[i] = 0.0;
}
for(i = 0; i < objc; ++i) {
idcol[i] = 1.0;
for(k = 0; k < clustc; ++k) {
val = pow(euclid_dist(membvec[k].mtx, idcol, objc), 2.0);
if(val != 0.0) {
val = (-2.0 * get(&dists, k, i)) / val;
if(first || val > deltabeta) {
deltabeta = val;
first = false;
}
}
}
idcol[i] = 0.0;
}
return deltabeta;
}
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;
}