planners/PL_GraphBase.cpp

Go to the documentation of this file.

//=========================================================================
//
//  File:  PL_GraphBase.cpp
//
//  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.



// PL_GraphBase
#include "synchronization/semaphore.h"
#include "color/colorf.h"
#include "Planners\PL_GraphBase.h"

#include <assert.h>
#include <math.h>
#include <math/math2.h>
#include <iosfwd>
#include <iostream>


// fix gl path...
// #include <gl/glos.h>
#include "opengl/glos.h"
#include <gl/gl.h>

using std::endl;

//--------------------- Constants ---------------
// File Constants
static const char FILEEXT[] = ".gph";
static const char FILEHEADER[] = "GRAPHBASE";
const int  PL_GraphBase::NIL_ID = -1;
// Other constants
const double COMPTOL = 1e-8;

// Class PL_GraphBase 

// Default Constructor
PL_GraphBase::PL_GraphBase()
{
        // Assign the file header and extension for this type
        strcpy( fileext, FILEEXT );
        strcpy( fileheader, FILEHEADER );

        // Clear the Graph
        ClearGraph();

        // Normalize the Default Distance Weight
        NormDistWeight();

        // DEBUGGING:  Ensure this parent constructor is being called
        TRACE("PL_GraphBase Default constructor completed.\n");


}


PL_GraphBase::~PL_GraphBase()
{
        // Default Destructor
}


//=============================================================================
// DrawExplicit
//
// Purpose: Draws the planner data to the planner window when called
//=============================================================================
bool PL_GraphBase::DrawExplicit () const
{
    double Xcor; 
    double Ycor;
    double Zcor;    // temp coordinate variables
    node   n1;      // index for the various nodes during iterations
    edge   e1;      // index for iteration of edges
        
        // Lock Graph for Drawing
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        // Setup GL Environment
        glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        BOOL lightingState = glIsEnabled( GL_LIGHTING );
        glDisable( GL_LIGHTING );

        // Create Shading Effects that will allow edges to have two end colours.
        //  IMPROVE:  Does this change the shading for others.  If so, is there a way
        //                    to check for current shading mode and save it.
        glShadeModel( GL_SMOOTH );

        // The depth function is set to its default of GL_LESS.  This means that any object closer to the viewport
        // will have its colour displayed for that pixel.  This means closer objects will be drawn on top of further
        // objects.  Any objects that are in the same plane, will have the colour of the object drawn first.

        // Draw Child Planner Specific Information
        DrawSpecific();


        // Draw all NODES first, as these are to be seen on top of any edges.
        glPointSize( 5.0f );    // Set size of points associated with the nodes.

        // Draw the Start and Goal nodes as these want to be predominately seen.
        glBegin( GL_POINTS );
                // Draw the Startnode, if valid.
                if ( startNode != nil )
                {
            Color::yellow.GlDraw();
                        GetCoords( startNode, Xcor, Ycor, Zcor );
                        glVertex3d( Xcor, Ycor, Zcor );
                }

                // Draw the Goalnode, if valid.
                if ( goalNode != nil )
                {
            Color::cyan.GlDraw();
                        GetCoords( goalNode, Xcor, Ycor, Zcor );
                        glVertex3d( Xcor, Ycor, Zcor );
                }
        glEnd();

        // Draw all remainning nodes.
        glBegin(GL_POINTS);
                // Search all nodes of the graph and draw them unless they are start and goal.
        Color::red.GlDraw();
                forall_nodes( n1, G )
                {
                        if ( ( n1 != startNode ) && ( n1 != goalNode ) )
                        {
                                GetCoords( n1, Xcor, Ycor, Zcor );      // Get Coordinates of the current node
                                glVertex3d( Xcor, Ycor, Zcor );
                        }
                }
        glEnd();


        // Draw EDGES joining the nodes
        // Draw the tentative path through the nodes, if it exists, by drawing edges connects the nodes of 
        // the path.
        if ( !graphPath.empty() )
        {
                glLineWidth( 3.0f );
                glBegin(GL_LINES);
                        node n1 = graphPath.tail();
        
                        // Search through the nodes of the graphPath, starting with
                        // the goalNode (last node listed in path) and ending on startNode (first node)
                        node firstNode = graphPath.head();
                        while ( n1 != firstNode )
                        {
                                // get the source node coordinates
                                GetCoords( n1, Xcor, Ycor, Zcor );
                Color::orange.GlDraw();
                                glVertex3d( Xcor, Ycor, Zcor ); // add source vertex for edge line

                                // advance to prev node (target vertex of edge)
                                n1 = graphPath.pred( n1 );

                                // get the target node coordinates
                                GetCoords( n1, Xcor, Ycor, Zcor );
                Color::yellow.GlDraw();
                                glVertex3d( Xcor, Ycor, Zcor ); // add target vertex for edge line

                                // for next iteration, want to use the current node and node index as the source for the next node.  
                                // Will automatically fall out when the end of the list is reached and the last node serves as the target 
                                // for the last edge only.
                        } 
                        
                glEnd();
        }


        // Draw all Edges joining the nodes of the graph.
        glLineWidth( 1.0f );
        glBegin(GL_LINES);
                // Draw all connecting edges
                forall_edges(e1,G)
                {
                        GetCoords( G.source(e1), Xcor, Ycor, Zcor );    // get coordinates of source node;
            Color::blue.GlDraw();
                        glVertex3d( Xcor, Ycor, Zcor ); // add source vertex for edge line.

                        GetCoords( G.target(e1), Xcor, Ycor, Zcor );    // get coordinates of target node;
            Color::white.GlDraw();
                        glVertex3d( Xcor, Ycor, Zcor ); // add target vertex for edge line.
                }
        glEnd();

        // Clean up Environment
        if( lightingState != FALSE )
        {
                glEnable( GL_LIGHTING );
        }

        // Unlock Semaphore
        semaphore.Unlock();

        return true;
}


