grid_path_finder.h

#pragma once
 
#include <memory>
#include <unordered_map>
#include <set>
#include <utility>
#include <unordered_set>
#include <scene.h>
#include "path_finder.h"
#include "log.h"
#include "state.h"
 
#include <iostream>
 
namespace bmpf {
    class GridPathFinder : public PathFinder {
    public:
 
        static const int ERROR_CAN_NOT_FIND_FREE_START_POINT = 2;
        static const int ERROR_CAN_NOT_FIND_FREE_END_POINT = 3;
 
        GridPathFinder(
                const std::shared_ptr<bmpf::Scene> &scene, bool showTrace, int gridSize, int threadCnt = 1
        );
 
        std::vector<double> coordsToState(std::vector<int> &coords) const;
 
        std::vector<double> coordsToState(std::vector<int> coords, unsigned long robotNum);
 
        bool checkCoords(std::vector<int> coords);
 
        std::vector<std::vector<double>>
        findGridPath(std::vector<int> &startCoords, std::vector<int> &endCoords, int &errorCode);
 
        bool checkCoords(const std::vector<int> &coords, unsigned int robotNum);
 
        virtual std::vector<int> stateToCoords(std::vector<double> state);
 
        void prepare(const std::vector<double> &startState, const std::vector<double> &endState) override;
 
        virtual void prepare(std::vector<int> &startCoords, std::vector<int> &endCoords);
 
        std::vector<std::vector<double>> checkTask(
                const std::vector<double> &startState, const std::vector<double> &endState,
                double opacity
        );
 
 
    protected:
 
        virtual std::vector<int> _findFreePoint(
                std::vector<int> coords, unsigned long pos, unsigned long maxPos,
                const std::vector<double> &state
        );
 
        std::vector<double> _gridSteps;
        std::vector<std::vector<double>> _groupedGridSteps;
        int _gridSize;
        std::vector<int> _startCoords;
        std::vector<int> _endCoords;
        std::vector<double> _startStateFromCoords;
        std::vector<double> _endStateFromCoords;
        double _maxDist;
        bool _coordsUsed = false;
        std::vector<std::vector<int>> _buildedGridPath;
 
    public:
 
        const std::vector<std::vector<int>> &getBuildedGridPath() const { return _buildedGridPath; }
 
        const std::vector<double> &getStartStateFromCoords() const { return _startStateFromCoords; }
 
        const std::vector<double> &getEndStateFromCoords() const { return _endStateFromCoords; }
 
        const std::vector<double> &getGridSteps() const { return _gridSteps; }
 
        const double &getMaxDist() const { return _maxDist; }
 
        const std::vector<std::vector<double>> &getGroupedGridSteps() const { return _groupedGridSteps; }
    };
 
 
}