planners/PL_GraphBase.h

Go to the documentation of this file.

//=========================================================================
//
//  File:  PL_GraphBase.h
//
//  Created 07-Feb-01 by Shane Schneider
//
//              Planner Base for planners requiring a graph data structure
//              Contains the actual graph structure as well as several draw and
//              computation functions.

#ifndef PL_GraphBase_h
#define PL_GraphBase_h 1

#pragma warning( disable : 4250 )

// Graph Include Files
#include <LEDA/graph.h>
#include <LEDA/node_list.h>
#include "kinematics/Configuration.h"


// PL_Boolean_Output
#include "Planners\PL_Boolean_Output.h"
#include "Planners\PL_OpenGL.h"

//----------- Forward Declarations -----------------
enum SuccessResultType;

//## Class: PL_GraphBase%3921BC600296
//## Category: Planners%36FB140B003C
//## Persistence: Transient
//## Cardinality/Multiplicity: n

class PL_GraphBase : 
        virtual public PL_Boolean_Output,  //## Inherits: <unnamed>%3921BC73013F
        virtual public PL_OpenGL
{

  public:
          // Default Constructor
          PL_GraphBase();

    //## Destructor (generated)
      virtual ~PL_GraphBase();

      virtual bool DrawExplicit () const;

          virtual bool Load (const char *filename);

      virtual bool Plan ();

          bool Save (const char *filename) const;

          virtual void SetStartConfig( const Configuration& configuration );
          virtual void SetGoalConfig( const Configuration& config ); 

          virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);

    // Additional Public Declarations

          // Graph Stats Functions
          //    Returns the number of nodes and edges in the current graph.
          inline int NodeCount() const { return G.number_of_nodes(); };
          inline int EdgeCount() const { return G.number_of_edges(); };

          virtual void ClearGraph();
          void ClearGraphPath();

          // NormDistWeight
          //    Normalizes the default distance weight used in calculating the distance between two configurations.
          void NormDistWeight();
          virtual void SetDistWeight( const VectorN& weights );
          VectorN GetDistWeight() const { return distWeight; };

          void CopySettings( PlannerBase* original );
          
  protected:
        // Additional Implementation Declarations

          // Distance Functions...
          //    Computes the squared Euclidean distance between two nodes (or configurations) and returns it.
          //    Used for determining the neighbour nodes and which node is closest.
          //    Functions either return a weighted squared sum or absoulte joint differences.
          virtual double Distance ( const node& a, const node& b ) const;
          virtual double Distance ( const Configuration& a, const Configuration& b ) const;
          virtual double Distance ( const node& a, const node& b, const VectorN& weight ) const;
          virtual double Distance ( const Configuration& a, const Configuration& b, const VectorN& weight ) const;
          virtual VectorN DiffV ( const node& a, const node& b ) const;
          virtual VectorN DiffV ( const Configuration& a, const Configuration& b ) const;

          // Length
          //    Returns the weighted length (cost) recorded for specified edge.
          double Length ( const edge& e1 ) const;

          // Measure
          //    Calculates the weighted length (cost) recorded for the specified edge by applying
          //    the appropriate Distance function on the end points (nodes) of the edge.
          double Measure ( const edge& e1 ) const;
          double Measure ( const edge& e1, const VectorN& weight ) const;


          // GetCspaceRange
          //    Computes the length of the diagonal of the Cspace using the given distance weights.  
          //    Used as an indication of the range of distances that can acheived within the Cspace.
          double GetCspaceRange( const VectorN& weight ) const;
          double GetCspaceRange() const ; 
          

          // FindConfig 
          //    Searches ALL the nodes in the graph and returns the node containning the given configuration.
          //    Returns nil if no node that matches the configuration is found
          node FindConfig( const Configuration& c1 ) const;
          
          // FindEdge
          //    Searches all edges associated with one of the nodes and returns the first edge found that 
          //    connects the two specified nodes.  Returns nil if no edge connecting the two nodes exists.
          edge FindEdge( const node& n1, const node& n2 ) const;

          // Create the Random Configurations of the C-space.
          virtual Configuration GenerateRandomConfig ();
          virtual Configuration GenerateRandomConfig ( const Configuration& seed, const double& std_dev );
          virtual Configuration GenerateRandomConfig ( const Configuration& centre );

          // IsInterfering
          //    Calls the associated collision detector to determine if node(s)/configuration(s)/edge
          //    are interfering.  Returns TRUE if the associated paremeters are interfering.
          virtual bool IsInterfering ( const edge& e1 );
          virtual bool IsInterfering ( const node& n1 );
          virtual bool IsInterfering ( const Configuration& c1 );
          virtual bool IsInterfering ( const node& n1, const node& n2 );
          virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );

          // Drawing Functions
          //    Various Functions used by the Drawing Routines for creating the OpenGL C-space display.
          //    GetCoords:  Returns the given parameters with appropriate coordinates based on configuration
          //                        stored in the given node.
          void GetCoords( const node n1, double& Xcor, double& Ycor, double& Zcor ) const;
          void GetCoords( const Configuration& c1, double& Xcor, double& Ycor, double& Zcor ) const;
          //    DrawSpecific:  Virtual function to be implemented in child planners that allow the planner
          //                               to draw additional planner specific OpenGL information.
          virtual bool DrawSpecific() const;

          // PrioritizeEdge
          //    Rearranges the order of the of edges assigned to the source node so the edge connecting
          //    the source and target nodes is made the first adj_edge.  This allows easy trace back along first
          //    adj_edges from the goal to the start node.
          bool PrioritizeEdge(node prev, node curr);  // Makes first adj_edge to point from curr node to the prev node.
          bool PrioritizeEdge(node source, edge e1);  // Makes first adj_edge point from source node to G.target(e1).

          // GetMidPoint
          //    Finds the Midpoint between two configurations
          Configuration GetMidPoint( const Configuration& a, const Configuration& b ) const;

          // TranslatePath
          //    Converts the path stored in graphPath and converts it to PA_Path for server to animate.
          virtual SuccessResultType TranslatePath();

          // SaveContents
          //    Saves the graph and other relevent PL_GraphPath parameters to the given ostream pointer
          virtual bool SaveContents( std::ofstream& outfile ) const;

          // LoadContents
          //    Load the graph and other relevent PL_GraphPath parameters to the given ostream pointer
          virtual bool LoadContents( std::ifstream& infile );

          // TranslateNodeID
          //    Searches the entire graph structure, returning the node (ptr) associated with the given index.
          node TranslateNodeID( const int& nodeID ) const;


    // Additional Protected Declarations

          // Actual Graph Structure to be used
          //    Nodes are of type Configuration and record different configurations at that point.
          //    Edges are of type double and record the squared euclidean distance between joinning nodes (configurations).
          GRAPH<Configuration,double> G;

          // Path Planning Nodes
          //     Keeps track which nodes in the graph contain the start and goal configurations
          node startNode;
          node goalNode;

          // Interm Path
          node_list graphPath;  // records current path through the nodes.

         
          // Vector containning the distance Weights
          VectorN distWeight;


          // ------- File Parameters -------------
          char fileext[10];                                             // File extension the graph planner  ( set in respective constructors )
          char fileheader[20];                                  // File Header for the Graph Planner  ( set in respective constructors )
      static const int NIL_ID;                          // Integer indicating an empty or nil pointer is currently saved.

  private:
    // Additional Private Declarations

  private: //## implementation
    // Additional Implementation Declarations

};


// Class PL_GraphBase 

enum SuccessResultType
{
        TIMER_EXPIRED = -1,
        FAIL = 0,
        PASS = 1
};


#endif

---