Syclop.h

Go to the documentation of this file.

#ifndef PMPL_SYCLOP_H_
#define PMPL_SYCLOP_H_
 
#include "BasicRRTStrategy.h"
#include "MPLibrary/MPTools/TetGenDecomposition.h"
#include "Workspace/WorkspaceDecomposition.h"
#include "Workspace/WorkspaceRegion.h"
 
class Syclop : public BasicRRTStrategy {
 public:
 
  typedef typename MPBaseObject::WeightType WeightType;
  typedef typename MPBaseObject::RoadmapType RoadmapType;
  typedef typename RoadmapType::VID VID;
 
 
  using typename BasicRRTStrategy::VertexSet;
 
 
  Syclop();
 
  Syclop(XMLNode& _node);
 
  virtual ~Syclop() = default;
 
 
 protected:
 
  virtual void Initialize() override;
 
 
  virtual VID FindNearestNeighbor(
      const Cfg& _cfg,
      const VertexSet* const _candidates = nullptr) override;
 
  virtual VID Extend(const VID _nearVID,
                     const Cfg& _qRand,
                     LPOutput& _lp,
                     const bool _requireNew = true) override;
 
  virtual std::pair<VID, bool> AddNode(const Cfg& _newCfg) override;
 
  virtual void AddEdge(const VID _source,
                       const VID _target,
                       const LPOutput& _lpOutput) override;
 
 
  std::vector<VID> DiscreteLead();
 
  void FindAvailableRegions(std::vector<VID> _lead);
 
  const WorkspaceRegion* SelectRegion();
 
 
  size_t Sel(const WorkspaceRegion* const _r1,
             const WorkspaceRegion* const _r2);
 
  size_t Conn(const WorkspaceRegion* const _r1,
              const WorkspaceRegion* const _r2);
 
  double Cost(const WorkspaceRegion* const _r1,
              const WorkspaceRegion* const _r2);
  double Cost(const WorkspacePortal& _p);  
 
 
  void ComputeFreeVolumes();  
 
 
  struct RegionData {
 
    double weight;
 
    double alpha;
 
    double freeVolume;
 
    size_t numTimesSelected{0};
 
    VertexSet vertices;  
 
    std::set<size_t> cells;  
 
    size_t Coverage() const { return cells.size(); }
 
 
    void AddCell(size_t _index) {
      cells.insert(_index);
      UpdateAlpha();
      UpdateWeight();
    }
 
    void UpdateAlpha() {
      alpha = 1. / ((1. + Coverage()) * std::pow(freeVolume, 4.));
    }
 
    void UpdateWeight() {
      weight = std::pow(freeVolume, 4.) /
               ((1. + Coverage()) * (1. + std::pow(numTimesSelected, 2.)));
    }
 
  };
 
  struct RegionPairData {
    void AddCell(size_t _index) { cells.insert(_index); }
 
    size_t Coverage() const { return cells.size(); }
 
 
    size_t numLeadUses{0};  
    size_t numAttempts{0};  
 
   private:
    std::set<size_t> cells;
 
  };
 
  struct WeightFunctor {
 
    typedef WorkspacePortal property_type;
    typedef double value_type;
    typedef Syclop strategy_type;
 
 
    strategy_type* m_syclop;
 
 
    WeightFunctor(strategy_type* _s) : m_syclop(_s) {}
 
 
    value_type get(property_type& _p) { return m_syclop->Cost(_p); }
 
    void put(property_type& _p, value_type& _v) {}
 
    template <typename functor>
    void apply(property_type& _p, functor _f) {
      _f(_p);
    }
 
  };
 
  struct DFSVisitor : public stapl::visitor_base<WorkspaceDecomposition> {
 
    typedef WorkspaceDecomposition graph_type;
    typedef stapl::visitor_return visitor_return;
    typedef std::map<VID, VID> map_type;
 
 
    map_type& m_parentMap;
 
 
    DFSVisitor(map_type& _pm) : m_parentMap(_pm) {}
 
 
    virtual visitor_return tree_edge(
        graph_type::vertex_iterator _vit,
        graph_type::adj_edge_iterator _eit) override {
      m_parentMap[_eit->target()] = _eit->source();
      return visitor_return::CONTINUE;
    }
 
  };
 
 
  std::map<const WorkspaceRegion*, RegionData> m_regionData;
 
  std::map<std::pair<const WorkspaceRegion*, const WorkspaceRegion*>,
           RegionPairData>
      m_regionPairData;
 
  std::string m_tmLabel;  
 
  std::set<const WorkspaceRegion*> m_availableRegions;
 
 
  size_t m_currentLeadUses{0};
  size_t m_maxLeadUses{6};
 
  size_t m_currentRegionUses{0};
  size_t m_maxRegionUses{6};
 
  std::string m_freeVolumeSampler{"UniformRandom"};
  std::string m_freeVolumeVcLabel{"pqp_solid"};
 
  const WorkspaceRegion* m_currentRegion{nullptr};
 
  bool m_improvement{false};  
 
};
 
#endif