//=============================================================================
// Load
//
// Purpose: Loads partailly complete planner data from the disk
//=============================================================================
bool PL_GraphBase::Load (const char *filename)
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        // Create file name with the extension.
        char tempstring[ 256 ] = "" ;
        strcpy( tempstring, filename ) ;
        strcat( tempstring, fileext ) ; 

        // Open the file
        std::ifstream infile( tempstring, std::ios::in );
        if( infile.good() == false )
        {
                semaphore.Unlock();
                MessageBox( NULL, "Could not open filename specified", "ERROR", MB_OK );
                return false;
        }

        // Check the Header
        infile >> tempstring;
        if ( strcmp( tempstring, fileheader ) != 0 )
        {
                infile.close();
                semaphore.Unlock();
                MessageBox( NULL, "Incompatible Base Graph File Loaded", "ERROR", MB_OK );
                return false;
        }

        // Clear the old graph and parameters
        this->ClearGraph();

        // Load the contents of the file
        bool success = this->LoadContents( infile );
        if ( !success )
        {
                infile.close();
                semaphore.Unlock();
                MessageBox( NULL, "Could not load graph or parameters from file", "ERROR", MB_OK );
                return false;
        }

        // Close the file
        infile.close();

        semaphore.Unlock();
        return true;
}

//=============================================================================
// LoadContents
//
// Purpose: Loads the graph and other related base planner parameters to the the
//                  given ifstream.
//      
//                      Returns success of load.
//=============================================================================
bool PL_GraphBase::LoadContents( std::ifstream& infile )
{
        // Load the Graph
        int result = G.read( infile );
        if ( result != 0 )
        {
                MessageBox( NULL, "Graph Load Error", "ERROR", MB_OK );
                return false;
        }

        // Load the Start and Goal Nodes
        int nodeID;
    Configuration newStartConfig;
        infile >> nodeID >> newStartConfig;
    SetStartConfig( newStartConfig );
        // Since graphs are optimized when loaded, the nodeID may not be valid,
        // use the FindConfig to find the correct node associated with the start and goal configs in the graph.
//      startNode = TranslateNodeID( nodeID );
        startNode = FindConfig( GetStartConfig() );     
        

    Configuration newGoalConfig;
        infile >> nodeID >> newGoalConfig;
    SetGoalConfig( newGoalConfig );
//      goalNode = TranslateNodeID( nodeID );
        goalNode = FindConfig( GetGoalConfig() );

        //Load the DistVector
        infile >> distWeight;

        return true;
        
}


