MedialAxisUtilities.h

Go to the documentation of this file.

#ifndef PMPL_MEDIAL_AXIS_UTILITY_H_
#define PMPL_MEDIAL_AXIS_UTILITY_H_
 
#include "Geometry/Bodies/Body.h"
#include "Geometry/Bodies/MultiBody.h"
//#include "MPLibrary/LocalPlanners/StraightLine.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/CDInfo.h"
#include "Utilities/MetricUtils.h"
#include "Utilities/MPUtils.h"
#include "MPLibrary/MPTools/ClearanceUtilities.h"
 
class MedialAxisUtility : public ClearanceUtility {
 
  public:
 
 
    
 
 
    MedialAxisUtility(
        string _vcLabel = "", string _dmLabel = "",
        bool _exactClearance = false, bool _exactPenetration = false,
        size_t _clearanceRays = 10, size_t _penetrationRays = 10,
        bool _useBBX = true, bool _positionalDofsOnly = true, bool _debug = false,
        double _epsilon = 0.1, size_t _historyLength = 5);
 
    MedialAxisUtility(XMLNode& _node);
 
    virtual ~MedialAxisUtility() = default;
 
 
    virtual void Print(ostream& _os) const override;
 
    void SetDebug(const bool _debug) { this->m_debug = _debug; }
 
 
    double GetEpsilon() const {return m_epsilon;}
 
 
    bool PushToMedialAxis(Cfg& _cfg, const Boundary* const _b);
 
    bool PushFromInsideObstacle(Cfg& _cfg, const Boundary* const _b);
 
 
    bool PushCfgToMedialAxisMidpointRule(Cfg& _cfg, const Boundary* const _b);
 
  private:
 
 
    // m_epsilon is used only externally right now (not used in pushing to MA)
    double m_epsilon{.1};
    size_t m_historyLength{5};
 
 
    bool FuzzyVectorEquality(mathtool::Vector3d _v1, mathtool::Vector3d _v2,
        const double _tolerance = 10.*std::numeric_limits<double>::epsilon());
 
    bool WitnessObstacleEquality(Cfg _cfg, CDInfo _firstWitnessInfo, CDInfo _secondWitnessInfo);
};
 
#endif