fsm.hpp

#ifndef FSM_HPP_
#define FSM_HPP_

#include <algorithm>

#include <fast_methods/fm/eikonalsolver.hpp>
#include <fast_methods/utils/utils.h>


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

    public:
        FSM(unsigned maxSweeps = std::numeric_limits<unsigned>::max()) : EikonalSolver<grid_t>("FSM"),
            sweeps_(0),
            maxSweeps_(maxSweeps) {}

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

        virtual void setEnvironment
        (grid_t * g) {
            EikonalSolver<grid_t>::setEnvironment(g);
            // Filling the size of the dimensions...
            std::array<unsigned, grid_t::getNDims()> dimsize = g->getDimSizes();
            size_t ncells = 1;
            for (size_t i = 0; i < grid_t::getNDims(); ++i) {
                dimsize_[i] = dimsize[i];
                ncells *= dimsize[i];
                d_[i] = ncells;
            }

            Tvalues_.reserve(grid_t::getNDims());
        }

        virtual void setup
        () {
            EikonalSolver<grid_t>::setup();
            initializeSweepArrays();
            if (int(goal_idx_) != -1)
                console::warning("Setting a goal point in FSM (and LSM) is experimental. It may lead to wrong results.");
        }

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

            // Initialization
            for (unsigned int i: init_points_) // For each initial point
                grid_->getCell(i).setArrivalTime(0);

            keepSweeping_ = true;
            stopPropagation_ = false;

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

        virtual void reset
        () {
            EikonalSolver<grid_t>::reset();
            sweeps_ = 0;
            initializeSweepArrays();
        }

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

    protected:
        void recursiveIteration
        (size_t depth, int it = 0) {
            if (depth > 0) {
                for(int i = inits_[depth]; i != ends_[depth]; i += incs_[depth])
                    recursiveIteration(depth-1, it + i*d_[depth-1]);
            }
            else {
                for(int i = inits_[0]; i != ends_[0]; i += incs_[0])
                    if (!grid_->getCell(it+i).isOccupied())
                        solveForIdx(it+i);
            }
        }

        virtual void solveForIdx
        (unsigned idx) {
            const double prevTime = grid_->getCell(idx).getArrivalTime();
            const double newTime = solveEikonal(idx);
            if(utils::isTimeBetterThan(newTime, prevTime)) {
                grid_->getCell(idx).setArrivalTime(newTime);
                keepSweeping_ = true;
            }
            // EXPERIMENTAL - Value not updated, it has converged
            else if(!isnan(newTime) && !isinf(newTime) && (idx == goal_idx_))
                stopPropagation_ = true;
        }

        virtual void setSweep
        () {
            // Inspired in http://stackoverflow.com/a/17758788/2283531
            for (size_t i = 0; i < grid_t::getNDims(); ++i)
            {
                if((incs_[i] += 2) <=1)
                    break;
                else
                    incs_[i] = -1;
            }

            // Setting inits and ends.
            for (size_t i = 0; i < grid_t::getNDims(); ++i)
            {
                if (incs_[i] == 1)
                {
                    inits_[i] = 0;
                    ends_[i] = dimsize_[i];
                }
                else
                {
                    inits_[i] = dimsize_[i]-1;
                    ends_[i] = -1;
                }
            }
        }

        virtual void initializeSweepArrays
        () {
            for (size_t i = 0; i < grid_t::getNDims(); ++i) {
                incs_[i] = 1;
                inits_[i] = 0;
                ends_[i] = 1;
            }
        }

        using EikonalSolver<grid_t>::grid_;
        using EikonalSolver<grid_t>::init_points_;
        using EikonalSolver<grid_t>::goal_idx_;
        using EikonalSolver<grid_t>::setup_;
        using EikonalSolver<grid_t>::name_;
        using EikonalSolver<grid_t>::time_;
        using EikonalSolver<grid_t>::Tvalues_;
        using EikonalSolver<grid_t>::solveEikonal;

        unsigned int sweeps_;

        unsigned maxSweeps_;

        bool keepSweeping_;

        bool stopPropagation_;

        std::array<int, grid_t::getNDims()> incs_;

        std::array<int, grid_t::getNDims()> inits_;

        std::array<int, grid_t::getNDims()> ends_;

        std::array<int, grid_t::getNDims()> dimsize_;

        std::array<int, grid_t::getNDims()> d_;
};

#endif /* FSM_HPP_*/