//=============================================================================
// Does Planning
//
// Purpose: performs the planning task
//=============================================================================
bool PL_GraphBase::Plan ()
{
        assert( false );                //this is an abstract base class, and should never be called
        return false;
}

//=============================================================================
// Save
//
// Purpose: Saves partial plan data to disk
//=============================================================================
bool PL_GraphBase::Save (const char *filename) const
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        // Create file name with the extension.
        char filenameWithExtension[ 256 ] = "" ;
        strcpy( filenameWithExtension, filename ) ;
        strcat( filenameWithExtension, fileext ) ;      

        // Open the file
        std::ofstream outfile( filenameWithExtension, std::ios::out );
        if( outfile.good() == false )
        {
                semaphore.Unlock();
                MessageBox( NULL, "Could not open filename specified", "ERROR", MB_OK );
                return false;
        }

        // Add the Header Information
        outfile << fileheader << endl;


        // Save the contents of the planner
        bool success = this->SaveContents( outfile );
        ASSERT ( success );

        // Close the file
        outfile.close();

        semaphore.Unlock();
        return true;
}

//=============================================================================
// SaveContents
//
// Purpose: Saves the graph and other related base planner parameters to the the
//                  given ofstream.
//      
//                      Returns success of save.
//=============================================================================
bool PL_GraphBase::SaveContents ( std::ofstream& outfile ) const
{
        // Save the Graph
        G.write( outfile );

        int nodeID;

        // Save Start and Goal Nodes
        if ( startNode == nil )
        {
                nodeID = NIL_ID;
        }
        else
        {
                nodeID = startNode->id();
        }
        outfile << nodeID << " " << GetStartConfig() << endl;
        if ( goalNode == nil )
        {
                nodeID = NIL_ID;
        }
        else
        {
                nodeID = goalNode->id();
        }
        outfile << nodeID << " " << GetGoalConfig() << endl;

        // Save the DistWeights
        outfile << distWeight << endl;

        return true;
}



//=============================================================================
// Clearing Functions
//
// Purpose: Clears the graph and start and goal nodes.
//                  
//=============================================================================
void PL_GraphBase::ClearGraph()
{
        // Clear the Graph
        if ( !G.empty() )
        {
                G.clear();
        }

        // Reset the start and goal nodes
        startNode = nil;
        goalNode = nil;

        // Clear any path
        ClearGraphPath();

}

void PL_GraphBase::ClearGraphPath()
{
        // Clear the path of nodes
        if ( !graphPath.empty() )
        {
                graphPath.clear();
        }
}




//=============================================================================
// Distance Functions
//
// Purpose: Returns a weighted squared Euclidean distance between two possible nodes (configurations).
//                  Used in selecting closest neighbours and path lengths
//=============================================================================
double PL_GraphBase::Distance ( const node& a, const node& b ) const
// default weighted squared Euclidean Distance
{
        return this->Distance( G.inf(a), G.inf(b) );
}


double PL_GraphBase::Distance ( const node& a, const node& b, const VectorN& weight ) const
// Weighted Distance
{
        return this->Distance( G.inf(a), G.inf(b), weight );
}


double PL_GraphBase::Distance ( const Configuration& a, const Configuration& b ) const
// default weighted squared Euclidean Distance
{
        return this->Distance( a, b, distWeight );
}


double PL_GraphBase::Distance ( const Configuration& a, const Configuration& b, const VectorN& weight ) const
// Weighted Distance
{
        // Get the differences between each of the joints
        VectorN jointDiff;
        jointDiff = DiffV( a, b );

        // Verify weight vector is at least as long as jointDiff.
        ASSERT( weight.Length() >= jointDiff.Length() ); 

        // Square and sum each of the joint differences
        double distance = 0;
        for( int i = 0; i < jointDiff.Length(); i++ )
        {
                distance += Sqr( weight[i]*jointDiff[i] );
        }

        // Return the squared euclidean distance
        return distance;
}


VectorN PL_GraphBase::DiffV ( const node& a, const node& b ) const
// Joint Differences
{
        return this->DiffV( G.inf(a), G.inf(b) );
}


