lsm.hpp

#ifndef LSM_HPP_
#define LSM_HPP_

#include <fast_methods/fm/fsm.hpp>
#include <fast_methods/ndgridmap/fmcell.h>
#include <fast_methods/utils/utils.h>

template < class grid_t > class LSM : public FSM<grid_t> {

    public:
        LSM(unsigned maxSweeps = std::numeric_limits<unsigned>::max()) : FSM<grid_t>("LSM", maxSweeps) {}

        LSM(const char * name, unsigned maxSweeps = std::numeric_limits<unsigned>::max()) : FSM<grid_t>(name, maxSweeps) {}

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

            // FMState::FROZEN - locked and FMState::OPEN - unlocked.
            // The time this takes is negligible and if done in setup or
            // setEnvironment it can affect other planners run in the same
            // grid.
            for(size_t i = 0; i < grid_->size(); ++i)
                grid_->getCell(i).setState(FMState::FROZEN);

            // Initialization
            for (unsigned int i: init_points_) {
                grid_->getCell(i).setArrivalTime(0);
                unsigned int n_neighs = grid_->getNeighbors(i, neighbors_);
                for (unsigned int j = 0; j < n_neighs; ++j)
                    grid_->getCell(neighbors_[j]).setState(FMState::OPEN);
            }

            // Getting dimsizes and filling the other dimensions.
            keepSweeping_ = true;
            stopPropagation_ = false;

            while (keepSweeping_ && !stopPropagation_ && sweeps_ < maxSweeps_) {
                keepSweeping_ = false;
                setSweep();
                ++sweeps_;
                recursiveIteration(grid_t::getNDims()-1);
            }
        }

        virtual void reset
        () {
            FSM<grid_t>::reset();
            for(size_t i = 0; i < grid_->size(); ++i)
                grid_->getCell(i).setState(FMState::FROZEN);
        }

        virtual void printRunInfo
        () const {
            console::info("Lock Sweeping Method");
            std::cout << '\t' << name_ << '\n'
                      << '\t' << "Maximum sweeps: " << maxSweeps_ << '\n'
                      << '\t' << "Sweeps performed: " << sweeps_ << '\n'
                      << '\t' << "Elapsed time: " << time_ << " ms\n";
        }

    protected:
        virtual void solveForIdx
        (unsigned idx) {
            if (grid_->getCell(idx).getState() == FMState::OPEN) {
                const double prevTime = grid_->getCell(idx).getArrivalTime();
                const double newTime = solveEikonal(idx);

                // Update time if better and unlock neighbors with higher time.
                if(utils::isTimeBetterThan(newTime, prevTime)) {
                    grid_->getCell(idx).setArrivalTime(newTime);
                    keepSweeping_ = true;
                    unsigned int n_neighs = grid_->getNeighbors(idx, neighbors_);
                    for (unsigned int i = 0; i < n_neighs; ++i)
                        if (utils::isTimeBetterThan(newTime, grid_->getCell(neighbors_[i]).getArrivalTime()))
                            grid_->getCell(neighbors_[i]).setState(FMState::OPEN);
                }
                // EXPERIMENTAL - Value not updated, it has converged
                else if(!isnan(newTime) && !isinf(newTime) && (idx == goal_idx_))
                    stopPropagation_ = true;

                grid_->getCell(idx).setState(FMState::FROZEN);
            }
        }

        // Inherited members from FSM.
        using FSM<grid_t>::grid_;
        using FSM<grid_t>::init_points_;
        using FSM<grid_t>::goal_idx_;
        using FSM<grid_t>::setup_;
        using FSM<grid_t>::setup;
        using FSM<grid_t>::name_;
        using FSM<grid_t>::time_;
        using FSM<grid_t>::recursiveIteration;
        using FSM<grid_t>::solveEikonal;
        using FSM<grid_t>::setSweep;
        using FSM<grid_t>::sweeps_;
        using FSM<grid_t>::maxSweeps_;
        using FSM<grid_t>::keepSweeping_;
        using FSM<grid_t>::stopPropagation_;
        using FSM<grid_t>::incs_;
        using FSM<grid_t>::inits_;
        using FSM<grid_t>::ends_;
        using FSM<grid_t>::d_;

        std::array <unsigned int, 2*grid_t::getNDims()> neighbors_;
};

#endif /* LSM_HPP_*/