pathFinder.h

Go to the documentation of this file.

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


#pragma once

#include <features/featuresDefines.h>
#include <features/pathFeature.h>

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

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/function.hpp>
#include <tbb/spin_rw_mutex.h>

#include <list>

namespace MAKVRinTerra
{
   //Properties of each vertex
   struct DT_DLL_features VertexInfo
   {
      VertexInfo()
         : location(0.0, 0.0, 0.0)
      {
      }
      VertexInfo(DtPoint point)
         : location(point)
      {
      }
      DtPoint location;
   };

   //Properties on each edge
   struct DT_DLL_features EdgeInfo
   {
      EdgeInfo()
         : feature()
         , distance(1.0)
      {
      }
      EdgeInfo(const DtPathFeature& p, const double& d)
         : feature(p.getPathPtr())
         , distance(d)
      {
      }
      EdgeInfo(DtPathFeature::CPtr p, const double& d)
         : feature(p)
         , distance(d)
      {
      }
      DtPathFeature::CPtr feature;
      double distance;
   };

   //Key for grid cells, for tracking which tiles of features are
   //already loaded.
   struct DT_DLL_features DtGridKey
   {
      DtGridKey(const int& xIndex, const int& yIndex)
         : x(xIndex), y(yIndex) { }

      bool operator<(const DtGridKey& rhs) const
      {
         if (x < rhs.x)
            return true;
         else if (x == rhs.x && y < rhs.y)
            return true;
         return false;
      }

      int x;
      int y;
   };

   DT_DLL_features std::ostream& operator<<(std::ostream& s, const DtGridKey& key);

   //DtPathFinder processes path features to make a graph for path planning.
   class DT_DLL_features DtPathFinder
   {
   public:

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

      typedef boost::adjacency_list<   //graph is adjacency list (vs adjacency matrix)
         boost::vecS,                  //per-vertex edge-list is a vector 
         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 DtBoostGraph::vertex_descriptor vertex_descriptor;
      typedef DtBoostGraph::edge_descriptor edge_descriptor;
      typedef std::list<boost::graph_traits<DtBoostGraph>::vertex_descriptor> VertexList;

      struct PathVertex
      {
         PathVertex() : localLocation(0.0, 0.0, 0.0), nextEdgeFeature() { }
         PathVertex(const DtPoint& l, DtPathFeature::CPtr& f)
            : localLocation(l), nextEdgeFeature(f) { }

         DtPoint localLocation;
         DtPathFeature::CPtr nextEdgeFeature;
      };
      typedef std::list<PathVertex> Path;

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

      //Used for finding points already in the graph
      struct PointLessThan
      {
         explicit PointLessThan() : myTolerance(1.0) { }
         explicit PointLessThan(const double& tolerance)
            : myTolerance(tolerance) { }

         bool operator()(const DtPoint& lhs, const DtPoint& rhs) const
         {
            if (DtALMOST(lhs.x(), rhs.x(), myTolerance))
            {
               if (DtALMOST(lhs.y(), rhs.y(), myTolerance))
               {
                  if (DtALMOST(lhs.z(), rhs.z(), myTolerance) || lhs.z() > rhs.z())
                  {
                     return false;
                  }
                  return true;
               }
               return lhs.y() < rhs.y();
            }
            return lhs.x() < rhs.x();
         }
         
         double myTolerance;
      };

      typedef std::map<DtPoint, vertex_descriptor, PointLessThan> DtLocationVertexMap;

      //Represents the results of the astar visitor's search.
      struct AstarSearchResults
      {
         AstarSearchResults()
            : myBestEdgeA(boost::graph_traits<DtPathFinder::DtBoostGraph>::null_vertex())
            , myBestEdgeB(boost::graph_traits<DtPathFinder::DtBoostGraph>::null_vertex())
            , myBestPointOnNetwork()
            , myBestEdgeDistance(std::numeric_limits<double>::infinity())
            , myEndFeature()
            , myGoalFound(false)
         {

         }

         vertex_descriptor myBestEdgeA;
         vertex_descriptor myBestEdgeB;
         DtPoint myBestPointOnNetwork;
         double myBestEdgeDistance;
         DtPathFeature::CPtr myEndFeature;
         bool myGoalFound;
      };

      enum ReturnCode
      {
         PathFinderFoundGoal,
         PathFinderPartialPath,
         PathFinderNoPathFound,
         PathFinderGraphTooFarAway,
         PathFinderStartProximityFailed,
         PathFinderEmptyGraph,
         PathFinderInvalidVertices,
         PathFinderError
      };

      explicit DtPathFinder(std::auto_ptr<DtPathFeatureSet> s, DtProj::CPtr localProj,
         boost::shared_ptr<DtTerrainInterfaceConfig> config);

      void addFeatureToGraph(const DtPathFeature & r);

      DtPoint locationForVertex(const vertex_descriptor& vertex);
      vertex_descriptor vertexForLocation(const DtPoint& localLocation);

      Path makePath(const DtPoint& startLocation, const DtPoint& endLocation, 
         const double& maxSearchRadius, const double& startProximityThreshold, 
         DtWeightFactorsListPtr weightFactors, boost::function<bool ()> cancelCheck,
         ReturnCode& result);

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

      static DtRailSegmentList* DtPath2RailSegmentList(const Path& path);

      DtGridKey MakeGridKey(const DtPoint& localLocation);
      DtFeatureGeometry::Area MakeGridCell(const DtGridKey& key);