VectorN PL_GraphBase::DiffV ( const Configuration& a, const Configuration& b ) const
// Absolute Joint Differences
{

        return collisionDetector->JointDisplacement( a, b );

}


//=============================================================================
// Measure Functions
//
// Purpose: Returns the weighted length (cost) of the given edge.
//                  Length returns the stored value for the edge; Measure calculates a new
//                      length by using the distance function with the end nodes of the edge.
//=============================================================================
double PL_GraphBase::Length( const edge& e1 ) const
// Return stored length (cost) for edge
{
        return G.inf(e1);
}

double PL_GraphBase::Measure( const edge& e1, const VectorN& weight ) const
// Calculate the weighted length (cost) of the edge
{
        return Distance( G.target(e1), G.source(e1), weight );
}

double PL_GraphBase::Measure( const edge& e1 ) const
// Calculate the default weighted length (cost) of the edge
{
        return Distance( G.target(e1), G.source(e1) );
}


//=============================================================================
// NormDistWeight
//      
// Purpose:  Normalizes the default distance weight used in calculating the 
//               distance between two configurations.
//=============================================================================
void PL_GraphBase::NormDistWeight()
{
        // Get the DOF
        int dof = 0;
        if ( collisionDetector != NULL )
        {
                dof = collisionDetector->DOF();
        }
        
        distWeight.SetLength( dof );

        for ( int i = 0; i < dof; i++ )
        {
                distWeight[i] = 1;      // Set to unity
        }

}

//=============================================================================
// GetCspaceRange
//      
// Purpose: 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 PL_GraphBase::GetCspaceRange( const VectorN& weight ) const
{
        
        // Get the DOF
        int dof = 0;
        if ( collisionDetector != NULL )
        {
                dof = collisionDetector->DOF();
        }
        
        // Create a configuration that represents the bottom corner of the diagonal (all joints at their min).
        Configuration minConfig;
        minConfig.SetNumDOF( dof );

        // Create a configuration that represents the top corner of the diagonal (all joints at their max).
        Configuration maxConfig;
        maxConfig.SetNumDOF( dof );

        // Assign the respective joint limits to each joint.
        for (int i=0; i < dof; i++ )
        {
                double jmin = collisionDetector->JointMin( i );
                double jmax = collisionDetector->JointMax( i );
                
                if ( collisionDetector->JointWraps( i ) )
                {
                        // Joint wraps, so take the mid point as the max value.  This will be the furthest away from the jmin.
                        jmax = ( jmax + jmin ) / 2;
                }
                
                // Assign the joints
                minConfig[i] = jmin;
                maxConfig[i] = jmax;
        }

        // return the distance between the two end points of the diagonal
        return Distance( minConfig, maxConfig, weight );

}

double PL_GraphBase::GetCspaceRange() const
// Use Default distWeight
{
        return GetCspaceRange( distWeight );
}


        

//=============================================================================
// Locate Functions
//
// Purpose: Searches the graph and returns the edge/node associated with the
//                      parameters indicated.  If no successful edge/node is found, nil
//                      is returned and can be used by the calling function for error checking
//=============================================================================
node PL_GraphBase::FindConfig( const Configuration& c1 ) const
// Seach ALL nodes in graph, returning the node that matches the configuration
{
        node n1;

        // Search the entire graph, returning early if matching node is found.
        // Graph is searched in reverse order, assuming the desired configuration is one that
        // was recently added.
        forall_rev_nodes( n1, G )
        {
                if ( c1.Compare( G.inf(n1) , COMPTOL) )
                {
                        // Matching node found
                        return n1;
                }
        }

        // if still in function, no matching node was found
        return nil;
}

edge PL_GraphBase::FindEdge( const node& n1, const node& n2 ) const
// Search all joining edges to n1 for edge that joins to n2.  Return joining edge
{
        edge e1;
        
        // search all edge joining n1
        forall_inout_edges( e1, n1 )
        {
                // Assume edge is pointing from n1.  Grab the target node
                node othernode = G.target(e1);

                // check if edge is reversed
                if ( othernode == n1 )
                {
                        // edge is reversed and pointing to n1.  Grab the source node.
                        othernode = G.source(e1);
                }

                if ( othernode == n2 )
                {
                        // edge is joining n1 (verified because we are searching all edges attached to n1)
                        // and n2.  Return early with the edge.
                        return e1;
                }
        }

        // if we are still in the function, no edge exists between n1 and n2.
        return nil;
}


