aStarGraph.h

Go to the documentation of this file.

/*******************************************************************************
** Copyright (c) 2012 MAK Technologies, Inc.
** All rights reserved.
*******************************************************************************/


#pragma once

//vrf includes
#include <features/featuresDefines.h>
#include <features/featureSetUtils.h>
#include <features/pathFeature.h>
#include <features/featuresDebugger.h>
#include <features/featureSetWrappers.h>

#include <vrfutil/asyncJob.h>
#include <vrfutil/kinematicTools.h>
#include <vrfutil/railSegment.h>
#include <vrfutil/terrainInterfaceConfig.h>

//third party includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/function.hpp>

#include <tbb/queuing_mutex.h>
#include <tbb/queuing_rw_mutex.h>
#include <atomic>
#include <tbb/mutex.h>

//std lib includes
#include <list>
#include <map>

namespace MAKVRinTerra
{
   class DtAStarGraphDebugger;

   //Properties of each vertex
   struct DT_DLL_features VertexInfo
   {
      explicit VertexInfo();
      explicit VertexInfo(const DtPoint& point);

      DtPoint location;

      //DtFeatureGeometry::Point geometry() const;
   };

   //Properties on each edge
   struct DT_DLL_features EdgeInfo
   {
      explicit EdgeInfo(const DtPathFeature& p, const DtFeatureGeometry::Path&);
      explicit EdgeInfo(const DtPathFeature::CPtr& p, const DtFeatureGeometry::Path&);

      typedef std::vector<DtPathFeature::CPtr> FeatureVector;

      DtFeatureGeometry::Path edgeGeometry() const;
      double length() const;
      double width() const;
      bool isOneway() const;
      bool isDeleted() const;
      FeatureVector features() const;
      DtPathFeature::CPtr firstValidFeature() const;
      void addFeature(const DtPathFeature::CPtr&);
      size_t hash() const;

   private:
      friend class DtAStarGraph;
      struct Data;

      std::shared_ptr<Data> myData;
   };

   

   //DtAStarGraph processes path features to make a graph for path planning.
   class DT_DLL_features DtAStarGraph : public DtPathFeatureSet::Source
   {
      class BoostGraphVisitor;
      friend class BoostGraphVisitor;

      using DtPathFeatureSet::Source::addFeature;
      using DtPathFeatureSet::Source::addFeatureRef;
      using DtPathFeatureSet::Source::addFeaturePtr;

   public:
      typedef std::shared_ptr<DtAStarGraph> Ptr;

      Ptr shared_from_this();

      typedef boost::adjacency_list<   //graph is adjacency list (vs adjacency matrix)
         boost::setS,                  //per-vertex edge-list is a set 
         boost::vecS,                  //graph's vertex-list is a vector
         boost::directedS,             //graph is directed
         VertexInfo,                   //VertexInfo struct is property on edges
         EdgeInfo,                     //EdgeInfo struct is property on edges
         boost::no_property,           //no property on graph
         boost::vecS>                  //graph's edge-list is a vector
            DtBoostGraph;

      typedef boost::graph_traits<DtBoostGraph> GraphTraits;

      typedef DtBoostGraph::vertex_descriptor vertex_descriptor;

      typedef DtBoostGraph::edge_descriptor   edge_descriptor;

      typedef GraphTraits::vertex_iterator    vertex_iterator;
      typedef GraphTraits::edge_iterator      edge_iterator;

      typedef std::list<vertex_descriptor> VertexList;

      //Weight is a property on edges.
      typedef boost::property<boost::edge_weight_t, double> Weight;

      struct DT_DLL_features PathVertex
      {
         typedef std::list<PathVertex> List;

         static double Distance(const List&);

         explicit PathVertex(
            const DtPoint& l,
            vertex_descriptor v = GraphTraits::null_vertex(),
            const DtPathFeature::CPtr& f = DtPathFeature::CPtr());

