Пример #1
0
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;
}
Пример #2
0
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;
}