NavGraph.hpp

Go to the documentation of this file.

#ifndef LGL_NAVIGATION_GRAPH_HPP
#define LGL_NAVIGATION_GRAPH_HPP

#include <vector>
#include <math.h>

#include <lgl/GeoData.hpp>
#include <lgl/Decomp.hpp>
#include <lgl/Quadtree.hpp>
#include <lgl/PositionManager.hpp>
#include <lgl/CostManager.hpp>
#include <lgl/GraphPath.hpp>
#include <lgl/Graphs.hpp>
#include <lgl/NavGEdge.hpp>
#include <lgl/NavGNode.hpp>
#include <lgl/NavAStar.hpp>

#include <boost/noncopyable.hpp>

using namespace boost;

#ifndef PRUNE_NAVGRAPH

#define PRUNING_ENABLED true
#define PRUNING_DISABLED false

#define DEFAULT_PRUNING_THRESHOLD 0.5

#endif

namespace jafar { // namespace jafar
  namespace lgl { // namespace lgl

    typedef std::pair<NavLocation, Attributes> NavVertex;

    typedef GraphPath<NavVertex> NavPath;

  class NavGraph : public boost::noncopyable {

    typedef double COST_TYPE;

  public:

    enum verbose_tag { 
      VERBOSE_SILENT=0,
      VERBOSE_ERRORS,
      VERBOSE_WARNINGS,
      VERBOSE_INFO,
      VERBOSE_DEBUG
    };

    verbose_tag verbose;

    typedef NavGraphT<NavGEdge,NavGNode,COST_TYPE> NavGraphContainer;

    typedef NavAStar<NavGraphContainer::GraphContainer,NavGraphContainer::Vertex,COST_TYPE> AStarPlanner;

    typedef std::pair<NavGraphContainer::Vertex, double> frontierPoint;

    NavGraph();

    NavGraph(GeoData* _geodata,
      Quadtree<double,GeoData>* _quadtree = NULL,
      bool       _doNavGraphPruning = PRUNING_DISABLED,
      double        _pruningThreshold = DEFAULT_PRUNING_THRESHOLD,
      PositionManager*    _posManager = NULL,
      CostManager*       _costManager = NULL,
      //void (*_displayHook)(const NavGraphContainer::Vertex&, int)   = NULL),
      boost::function<void(const NavGNode& , int )> displayHook   = NULL
      );

    ~NavGraph();

    NavGraphContainer* getGraph();

    Quadtree<double,GeoData>* getQuadtree();

    bool rebuildGraph();

    bool findPath(const NavLocation& locstart, const NavLocation& locend, NavPath& navpath);

    bool fastFindPath(const NavLocation& locstart, const NavLocation& locend, NavPath& navpath);

    bool findPath(const NavLocation& locstart, const NavLocation& locend, NavPath& navpath, double& pCost);

    bool fastFindPath(const NavLocation& locstart, const NavLocation& locend, NavPath& navpath, double& pCost);

    bool findPathOnUtility(const NavLocation& start, const NavLocation& end, NavPath& navpath, double authDist = 0.0);

    double navCostAtNode(const NavLocation& _loc);

    double navCostAtNode(const NavGraphContainer::Vertex& vn);

    double getPathCost();

    double getCoopPathCost();

    int getNumberOfVertices();

    int getNumberOfEdges();

    void getNavGNodeFromNavLocation(NavGNode& nnode, const NavLocation& v);

    void getOffClusterFromVertex(const NavGraphContainer::Vertex& A, DecompCluster** dcluster, int& i, int& j);

    NavLocation getNavLocFromVertex(const NavGraphContainer::Vertex& A);

    bool areClose( const NavGraphContainer::Vertex& a, const NavGraphContainer::Vertex& b);

    double euclideanDistance( const NavGraphContainer::Vertex& a, const NavGraphContainer::Vertex& b);

    bool saveAnyField(const std::string& outfile, attributeKey _at);

    bool resetGoalReachingPotential();

    bool updateNavigationUtility();

    double* getPotential();

    void calculateNavigationUtility(bool bsetter);

    void calculateGoalReachingPotential(bool bsetter);

    void setGeodataPotential(bool bsetter);

    void setGeodataUtility(bool bsetter);

    void calculateTerrainFactor();

    void addNode(const NavLocation& atThisLoc,  NavGraphContainer::Vertex& vloc);

    bool isOnFrontier(NavGraphContainer::Vertex& vloc);

    bool extendFrontiers(NavGraphContainer::Vertex& vfrontier, double vcost);

    void setNavGraphPruningOption(bool _doNavGraphPruning) ;

    bool getNavGraphPruningOption();

    void setPathPruningOption(bool _doPathPruning) ;
  
    bool getPathPruningOption();

    void setPruningThreshold(double _pruningThreshold) ;

    double getPruningThreshold();

    PositionManager* getPositionManager();

    std::list<frontierPoint> getFrontiers();

    double getNSP(const NavGraphContainer::Vertex& v);

    bool isCalculateGRP();

    bool isSetGeodataPotential();

  protected:

    // the raster data representing the terrain
    GeoData* geodata;

    Quadtree<double,GeoData>* quadtree;

    bool doNavGraphPruning ;

    double pruningThreshold ;

    bool doPathPruning;

    PositionManager* posManager;

    CostManager* costManager;

    NavGraphContainer* graph;

    AStarPlanner* planner;

    boost::function<void(const NavGNode& , int )> displayHook;

    void setDisplayHookFunction(boost::function<void(const NavGNode&, int)>_displayHook );

  private:

    bool m_quadtree_madeinhere;

    bool m_costmanager_madeinhere;
    bool m_posmanager_madeinhere;

    double m_closeDistance;

    bool buildGraph();

    bool set_geodata_gru;

    bool set_geodata_grp;

    bool calculate_navutil;

    bool calculate_grp;

    double terrainFactor[((T_OBSTACLE-T_FREE)+1)];

    double pathCost;

    double coopPathCost;

    std::list<frontierPoint> frontiers;


  };

}
}

#endif