//=============================================================================
// GenerateRandomConfig
//
// Purpose: generates a random configuration
//=============================================================================
Configuration PL_GraphBase::GenerateRandomConfig ()
{
        int dof = collisionDetector->DOF() ;

        Configuration returnme ;
        returnme.SetLength( dof ) ;

        for( int i = 0; i < dof; i++ )
        {
                double max = collisionDetector->JointMax( i ) ;
                double min = collisionDetector->JointMin( i ) ;
                double random = 2*( double( rand() ) / RAND_MAX ) - 1; //this is a random number between [-1..1]
                double jointvalue = ( max - min ) * random;

                // Create a uniform distribution by having the random number go over the limits have wrapping the C-space;
                if ( jointvalue < min )
                {
                        jointvalue += (max - min);
                }
                else if ( jointvalue > max )
                {
                        jointvalue -= (max - min);
                }

                returnme[ i ] = jointvalue;
        }

        return returnme ;
}

//=============================================================================
// GenerateRandomConfig
//
// Purpose: generates a random configuration about a centre configuration,
//          within C-space and without wrapping.
//=============================================================================
Configuration PL_GraphBase::GenerateRandomConfig ( const Configuration& centre )
{
        enum LimitExceeded
        { 
                MAXLIMIT = 1,
                MINLIMIT = -1,
                NEITHER  = 0
        };

        int dof = collisionDetector->DOF() ;

        Configuration newconfig;
        newconfig.SetLength( dof );
        
        bool limits_exceeded = false;
        LimitExceeded* joint_limit_exceeded = new LimitExceeded[dof];
        assert ( joint_limit_exceeded != NULL );
        
        for( int i = 0; i < dof; i++ )
        {
                double max = collisionDetector->JointMax( i ) ;
                double min = collisionDetector->JointMin( i ) ;
                double random = ( double( rand() ) / RAND_MAX ); //this is a random number between [0..1]
                double jointvalue = ( max - min ) * random + min;

                // Create a distribution about centre by adding the jointvalue to the centre config.
                jointvalue += centre[ i ];

                // Check if joint exceeds limits
                if ( jointvalue < min ) 
                {
                        limits_exceeded = true;
                        joint_limit_exceeded[i] = MINLIMIT;
                }
                else if ( jointvalue > max )
                {
                        limits_exceeded = true;
                        joint_limit_exceeded[i] = MAXLIMIT;
                }
                else
                {
                        joint_limit_exceeded[i] = NEITHER;
                }

                newconfig[ i ] = jointvalue;
        }

        // check if any of the limits exceeded
        if ( limits_exceeded )
        {
                // get slopes between the centre and the new config
                VectorN slope = ( newconfig - centre );
                double t = 1;
                
                // determine how far along the line connecting centre and new config the first limit is exceeded.
                for ( int i = 0; i < dof; i++ )
                {
                        double limit;
                        if ( joint_limit_exceeded[i] == NEITHER )
                        {
                                // this joint does not exceed limit, continue with next iteration
                                continue;
                        }
                        else if ( joint_limit_exceeded[i] == MINLIMIT )
                        {
                                limit = collisionDetector->JointMin(i);
                        }
                        else
                        {
                                limit = collisionDetector->JointMax(i);
                        }

                        double s = ( limit - centre[i] ) / slope[i];

                        // find the earliest time the limits are exceeded.
                        if ( s < t )
                        {
                                // new earlier time
                                t = s;
                        }
                }

                // update the new configuration so it does not exceed the limits
                // by computing the config along the line that is at the first limit.
                newconfig = (slope * t) + centre;
        }

        delete [] joint_limit_exceeded;
        return newconfig ;
}



