monotone_trajectory_finder.h

#pragma once
 
#include "base/path_finder.h"
#include "continuous_path_finder.h"
#include "monotone_cubic_interpolation.h"
#include <scene.h>
#include <log.h>
#include "state.h"
 
namespace bmpf {
    class MonotoneTrajectoryFinder {
    public:
        virtual void init(const std::shared_ptr<bmpf::Scene> &scene, double intervalDuration,
                          bool showTrace,
                          unsigned int maxOpenSetSize, int gridSize, unsigned int maxNodeCnt, int checkCnt,
                          int threadCnt = 1);
 
        virtual void prepareTrajectory(
                const std::vector<double> &startState, const std::vector<double> &endState, int &errorCode
        );
 
        virtual ~MonotoneTrajectoryFinder() {
            _interpolators.clear();
        }
 
        std::vector<double> getTrajectoryNode(double tm);
 
        std::vector<double> getTrajectoryPosition(double tm);
 
        std::vector<double> getTrajectorySpeed(double tm);
 
        std::vector<double> getTrajectoryAcceleration(double tm);
 
 
    protected:
 
        void _prepareTrajectory(
                const std::vector<double> &startState, const std::vector<double> &endState,
                std::vector<double> &timeIntervals, int &errorCode
        );
 
        std::vector<std::shared_ptr<MonotoneCubicInterpolation>> _interpolators;
        std::vector<std::vector<double>> _lastPath;
        std::shared_ptr<bmpf::PathFinder> _pf;
        std::vector<double> _timeIntervals;
        double _startIntervalDuration{};
        std::vector<std::vector<double>> _lastTrajectory;
        int _nodeCnt{};
        std::shared_ptr<bmpf::Scene> _scene;
        double _wholeDuration{};
        bool _isReady{};
 
    public:
 
        double getWholeDuration() const { return _wholeDuration; }
 
        const std::vector<std::vector<double>> &getLastPath() const { return _lastPath; }
 
        const std::vector<std::vector<double>> &getLastTrajectory() const { return _lastTrajectory; }
 
        int getNodeCnt() const { return _nodeCnt; }
 
        const std::shared_ptr<bmpf::PathFinder> &getPF() const { return _pf; }
 
        const std::shared_ptr<bmpf::Scene> &getScene() const { return _scene; }
 
        bool isReady() const { return _isReady; }
 
    };
}