ufmm.hpp

#ifndef UFMM_HPP_
#define UFMM_HPP_

#include <fast_methods/fm/eikonalsolver.hpp>
#include <fast_methods/datastructures/fmuntidyqueue.hpp>

template <class grid_t, class cell_t = FMCell> class UFMM : public EikonalSolver<grid_t> {

    public:
        UFMM
        (unsigned s = 1000, double inc = 2) : EikonalSolver<grid_t>("UFMM"), heap_s_(s), heap_inc_(inc) {
            narrow_band_ = new FMUntidyQueue<cell_t> (heap_s_, heap_inc_);
        }
        UFMM
        (const char * name, unsigned s = 1000, double inc = 2) : EikonalSolver<grid_t>(name), heap_s_(s), heap_inc_(inc) {
            narrow_band_ = new FMUntidyQueue<cell_t> (heap_s_, heap_inc_);
        }

        virtual ~UFMM() { clear(); }

        virtual void computeInternal
        () {
            if (!setup_)
                setup();

            unsigned int j= 0;
            unsigned int n_neighs = 0;
            bool stopWavePropagation = false;

            // Algorithm initialization
            for (unsigned int &i : init_points_) { // For each initial point
                grid_->getCell(i).setArrivalTime(0);
                narrow_band_->push( &(grid_->getCell(i)) );
            }

            // Main loop.
            unsigned int idxMin = 0;
            while (!stopWavePropagation && !narrow_band_->empty()) {
                idxMin = narrow_band_->topIdx(); // pop() has to be called after pushing in this case (because
                                                 // of the untidy queue implementation.
                grid_->getCell(idxMin).setState(FMState::FROZEN);
                n_neighs = grid_->getNeighbors(idxMin, neighbors_);
                for (unsigned int s = 0; s < n_neighs; ++s) { // For each neighbor.
                    j = neighbors_[s];
                    if ( (grid_->getCell(j).getState() == FMState::FROZEN) || grid_->getCell(j).isOccupied())
                        continue;
                    else {
                        double new_arrival_time = solveEikonal(j);
                        if (grid_->getCell(j).getState() == FMState::NARROW) { // Updating narrow band if necessary.
                            if (utils::isTimeBetterThan(new_arrival_time, grid_->getCell(j).getArrivalTime()) ) {
                                grid_->getCell(j).setArrivalTime(new_arrival_time);
                                narrow_band_->increase( &(grid_->getCell(j)) );
                            }
                        }
                        else {
                            grid_->getCell(j).setState(FMState::NARROW);
                            grid_->getCell(j).setArrivalTime(new_arrival_time);
                            narrow_band_->push( &(grid_->getCell(j)) );
                        } // neighbors open.
                    } // neighbors not frozen.
                } // For each neighbor.
                narrow_band_->pop();
                if (idxMin == goal_idx_)
                    stopWavePropagation = true;
            } // while narrow band is not empty
        }

        virtual void printRunInfo
        () const {
            console::info("Untidy Fast Marching Method");
            std::cout << '\t' << name_ << '\n'
                      << '\t' << "Number of buckets: " << heap_s_ << '\n'
                      << '\t' << "Maximum increment " << heap_inc_ << '\n'
                      << '\t' << "Elapsed time: " << time_ << " ms\n";
        }

        virtual void clear
        () {
            delete narrow_band_;
        }

        virtual void reset
        () {
            EikonalSolver<grid_t>::reset();
            narrow_band_->clear();
        }

    protected:
        using EikonalSolver<grid_t>::grid_;
        using EikonalSolver<grid_t>::solveEikonal;
        using EikonalSolver<grid_t>::init_points_;
        using EikonalSolver<grid_t>::goal_idx_;
        using EikonalSolver<grid_t>::setup;
        using EikonalSolver<grid_t>::setup_;
        using EikonalSolver<grid_t>::neighbors_;
        using EikonalSolver<grid_t>::name_;
        using EikonalSolver<grid_t>::time_;

    private:
        unsigned                heap_s_;

        double                  heap_inc_;

        FMUntidyQueue<cell_t> * narrow_band_;
};

#endif /* UFMM_HPP_*/