//=============================================================================
// GenerateRandomConfig
//
// Purpose: generates a random configuration based on a normal distribution about
//                      the given seed and standard devation.  
//=============================================================================
Configuration PL_GraphBase::GenerateRandomConfig( const Configuration& seed, const double& std_dev )
{
        int dof = seed.DOF() ;

        Configuration returnMe;
        returnMe.SetLength( dof );
        
        for( int i=0; i < dof; i++ )
        {
                double jointvalue = RandNorm( seed[i], std_dev/distWeight[i] );

                double jmax = collisionDetector->JointMax( i );
                double jmin = collisionDetector->JointMin( i );
                
                // Use wrapping of the C-space (for all joints) to ensure normal distribution about seed.
                double jrange = jmax - jmin;
                assert( jrange > 0 );
                if ( jrange == 0 )
                {
                        jointvalue = jmax;
                }
                while ( jointvalue > jmax )
                {
                        jointvalue -= jrange;
                }
                while ( jointvalue < jmin )
                {
                        jointvalue += jrange;
                }

                returnMe[i] = jointvalue;
        }

        return returnMe;

}


//=============================================================================
// IsIntering Functions
//
// Purpose: Used to determine if a node/configuration or a straight-line path between
//                      two valid nodes/configurations or along a linear edge is interference free.
//=============================================================================
bool PL_GraphBase::IsInterfering( const edge& e1 )
// Check if path along linear edge is NOT collision free
{
        return this->IsInterfering( G.target(e1), G.source(e1) );
}

bool PL_GraphBase::IsInterfering( const node& n1, const node& n2 )
// Check if linear path between n1 and n2 is NOT collision free
{
        return this->IsInterfering( G.inf(n1), G.inf(n2) );
}

bool PL_GraphBase::IsInterfering( const Configuration& c1, const Configuration& c2 )
// Check if linear path between c1 and c2 is NOT collision free
{
        return collisionDetector->IsInterferingLinear( c1, c2 );
}

bool PL_GraphBase::IsInterfering( const node& n1 )
// Check if n1 is interfering and therefore NOT valid.
{
        return this->IsInterfering( G.inf(n1) );
}

bool PL_GraphBase::IsInterfering( const Configuration& c1 )
// Check if c1 is interfering and therefore NOT valid.
{
        return this->collisionDetector->IsInterfering( c1 );
}

//=============================================================================
// DrawSpecific
//
// Purpose: Draw addition child planner specific information to the planner 
//              window when called.  
//
// Note:    This function is to be implemented in child classes only.  If there
//          is no additional planner specific info, THIS function will be called
//                      and implement nothing new.
//=============================================================================
bool PL_GraphBase::DrawSpecific() const
{
        return false;
}

//=============================================================================
// GetCoords
//
// Purpose: Pass appropriate coordinates by reference for the configuration stored
//                  stored in the given node/configuration.
//=============================================================================
void PL_GraphBase::GetCoords( const node n1, double& Xcor, double& Ycor, double& Zcor ) const 
// Using actual node
{
        GetCoords( G.inf(n1), Xcor, Ycor, Zcor);
}

void PL_GraphBase::GetCoords( const Configuration& c1, double& Xcor, double& Ycor, double& Zcor ) const
// Using configutation
{
        // Determine DOF available.
        int dof = c1.DOF();

        // Assign each of the Coordinates, if that DOF exists (C-space).  
        if ( dof > 0 )
        {
                Xcor = c1[0];
        }
        else
        {
                Xcor = 0;
        }

        if ( dof > 1 )
        {
                Ycor = c1[1];
        }
        else
        {
                Ycor = 0;
        }

        if ( dof > 2 )
        {
                Zcor = c1[2];
        }
        else
        {
                Zcor = 0;
        }

        return;
}

/*//=============================================================================
// NodeCount
//
// Purpose: Returns the number of nodes in the current graph.
//=============================================================================
int PL_GraphBase::NodeCount() const
{
        return G.number_of_nodes();
}

//=============================================================================
// EdgeCount
//
// Purpose: Returns the number of edges in the current graph.
//=============================================================================
int PL_GraphBase::EdgeCount() const
{
        return G.number_of_edges();
}*/