         DtPoint localLocation;
         vertex_descriptor vertex;
         DtPathFeature::CPtr nextEdgeFeature;
      };

      struct DT_DLL_features MultiPathVertex
      {
         typedef std::list<MultiPathVertex> List;

         explicit MultiPathVertex(
            const DtPoint& l,
            vertex_descriptor v = GraphTraits::null_vertex(),
            const DtPathFeature::CPtr& f = DtPathFeature::CPtr());

         explicit MultiPathVertex(const PathVertex&);

         PathVertex::List getSinglePath() const;
         void addPath(PathVertex::List::const_iterator, PathVertex::List::const_iterator);
         void addPathToFront(const PathVertex::List&);
         void addPathToEnd(PathVertex::List::const_iterator, PathVertex::List::const_iterator);
         void addPathToEnd(const MultiPathVertex&);

         PathVertex pathVertex;
         List nextVertices;
      };

      class DT_DLL_features Path
      {
      public:
         typedef std::vector<Path> Sequence;

         explicit Path(const std::string& = std::string());

         PathVertex::List getSinglePath() const;

         void addPath(const PathVertex::List& ptList);

         void addPath(const MultiPathVertex::List& listOfPointPaths);

         void addPath(const Path& pathCollection);
         void addPathToFront(const PathVertex::List&);
         void addPathToEnd(const PathVertex::List&);

         bool empty() const { return myPathVertices.empty(); }
         size_t numPaths() const { return myPathVertices.size(); }

         std::string description() const { return myDescription; }

         const MultiPathVertex::List& pathVertices() const { return myPathVertices; }
         MultiPathVertex::List& pathVertices() { return myPathVertices; }

      private:
         MultiPathVertex::List myPathVertices;
         std::string myDescription;
      };

      template <typename T>
      class DynamicPropertyMap;

      typedef DynamicPropertyMap<unsigned> VertexBoolMap;

      typedef std::pair<DtQuery, double>             DtWeightFactor;
      typedef std::list<DtWeightFactor>              DtWeightFactorsList;

      typedef boost::function<double (edge_descriptor)> EdgeWeightAdjustor;

      typedef boost::function<bool ()> CancelCheck;

      static DtRailSegment* vertex2RailSegment(const PathVertex& v,
         const PathVertex& next);

      static DtRailSegmentList* DtPath2RailSegmentList(const Path& path);

      static Ptr Make(
         std::unique_ptr<DtFeatureSet> input,
         DtProj::CPtr localProj,
         const DtTerrainInterfaceConfig& config,
         std::string label = std::string());

      virtual ~DtAStarGraph() override;

      virtual void shutdown();
      virtual bool isShutdown() const;

      virtual bool cancel() const;

      Path makePath(
         const DtPoint& startLocation,
         const DtPoint& endLocation, 
         double maxSearchRadius,
         double startProximityThreshold,
         double stopProximityThreshold,
         const DtWeightFactorsList& weightFactors,
         const CancelCheck& cancelCheck,
         const double offRoadPathWeight = 10.0);

      struct Results;

      struct SearchParameters
      {
         DtFeatureGeometry::Point startPoint;
         DtFeatureGeometry::Point endPoint;
         boost::optional<DtFeatureGeometry::Area> searchArea;
         double startProximityThreshold;
         double stopProximityThreshold;
         double minOffFeatureDistanceToDestination;
         DtWeightFactorsList weightFactors;
         double offRoadPathWeight;
         CancelCheck cancelCheck;
         double directPathCost;

         SearchParameters(
            const DtFeatureGeometry::Point& start,
            const DtFeatureGeometry::Point& end);

         void setMaxSearchRadius(double radius);
         void setSearchRadius(double maxRadius, double minRadius);

         bool cancel() const;
      };

      Path makePath(
         SearchParameters params,
         Results* results = 0,
         unsigned recursionLevel = 0);

