solid_collider.h

#pragma once
 
 
#include <MT_Quaternion.h>
#include <vector>
#include <map>
#include <utility>
#include <thread>
#include <memory>
#include <mutex>
 
#include "base/collider.h"
#include "log.h"
 
namespace bmpf {
    class SolidCollider : public Collider {
    public:
 
        SolidCollider() = default;
 
        virtual ~SolidCollider();
 
        void init(std::vector<std::vector<std::string>> groupedModelPaths, bool subCollider) override;
 
        void paint(std::vector<Eigen::Matrix4d> matrices, bool onlyRobot) override;
 
        bool isCollided(std::vector<Eigen::Matrix4d> matrices) override;
 
        std::vector<float> getPoints(std::vector<Eigen::Matrix4d> matrices) override;
 
        std::vector<double> getBoxCoords(unsigned long robotNum, std::vector<Eigen::Matrix4d> matrices) override;
 
        bool isCollided(std::vector<Eigen::Matrix4d> matrices, std::vector<int> robotIndexes) override;
 
        std::vector<double> getBoxPoints(unsigned long robotNum, std::vector<Eigen::Matrix4d> matrices) override;
 
 
    private:
 
 
        static std::vector<std::vector<std::string>> _getSubGroupedModelPaths(
                const std::vector<std::vector<std::string>> &groupedModelPaths, const std::vector<int> &indexes
        );
 
        void _populateColliderMap(const std::vector<std::vector<std::string>> &groupedModelPaths) {
            _populateColliderMap(groupedModelPaths, {}, 0, 0);
        }
 
        void _populateColliderMap(
                const std::vector<std::vector<std::string>> &groupedModelPaths,
                std::vector<int> indexes, int pos, int val
        );
 
 
        void _makeFree() { _setTransformMutex.unlock(); };
 
        void _setTransformMatrices(unsigned long robotNum, std::vector<Eigen::Matrix4d> matrices);
 
        void _setTransformMatrices(std::vector<Eigen::Matrix4d> matrices);
 
        static double *_eigenToDouble(Eigen::Matrix4d m);
 
        bool _isCollided();
 
        bool _isSingleObject{};
 
        std::vector<std::pair<long, long>> _objectIndexRanges;
 
        DT_SceneHandle _scene{};
        std::vector<std::shared_ptr<Solid3Object>> _links;
        std::vector<std::vector<std::shared_ptr<Solid3Object>>> _groupedLinks;
        std::mutex _setTransformMutex;
        std::map<std::vector<int>, std::shared_ptr<SolidCollider>> _collidersMap;
 
    };
}