//=============================================================================
// SetStartConfig
//
// Purpose: Sets the start config and assigns it to a node in the current graph.
//
//                      This function does not connect the start node in the graph, but just
//                      adds it.  It is upto the child classes to connect the node
//                      if desired.
//=============================================================================
void PL_GraphBase::SetStartConfig( const Configuration& config )
{
        // Call the parent SetStartConfig to ensure all required initialization is established.
        PL_Boolean_Output::SetStartConfig( config );

        // Search the graph to see if this config is already saved.
        startNode = FindConfig( config );

        // Add config to graph if it is not already
        if ( startNode == nil )
        {
                startNode = G.new_node( config );
        }

}

//=============================================================================
// SetGoalConfig
//
// Purpose: Sets the goal config and assigns it to a node in the current graph.
//
//                      This function does not connect the goal node in the graph, but just
//                      adds it.  It is upto the child classes to connect the node
//                      if desired.
//=============================================================================
void PL_GraphBase::SetGoalConfig( const Configuration& config )
{
        // Call the parent SetGoalConfig to ensure all required initialization is established.
        PL_Boolean_Output::SetGoalConfig( config );

        // Search the graph to see if this config is already saved.
        goalNode = FindConfig( config );

        // Add config to graph if it is not already
        if ( goalNode == nil )
        {
                goalNode = G.new_node( config );
        }

}

//=============================================================================
// SetCollisionDetector
//
// Purpose: Assigns the CollisionDetector and extracts the DOF from it to allow
//                  other graph specific parameters to be defined.
//=============================================================================
void PL_GraphBase::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        // Call the parent's SetCollisionDetector to ensure proper assignment.
        PL_Boolean_Output::SetCollisionDetector( collisionDetector );

        // Assign the DOF to the start and goal configurations
        int dof = collisionDetector->DOF();

    //IJG_Assert( GetStartConfig().DOF() == dof );
    //IJG_Assert( GetGoalConfig().DOF() == dof );
        //startConfig.SetNumDOF( dof );
        //goalConfig.SetNumDOF( dof );

        // Initialize the Weight Function if not already
        if ( distWeight.Length() != dof )
        {
                NormDistWeight();
        }

}

//=============================================================================
// SetDistanceWeights
//
// Purpose: Assigns the distance weight vector for the graph.  If it has changed,
//                      the graph is cleared.
//=============================================================================
void PL_GraphBase::SetDistWeight( const VectorN& weights )
{
        if ( distWeight == weights )
        {
                return; // no change in distance vector.  Exit early
        }


        // Assign new weights
        distWeight = weights;

        // Clear the graph  ( use child clear if it exists )
        this->ClearGraph();

}

//=============================================================================
// PrioritizeEdge
//
// Purpose: Moves the given edge (or implied edge) between the source and target
//                  nodes of the edge to be the first adj_edge of the "source" node. 
//                      This allows an easy trace back from the goal node to the start node
//                      by following the first adj_edge associated with each node pointing to
//                      the prev node in the chain.  
//
//                      Function returns the success of prioritizing the edge.
//=============================================================================
bool PL_GraphBase::PrioritizeEdge(node sourcenode, edge e1)
// Makes the edge e1 the first adj_edge connect source node to the undirected target node.
// This function assumes the e1 already connects source and target nodes, but not necessarily
// in the correct direction or as the first adj_edge.  This function will make the edge e1
// connect the source node to whatever the current target node is.
{
        node targetnode = G.target(e1);

        // since this edge may be reversed, will have to flip it to get the correct node
        if (targetnode == sourcenode)
        {
                targetnode = G.source(e1);

                // if target node is still the same as the source, this edge is a self loop and should not be prioritized
                if (targetnode == sourcenode)
                {
                        return FALSE;
                }
        }


        edge e2 = G.first_adj_edge(sourcenode); // find current first edge for source node.
        if (e2 == nil)
        {
                // no adj_edges for source node, therefore can successful append this edge directly.
                G.move_edge( e1, sourcenode, targetnode );  // moves edge to point from source node to target node
        }
        else if (e1 != e2)
        {
                // if not already the first edge
                G.move_edge( e1, e2, targetnode, LEDA::before); // place desired edge before current first edge, pointing from 
                                                                                                           // source(e2) to target node.
        }

        return TRUE;            // process completed successful

}