      bool onPathFeature(const DtPoint& location, double offset = 0.5);

      std::unique_ptr<DtPathFeatureSet> paths() const;

      const DtFeatureSet& inputFeatureSet() const;

      DtProj::CPtr localProj() const;

      DtClosestPathFeatureFinder closestFeatureOnNetwork(
         const DtPoint& startingLocation, const double& searchRadius, bool* dataAvailable = NULL);

      virtual std::string label() const override;

      virtual DtFeaturesDebugger::Record& addToDebugger(DtFeaturesDebugger&);

      void addFeatureToGraph(
         const DtFeature & r,
         bool joinToIntersectingEdges,
         const boost::optional<DtFeatureGeometry>& searchArea = boost::none);

      void addFeatureToGraph(
         const DtFeature&,
         const boost::optional<DtFeatureGeometry>& searchArea = boost::none);

      void addGeometryToGraph(DtFeatureGeometry g);

      bool loadTile(const DtPoint& localLocation, double timeout = 0);
      bool loadTile(const DtFeatureGeometry& geom, double timeout = 0);
      bool loadTileAndBlock(
         const DtPoint& localLocation, const CancelCheck& = CancelCheck());
      bool loadTileAndBlock(
         const DtFeatureGeometry& geom, const CancelCheck& = CancelCheck());

   protected:

      void maybeAddEdgeToGraph(const DtPoint& vA, const vertex_descriptor vAIndex,
         const DtPoint& vB, const vertex_descriptor vBIndex,
         std::shared_ptr<DtPathFeature> path,
         unsigned& numAddedEdges, unsigned& numDuplicates);

      bool maybeAddVertexToGraph(const DtPoint& vLocation, const VertexBoolMap& existingVertices,
         const double tolerance,
         vertex_descriptor& vIndex);

   protected:
      DtAStarGraph(
         std::unique_ptr<DtFeatureSet> input,
         DtProj::CPtr localProj,
         const DtTerrainInterfaceConfig& config,
         std::string label);

      const std::unique_ptr<DtFeatureSet> myInputFeatureSet;


      DtProj::CPtr myLocalProj;

      typedef tbb::queuing_rw_mutex GraphMutex;

      mutable GraphMutex myGraphMutex;

      DtBoostGraph myGraph;

      struct GridTile : public std::enable_shared_from_this<GridTile>
      {
         typedef std::shared_ptr<GridTile> TilePtr;
         typedef std::vector<TilePtr> TilePtrVector;

         struct EdgeRecord
         {
            struct Hash 
            {
               size_t operator()(const EdgeRecord& edge) const;
            };

            bool operator==(const EdgeRecord& other) const;

            edge_descriptor myEdgeDescriptor;
            EdgeInfo myEdgeInfo;

            explicit EdgeRecord(edge_descriptor, const EdgeInfo&);
         };

         typedef std::vector<EdgeRecord> EdgeSet;
         //typedef boost::unordered_set<EdgeRecord, EdgeRecord::Hash> EdgeSet;

         const unsigned myLevel;
         const unsigned myMaxLevel;

         const DtProj::Bounds myBounds;
         const DtFeatureGeometry::Area myBoundsGeometry;

         explicit GridTile(const GridTile& parent, unsigned x, unsigned y);

         explicit GridTile(DtAStarGraph&, const DtProj::Bounds&, unsigned maxLevel);

         ~GridTile();

         bool loadAllLeafSubTiles(const DtFeatureGeometry&, double timeout);

         void findIntersectingSubTiles(const DtFeatureGeometry&, TilePtrVector& tileList);

         void getTiles(TilePtrVector&);

         void addEdge(edge_descriptor, const EdgeInfo&);

         void getEdges(const DtFeatureGeometry&, EdgeSet& allIntersectingEdges);

         void getEdgesWithWidth(const DtFeatureGeometry&, EdgeSet& allIntersectingEdges);

