PropertyMap.h

Go to the documentation of this file.

#ifndef PROPERTY_MAP_H_
#define PROPERTY_MAP_H_
 
#include <queue>
#include <unordered_map>
 
#include "ConfigurationSpace/Cfg.h"
#include "Geometry/Boundaries/Boundary.h"
#include "Geometry/Bodies/MultiBody.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/CDInfo.h"
#include "MPLibrary/MPBaseObject.h"
#include "WorkspaceSkeleton.h"
 
#include <containers/sequential/graph/directed_preds_graph.h>
 
#include "Vector.h"
 
using namespace mathtool;
using namespace std;
 
 
struct edgeHash {
 
  size_t operator()(const WorkspaceSkeleton::ED&  _ed) const {
    return hash<typename WorkspaceSkeleton::ED::edge_id_type>()(_ed.id());
  }
};
 
 
template<typename EdgeProperty, typename VertexProperty=void>
class PropertyMap {
 
  public:
 
 
    typedef WorkspaceSkeleton                    GraphType;
    typedef WorkspaceSkeleton::VD                VD;
    typedef WorkspaceSkeleton::ED                ED;
    typedef typename GraphType::vertex_iterator        vertex_iterator;
    typedef typename GraphType::adj_edge_iterator      adj_edge_iterator;
 
    typedef function<bool(EdgeProperty&)>     EdgeFilterFunction;
    typedef function<bool(VertexProperty&)>   VertexFilterFunction;
 
    typedef unordered_map<ED, EdgeProperty, edgeHash> EdgeMapType;
    typedef unordered_map<VD, VertexProperty> VertexMapType;
 
 
    PropertyMap() = default;
    PropertyMap(WorkspaceSkeleton* _ws) : m_skeleton(_ws)  {}
 
 
    WorkspaceSkeleton* GetEdgeFilteredSkeleton(EdgeFilterFunction&& _f);
 
    WorkspaceSkeleton* GetVertexFilteredSkeleton(VertexFilterFunction&& _f);
 
    void SetSkeleton(WorkspaceSkeleton* _ws) { m_skeleton = _ws;}
    VertexMapType& GetVertexMap() { return m_vertexMap; }
    EdgeMapType& GetEdgeMap() { return m_edgeMap; }
    void SetVertexMap(VertexMapType& _vMap)  { m_vertexMap = _vMap; }
    void SetEdgeMap(EdgeMapType& _eMap)  { m_edgeMap = _eMap; }
 
 
    void SetVertexProperty(const VD& _v, const VertexProperty& _vp);
    void SetEdgeProperty(const ED& _e, const EdgeProperty& _ep);
    VertexProperty& GetVertexProperty(const VD& _v);
    EdgeProperty& GetEdgeProperty(const ED& _e);
 
 
 
    void Write(const std::string& _file);
 
 
    void Read(const std::string& _file);
 
 
 
  private:
 
    WorkspaceSkeleton* m_skeleton{nullptr};  
    EdgeMapType m_edgeMap;  
    VertexMapType m_vertexMap;  
};
 
PropertyMap<vector<double>,double>*
ClearanceAnnotatedSkeleton(MPBaseObject* _mp, WorkspaceSkeleton* _ws,
       bool _boundary = true);
 
// void
// ClearanceFilteredSkeleton(double _t, MPBaseObject* _mp,
//                           WorkspaceSkeleton* _ws, bool _boundary = true);
 
/*------------------------------------------------------------------------*/
 
 
template<typename EdgeProperty, typename VertexProperty>
EdgeProperty&
PropertyMap<EdgeProperty, VertexProperty>::
GetEdgeProperty(const ED& _e) {
  return m_edgeMap.at(_e);
}
 
template<typename EdgeProperty, typename VertexProperty>
void
PropertyMap<EdgeProperty, VertexProperty>::
SetEdgeProperty(const ED& _e, const EdgeProperty& _ep) {
  m_edgeMap.emplace(_e, _ep);
}
 
template<typename EdgeProperty, typename VertexProperty>
VertexProperty&
PropertyMap<EdgeProperty, VertexProperty>::
GetVertexProperty(const VD& _v) {
  return m_vertexMap.at(_v);
}
 