bool PL_GraphBase::PrioritizeEdge(node prev, node curr)
// Makes the edge that connects from curr node to prev node the first adj_edge for the curr node,
// and sets the direction to point from curr to prev, thus making the curr the source and prev the
// target nodes for the edge.
{
        edge e1;
        edge e2;

        if (prev == curr)
        {
                // same node, there should not be an edge joining the two.
                return FALSE;
        }

                
        // Check all edges currently pointing from curr, searching for edge that points to prev
        forall_inout_edges(e1, curr)
        {
                if (( G.target(e1) == prev ) || ( G.source(e1) == prev ))
                {
                        // make edge first in the list (giving it top priority)
                        e2 = G.first_adj_edge(curr); // find current first edge for source (current) node.
                        if (e2 == nil)
                        {
                                // no adj_edges for curr node, therefore can successful append this edge directly.
                                G.move_edge( e1, curr, prev );  // moves edge to point from curr node to prev
                        }
                        else if (e1 != e2)
                        {
                                // if not already the first edge
                                G.move_edge( e1, e2, prev, LEDA::before); // place desired edge before current first edge, pointing from 
                                                                                                          // source(e2) to prev node.
                        }
                        return TRUE;            // process completed successful
                }
        }
        
        // if still in the function, edge does not exisit.
        return FALSE;  // process not successful
}

//=============================================================================
// GetMidPoint
//
// Purpose: Returns the midpoint between two configurations.  
//=============================================================================
Configuration PL_GraphBase::GetMidPoint( const Configuration& a, const Configuration& b ) const
{
        VectorN jointdisp = collisionDetector->JointDisplacement( a, b );

        Configuration midpt = a + ( jointdisp / 2 );    // midpt = a + (b-a)/2;

        return midpt;
}

//=============================================================================
// TranslatePath
//
// Purpose: Will take the collision free path through the nodes of the graph
//                      and create the PA_Points the server requires to animate the path.
//
//                      Function does not check if timer expires.  It will report either 
//                      success or failure.
//=============================================================================
SuccessResultType PL_GraphBase::TranslatePath()
{
        // Clear any current path.
        path.Clear();

        // Check if a graph path is available.
        if ( graphPath.empty() )
        {
                // Path is empty.  Set start as the beginning of the path, and return.
                path.AppendPoint( GetStartConfig() );

                // no path exists.
                return FAIL;
        }

        // If still in the function, there is a graphPath.
        // Go through all nodes stored in graphPath and assign the configuration they correspond to the
        // actual path.
        node n1;
        forall( n1, graphPath )
        {
                path.AppendPoint( G.inf(n1) );
        }


        // Path has been translated and is ready to be animated.
        return PASS;
}

//=============================================================================
// CopySettings
//
// Purpose: copies the settings from a different planner
//=============================================================================
void PL_GraphBase::CopySettings( PlannerBase* original )
{
        PL_GraphBase* originalGraphBase = dynamic_cast< PL_GraphBase* >( original );
        if( originalGraphBase == NULL )
        {
                return;
        }
        this->distWeight = originalGraphBase->distWeight;
}

//=============================================================================
// TranslateNodeID
//
// Purpose: Searches the entire graph structure, returning the node (ptr) associated
//                      with the given index.  Used for saving and loading of the graph data.
//=============================================================================
node PL_GraphBase::TranslateNodeID( const int& nodeID ) const
{
        // Quick check for nil node
        if ( ( nodeID == NIL_ID ) || 
                 ( G.empty() )  || 
                 ( nodeID > G.number_of_nodes() ) )
        {
                return nil;
        }

        // Search the graph from an appropriate end
        if ( nodeID < (G.number_of_nodes()/2) )
        {
                // nodeID in first half of the graph, start searching from the beginning
                node n1;
                forall_nodes( n1, G )
                {
                        if ( n1->id() == nodeID )
                        {
                                return n1;
                        }
                }
        }
        else
        {
                // nodeID is in second half of the graph, start searching from the end
                node n1;
                forall_rev_nodes( n1, G )
                {
                        if ( n1->id() == nodeID )
                        {
                                return n1;
                        }
                }
        }

        // if still in this function, an error has occurred.
        ASSERT( FALSE );

        return nil;
}

---