      private:
         void loadAndGet();

         bool loaded(double timeout) const;

         DtAStarGraph& myAStarGraph;

         typedef tbb::mutex LoadingMutex;

         mutable LoadingMutex myLoadingMutex;
         enum State { None, Loading, Done, Error };
         std::atomic<State> myState;

         typedef tbb::queuing_rw_mutex Mutex;

         mutable Mutex myMutex;
         DtFeatureSetWrapper<DtFeatureSet, std::shared_ptr> myFeatureSet;

         DtFeatureSet::DetachCallback myDetacherCallbackFn;
         std::shared_ptr<GridTile> mySubtiles[2][2];

         mutable Mutex myEdgesMutex;
         EdgeSet myEdgeSet;
      }; 

      friend struct GridTile;

      GridTile::TilePtr myLoadedTiles;

      DtTerrainInterfaceConfig myTerrainInterfaceConfig;

      std::atomic<bool> myShutdownFlag;

      std::string myLabel;

      int myDebuggingGraphicIndex;

      CancelCheck myCancelCheck;
   }; 


   template <typename T>
   class DtAStarGraph::DynamicPropertyMap
   {
      typedef std::vector<T> Sequence;

      static T CreateDefault(T def, size_t) { return def; }

   public:
      typedef size_t                         key_type;
      typedef typename Sequence::value_type  value_type;
      typedef typename Sequence::reference   reference;
      typedef boost::lvalue_property_map_tag category;

      typedef boost::function<T (size_t)> DefaultFactory;

      explicit DynamicPropertyMap(const DefaultFactory& def, size_t size = 0)
         : myDefaultFactory(def)
         , myValues(new Sequence(size))
      {
         for (size_t i=0; i != size; ++i)
            (*myValues)[i] = myDefaultFactory(i);
      }

      explicit DynamicPropertyMap(T def, size_t size = 0)
         : myDefaultFactory(boost::bind(&CreateDefault, def, boost::placeholders::_1))
         , myValues(new Sequence(size, def))
      {
      }

      T& operator[](size_t index)
      {
         while (index >= size())
            myValues->push_back(myDefaultFactory(index));

         return (*myValues)[index];
      }

      T& operator[](size_t index) const
      {
         while (index >= size())
            myValues->push_back(myDefaultFactory(index));

         return (*myValues)[index];
      }

      size_t size() const
      {
         return myValues->size();
      }

      void clear()
      {
         myValues->clear();
      }

   private:
      DefaultFactory myDefaultFactory;
      std::shared_ptr<Sequence> myValues;
   };

   template <typename T>
   T get(const DtAStarGraph::DynamicPropertyMap<T>& map, size_t index)
   {
      return map[index];
   }


   template <typename T>
   void put(DtAStarGraph::DynamicPropertyMap<T>& map, size_t index, const T& value)
   {
      map[index] = value;
   }


   class DT_DLL_features DtMakePathResult : public DtAsyncJobResult
   {
   public:
      MAKVRinTerra::DtAStarGraph::Path path;
   };

   class DT_DLL_features DtMakePathJob : public DtAsyncManagedJob
   {
   public:
      DtMakePathJob(const DtAStarGraph::Ptr& pathFinder, const DtPoint& localStart, const DtPoint& localDestination,
         double maxSearchRadius, double startProximityThreshold, double stopProximityThreshold,
         const DtAStarGraph::DtWeightFactorsList& factors, const double offRoadPathWeight = 10.0);

      virtual void onExecute() override;
      virtual void onException() override;

   public:
      DtAStarGraph::Ptr myPathFinder;
      DtPoint myLocalStart;
      DtPoint myLocalDestination;
      double myMaxSearchRadius;
      double myStartProximityThreshold;
      double myStopProximityThreshold;
      DtAStarGraph::DtWeightFactorsList myWeightFactors;
      double myOffRoadPathWeight;
   };
}