template<typename EdgeProperty, typename VertexProperty>
void
PropertyMap<EdgeProperty, VertexProperty>::
SetVertexProperty(const VD& _v, const VertexProperty& _vp) {
  m_vertexMap.emplace(_v, _vp);
}
 
template<typename EdgeProperty, typename VertexProperty>
WorkspaceSkeleton*
PropertyMap<EdgeProperty, VertexProperty>::
GetVertexFilteredSkeleton(VertexFilterFunction&& _f)  {
 
  for(auto mit = m_vertexMap.begin();
    mit != m_vertexMap.end(); ++mit) {
    if(_f(mit->second)) {
      m_skeleton->DeleteVertex(mit->first);
    }
  }
 
  return m_skeleton;
}
 
template<typename EdgeProperty, typename VertexProperty>
WorkspaceSkeleton*
PropertyMap<EdgeProperty, VertexProperty>::
GetEdgeFilteredSkeleton(EdgeFilterFunction&& _f)  {
 
  for(auto mit = m_edgeMap.begin();
    mit != m_edgeMap.end(); ++mit)  {
    if(_f(mit->second))  {
      m_skeleton->delete_edge(mit->first);
    }
  }
 
  return m_skeleton;
}
 
/*------------------------------------- I/O helpers ---------------------------------*/
 
template<typename EdgeProperty, typename VertexProperty>
void
PropertyMap<EdgeProperty, VertexProperty>::
Write(const std::string& _file) {
  std::ofstream ofs(_file);
 
  ofs << m_vertexMap.size() <<" "<< m_edgeMap.size() << std::endl;
  for(auto vit : m_vertexMap){
    auto spk = this->GetVertexProperty(vit.first);
    ofs << vit.first <<" "<< spk.size() <<" ";
    for(auto s : spk)
      ofs << s <<" ";
    ofs << std::endl;
  }
 
  for(auto eit : m_edgeMap) {
   auto spks = this->GetEdgeProperty(eit.first);
 
    ofs << eit.first.id() << " ";
    ofs << eit.first.source() << " " << eit.first.target() << " " << spks.size() << std::endl;
 
    for(auto spk : spks) {
      ofs << spk.size() << " ";
      for(auto s : spk) {
        ofs << s << " ";
      }
      ofs << std::endl;
    }
  }
 
  ofs.close();
}
 
template<typename EdgeProperty, typename VertexProperty>
void
PropertyMap<EdgeProperty, VertexProperty>::
Read(const std::string& _file) {
  typedef WorkspaceSkeleton::VD VD;
  typedef WorkspaceSkeleton::ED ED;
 
  std::fstream ifs(_file);
  size_t nVerts, nEdges;
 
  ifs >> nVerts >> nEdges;
 
  for(size_t vit = 0; vit < nVerts; vit++) {
    size_t propSize;
    VD vid;
    std::vector<Point3d> vertexProperty;
 
    ifs >> vid >> propSize;
    for (size_t propit = 0; propit < propSize; propit++) {
      Point3d prop;
      ifs >> prop;
      vertexProperty.push_back(prop);
    }
    this->SetVertexProperty(vid, vertexProperty);
  }
 
  for(size_t eit = 0; eit < nEdges; eit++) {
 
    size_t proSize, eSource, eTarget, eDesID;
    std::vector<vector<Point3d> > intermediateWitnesses;
 
    ifs >> eDesID;
    ifs >> eSource >> eTarget >> proSize;
    for (size_t pit = 0; pit < proSize; pit++) {
 
      std::vector<Point3d> edgeProperty;
      size_t eSize;
      ifs >> eSize;
      for(size_t it = 0; it < eSize; it++) {
        Point3d prop;
        ifs >> prop;
        edgeProperty.push_back(prop);
      }
 
      intermediateWitnesses.push_back(edgeProperty);
    }
 
    auto ed =  ED(eSource, eTarget, eDesID);
    this->SetEdgeProperty(ed, intermediateWitnesses);
  }
}
 
/*------------------------------------------------------------------------*/
#endif