fm2star.hpp

#ifndef FM2STAR_HPP_
#define FM2STAR_HPP_

#include <iostream>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <array>
#include <limits>

#include <fast_methods/ndgridmap/fmcell.h>
#include <fast_methods/fm/fmm.hpp>
#include <fast_methods/fm2/fm2.hpp>
#include <fast_methods/gradientdescent/gradientdescent.hpp>

// template < class grid_t, class solver_t = FMM<grid_t> > class FM2Star : public FM2<grid_t> {

template < class grid_t, class heap_t = FMDaryHeap<FMCell> > class FM2Star : public FM2<grid_t, heap_t> {

    typedef std::vector< std::array<double, grid_t::getNDims()> > path_t;
    
    typedef FM2<grid_t, heap_t > FM2Base;

    public:
        FM2Star
        (HeurStrategy heurStrategy = TIME, double maxDistance = -1) : FM2Base("FM2*", maxDistance), heurStrategy_(heurStrategy) { }

        FM2Star
        (const char * name, HeurStrategy heurStrategy = TIME, double maxDistance = -1) : FM2Base(name, maxDistance), heurStrategy_(heurStrategy) { }

        virtual void setInitialAndGoalPoints
        (const std::vector<unsigned int> & init_points, unsigned int goal_idx) {
            FM2Base::setInitialAndGoalPoints(init_points, goal_idx);
            solver_->precomputeDistances();
        }

        virtual void setup
        () {
            FM2Base::setup();
            if(int(goal_idx_) == -1)
            {
                console::error("A goal point has to be set for FM2-based solvers.");
                exit(1);
            }
        }
        
        virtual void computeInternal
        () {
            if (!setup_)
                 setup();

            computeVelocitiesMap();

            // Reset time counter so that time_ returns the time of the second wave.
            start_ = std::chrono::steady_clock::now();

            // According to the theoretical basis the wave is expanded from the goal point to the initial point.
            std::vector <unsigned int> wave_init;
            wave_init.push_back(goal_idx_);
            unsigned int wave_goal = init_points_[0];

            solver_->setInitialAndGoalPoints(wave_init, wave_goal);
            solver_->setHeuristics(heurStrategy_);
            solver_->compute();
            // Restore the actual grid status.
            grid_->setClean(false);
        }

    protected:
        using FM2Base::grid_;
        using FM2Base::init_points_;
        using FM2Base::goal_idx_;
        using FM2Base::solver_;
        using FM2Base::setup;
        using FM2Base::setup_;
        using FM2Base::start_;
        using FM2Base::computeVelocitiesMap;
        using FM2Base::maxDistance_;

        HeurStrategy heurStrategy_;
};

#endif /* FM2STAR_HPP_*/