path_finder.h

#pragma once
 
#include <memory>
#include <utility>
#include <unordered_map>
#include <set>
#include <unordered_set>
#include <scene.h>
#include <json/json.h>
#include <chrono>
#include "base/collider.h"
#include "log.h"
#include "solid_collider.h"
#include "solid_sync_collider.h"
#include "state.h"
 
namespace bmpf {
    class PathFinder {
    public:
 
        static const int NO_ERROR = -1;
        static const int ERROR_CAN_NOT_FIND_PATH = 1;
 
        PathFinder(const std::shared_ptr<bmpf::Scene> &scene, bool showTrace, int threadCnt = 1);
 
        std::vector<std::vector<double>>
        findPath(const std::vector<double> &startState, const std::vector<double> &endState, int &errorCode);
 
        virtual bool findTick(std::vector<double> &state) = 0;
 
        virtual void prepare(const std::vector<double> &startState, const std::vector<double> &endState) = 0;
 
        virtual void buildPath() = 0;
 
        void updateCollider();
 
        virtual void paint(const std::vector<double> &state, bool onlyRobot);
 
        bool divideCheckPathSegment(const std::vector<double> &prevPoint, std::vector<double> nextPoint, int checkCnt);
 
        int divideCheckPath(std::vector<std::vector<double>> path, int checkCnt);
 
        static void infoPath(const std::vector<std::vector<double>> &path);
 
        static Json::Value getJSONPath(std::vector<std::vector<double>> path);
        static std::vector<std::vector<double>> getPathFromJSON(const Json::Value& json);
 
        static void savePathToFile(std::vector<std::vector<double>> path, const std::string &filename);
 
        static std::vector<std::vector<double>> loadPathFromFile(const std::string &filename, std::string &scenePath);
 
        static std::vector<std::vector<std::vector<double>>>
        loadPathsFromFile(const std::string &filename, std::string &scenePath);
 
        static std::vector<std::vector<double>> splitPath(std::vector<std::vector<double>> path, unsigned long partCnt);
 
 
        bool simpleCheckPath(const std::vector<std::vector<double>> &path, double maxDist);
 
        bool checkState(const std::vector<double> &state);
 
        std::vector<double> getRandomState();
 
        std::vector<double> getPathStateFromTM(double tm);
 
        static std::vector<double> getPathStateFromTM(std::vector<std::vector<double>> &path, double tm);
 
        bool checkCollision(const std::vector<double> &state);
 
        void addObjectToScene(std::string path);
 
        void deleteObjectFromScene(long robotNum);
 
        static double calculatePathLength(std::vector<std::vector<double>> path);
 
    protected:
 
        double _pathLength{};
        int _errorCode;
        std::shared_ptr<bmpf::Scene> _scene;
        bool _showTrace;
        std::shared_ptr<bmpf::Collider> _collider;
        bool _ready;
        bool _threadCnt;
        std::vector<double> _endState;
        std::vector<double> _startState;
        std::vector<std::vector<double>> _buildedPath;
        std::chrono::time_point<std::chrono::system_clock> _startTime;
        double _calculationTimeInSeconds;
 
    public:
 
        double getPathLength() const { return _pathLength; }
 
        bool isReady() const { return _ready; }
 
        bool getThreadCnt() const { return _threadCnt; }
 
        void setReady(bool ready) { _ready = ready; }
 
        std::vector<double> &getStartState() { return _startState; }
 
        std::vector<double> &getEndState() { return _endState; }
 
        int getErrorCode() const { return _errorCode; }
 
        const std::vector<std::vector<double>> &getBuildedPath() const { return _buildedPath; }
 
        const std::shared_ptr<bmpf::Scene> &getScene() const { return _scene; }
 
        const std::shared_ptr<bmpf::Collider> &getCollider() const { return _collider; }
 
        double getCalculationTimeInSeconds() const { return _calculationTimeInSeconds; }
    };
 
 
}