      bool tileIsLoaded(const DtGridKey& key);
      void loadTile(const DtGridKey& key, tbb::spin_rw_mutex::scoped_lock&, 
         boost::function<bool ()> cancelCheck);

      DtBoostGraph* graph();

      std::auto_ptr<DtPathFeatureSet> paths() const
      {
         return std::auto_ptr<DtPathFeatureSet>(myInputPathFeatureSet->clone());
      }

      DtProj::CPtr localProj() const
      {
         return myLocalProj;
      }

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

      int numFeatures() const;

   private:

      Path makePath(const vertex_descriptor& source, const vertex_descriptor& destination,
         double maxSearchRadius, DtWeightFactorsListPtr weightFactors, 
         tbb::spin_rw_mutex::scoped_lock& lock, boost::function<bool ()> cancelCheck, 
         AstarSearchResults& astarResults, ReturnCode& pathFindResult);

      DtPoint closestPointOnNetwork(const DtPoint& startingLocation,
         const double& searchRadius, vertex_descriptor& edgeA, vertex_descriptor& edgeB,
         DtPathFeature::CPtr& edgeFeature);

      bool determineStartAndEndVertices(const DtPoint& startLocation, const DtPoint& endLocation, 
         vertex_descriptor& startEdgeVertexToUse, vertex_descriptor& startEdgeOtherVertex,
         vertex_descriptor& endEdgeVertexToUse, vertex_descriptor& endEdgeOtherVertex,          
         DtPoint& startEdgeClosestPoint, DtPoint& endEdgeClosestPoint,
         DtPathFeature::CPtr& startFeature, DtPathFeature::CPtr& endFeature);

      std::auto_ptr<DtPathFeatureSet> myInputPathFeatureSet;

      std::auto_ptr<DtPathFeatureSet::Storage> myLoadedPathFeatureSet;

      DtProj::CPtr myLocalProj;

      DtProj::CPtr myGridProj;

      DtProj::CPtr myVertexProj;

      DtBoostGraph myGraph;

      std::set<DtGridKey> myLoadedFeatures;

      boost::shared_ptr<DtTerrainInterfaceConfig> myTerrainInterfaceConfig;

      DtLocationVertexMap myLocationToVertexMap;

      mutable tbb::spin_rw_mutex myGraphMutex;
   };

   class DT_DLL_features DtMakePathResult : public DtAsyncJobResult
   {
   public:
      DtPathFinder::ReturnCode returnCode;
      MAKVRinTerra::DtPathFinder::Path path;
   };

   class DT_DLL_features DtMakePathJob : public DtAsyncJob
   {
   public:
      DtMakePathJob(DtPathFinder& pathFinder,
         const DtPoint& localStart, const DtPoint& localDestination,
         const double& maxSearchRadius, const double& startProximityThreshold,
         DtPathFinder::DtWeightFactorsListPtr factors)
         : DtAsyncJob()
         , myPathFinder(pathFinder)
         , myLocalStart(localStart)
         , myLocalDestination(localDestination)
         , myMaxSearchRadius(maxSearchRadius)
         , myStartProximityThreshold(startProximityThreshold)
         , myWeightFactors(factors)
         , myPathFindReturnCode(DtPathFinder::PathFinderError)
      {
      };

      task* onExecute() 
      {
         DtJobCancelStruct cancelCheck(myId, myJobServer);
         if (cancelCheck())
         {
            DtVerbose << "Job canceled before path plan." << std::endl;
            throw job_canceled();
         }

         {
            DtAsyncJobMap::accessor a;
            myJobServer->accessor(a, myId);
            DtAsyncJobMapEntry* entry = a->second;
            if (entry->status() == queued)
            {
               entry->setStatus(processing);
            }
            else
            {
               DtWarn << "Job " << myId << " in invalid state " << entry->status() << std::endl;
               entry->setStatus(canceled);
               throw DtException("Invalid state.");
            }
         }

         DtPathFinder::Path path = myPathFinder.makePath(
            myLocalStart, myLocalDestination, myMaxSearchRadius, 
            myStartProximityThreshold, myWeightFactors, boost::ref(cancelCheck),
            myPathFindReturnCode);

         std::auto_ptr<DtMakePathResult> result(new DtMakePathResult());
         result->returnCode = myPathFindReturnCode;
         result->path = path;

         {
            DtAsyncJobMap::accessor a;
            myJobServer->accessor(a, myId);
            DtAsyncJobMapEntry* entry = a->second;
            entry->setResult(std::auto_ptr<DtAsyncJobResult>(result));
            entry->setStatus(complete);
         }
         return 0;   
      }

      task* onException()
      {
         DtAsyncJobMap::accessor a;
         myJobServer->accessor(a, myId);
         DtAsyncJobMapEntry* entry = a->second;
         std::auto_ptr<DtMakePathResult> result(new DtMakePathResult());
         result->returnCode = myPathFindReturnCode;
         entry->setResult(std::auto_ptr<DtAsyncJobResult>(result));
         entry->setStatus(complete);
         return 0;
      }

   public:
      DtPathFinder& myPathFinder;
      DtPoint myLocalStart;
      DtPoint myLocalDestination;
      double myMaxSearchRadius;
      double myStartProximityThreshold;
      DtPathFinder::DtWeightFactorsListPtr myWeightFactors;
      DtPathFinder::ReturnCode myPathFindReturnCode;
   };
}