pathfinding.cpp

    struct advanced_weighted_cell {
        iVec2D cell;
        int cost;
        int desire;
        int bends = -1;
        temppodvector<char> current_path;

        int path_value() const {
            return cost*100+bends;
        }

        std::weak_ordering operator <=>(const advanced_weighted_cell& rhs) const {
            if(desire != rhs.desire) return desire <=> rhs.desire;
            if(cost != rhs.cost) return cost <=> rhs.cost;
            if(bends != rhs.bends) return bends <=> rhs.bends;
            return std::weak_ordering::equivalent;
        }
    };

    podvector<iVec2D> TacticsGrid::advanced_pathfind(Character* source, iVec2D begin, iVec2D end, int max_range, bool sunken_only){
        max_range *= pathfind_cost_resolution;

        podvector<iVec2D> path;
        dynamic_grid<int> pathvalues(width, depth);
        for(auto& t:pathvalues) t = INT_MAX;

        if(!Debug.CheckAssert(pathvalues.in_bounds(begin)) || !Debug.CheckAssert(pathvalues.in_bounds(end))) {
            return path;
        }

        auto costs = pathfind_costs(source, sunken_only);

        std::priority_queue<advanced_weighted_cell, std::vector<advanced_weighted_cell>, std::greater<advanced_weighted_cell>> queue;

        queue.push({begin, 0, 0, 0});

        int total_checked = 0;

        while(!queue.empty()){
            auto tile = queue.top();
            queue.pop();

            if(tile.path_value() <= pathvalues[tile.cell]){
                pathvalues[tile.cell] = tile.path_value();

                if(tile.cell == end) {
                    iVec2D current = begin;
                    for(auto i : tile.current_path){
                        current += all_orientations[i];
                        path.push_back(current);
                    }

                    //Debug.DisplayVariable(total_checked);

                    return path;
                    break;
                }

                for(int i = 0; i<4; i++){
                    auto &v = all_orientations[i];
                    iVec2D t2 = v+tile.cell;

                    if(pathvalues.in_bounds(t2)){
                        advanced_weighted_cell next = {t2, 
                                                       tile.cost+costs[t2].enter_cost + costs[tile.cell].exit_cost,
                                                       tile.desire+(t2==end?0:costs[t2].desire_cost),// + iso_dist(t2-end), //heuristic is more efficient, but results in paths with non-optimal numbers of bends
                                                       tile.bends};

                        if(tile.current_path.size() > 0 && i != tile.current_path.back()) next.bends++;

                        if(next.cost <= max_range && next.path_value() <= pathvalues[t2]){
                            next.current_path.resize(tile.current_path.size()+1); //minimize reallocations
                            if(tile.current_path.size() > 0) {
                                std::memcpy(next.current_path.data(), tile.current_path.data(), tile.current_path.size()*sizeof(char));
                            }
                            next.current_path.back() = i;

                            queue.push(next);
                            total_checked++;
                        }
                    }
                }
            }
        }

        return path;
    }