planners/prm/PL_PRM.cpp

Go to the documentation of this file.

//=========================================================================
//
//  File:  PL_PRM.cpp
//
//  Created by Ian Gipson, Modified by Shane Schneider.
//
//              Full and Lazy PRM planner.  Uses PL_GraphBase as parent class
//              the basic graph structure and access functions.  This planner
//              will modify the graph and implement the PRM planners.

// PL_PRM
#include "synchronization/semaphore.h"
#include "PL_PRM.h"
#include "smoothers\SmootherBase.h"
// LEDA Include Files
#include <LEDA/node_map.h> 
#include <LEDA/node_pq.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;
using std::istrstream;

//--------------------- Constants ---------------
static const double DEFAULT_TOL = 0.1;          // 10% of CSpaceRange.
static const int DEFAULT_INIT_QUANT = 100; // Minimal Number of nodes to maintain in a roadmap
static const int DEFAULT_ENHANCE_QUANT = 50;  // Number of nodes to add during enhancement.
static const double DEFAULT_SEED_RATIO = 0.5;   // Default ratio between seeded nodes and uniform nodes.
static const int DEFAULTBASECONNECTID = 0;              // Default baseConnect ID for tree graphs.
// File Constants
static const char FILEEXT[] = ".prm";           // File extension for a base graph planner
static const char FILEHEADER[] = "PL_PRM";


// Class PL_PRM 

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


        // Initialize the node and edge checked maps to the correct graph.
        edgeChecked.init(G,FALSE);
        nodeChecked.init(G,FALSE);

        // Assume node are to be added uniformly.
        uniformlyAdded.init(G,TRUE);

        // Assign the open ptr
        openp = NULL;

        // Assign the tree connected component ID
        baseConnectID = DEFAULTBASECONNECTID;
        connectIDp = NULL;

        // Default Parameter Settings
        SetRadiusTol( DEFAULT_TOL );
        lazyMode = TRUE;
        validNodesOnly = FALSE;
        validEdgesOnly = FALSE;
        useMidPts = FALSE;

        initQuant    = DEFAULT_INIT_QUANT;
        enhanceQuant = DEFAULT_ENHANCE_QUANT;
        seedRatio        = DEFAULT_SEED_RATIO;

        // Start with a blank list of config seeds.
        config_seeds.clear();


        // Set DOF and initialize the diagonal Length.
        diagonal_squared = 0;

        // Since we are starting a new planner. We want to start a new planning session.
        lastPlanningState = PRM_START;
}


PL_PRM::~PL_PRM()
{
        // Delete the open list pointer
        if ( openp != NULL )
        {
                delete openp;
                openp = NULL;
        }

        // Delete the connection ID node map if it exists.
        if ( connectIDp != NULL )
        {
                delete connectIDp;
                connectIDp = NULL;
        }
}


//=============================================================================
// DrawExplicit
//
// Purpose: Draws the planner data to the planner window when called
//                      This draw is a modified version from the GraphBase drawer since
//                      we want to use different colours to inidacate which edges and nodes 
//          have been checked.
//=============================================================================
bool PL_PRM::DrawExplicit () const
{
        // IMPROVE:  Add Open GL code to inser the graph here.
        double Xcor, Ycor, 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 )
                {
                        glColor3f(1.0f,1.0f,0.0f);              // set colour to yellow
                        GetCoords( startNode, Xcor, Ycor, Zcor );
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
                }

                // Draw the Goalnode, if valid.
                if ( goalNode != nil )
                {
                        glColor3f(0.0f,1.0f,1.0f);              // set colour to cyan
                        GetCoords( goalNode, Xcor, Ycor, Zcor );
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
                }
        glEnd();

        // Draw all remainning nodes.
        glBegin(GL_POINTS);
                // Search all nodes of the graph and draw them unless they are start and goal.
                forall_nodes( n1, G )
                {
                        if ( ( n1 != startNode ) && ( n1 != goalNode ) )
                        {
                                if ( nodeChecked[n1] )
                                {
                                        if ( uniformlyAdded[n1] )
                                        {
                                                // uniformly collision checked node
                                                glColor3f(1.0f,0.0f,0.0f);              // set colour to red
                                        }
                                        else
                                        {
                                                // seeded collision checked node
                                                glColor3f(0.0f,1.0f,0.0f);              // set colour to green
                                        }
                                }
                                else if ( uniformlyAdded[n1] )
                                {
                                        // uniformly non-checked node
                                        glColor3f(0.75f,0.4f,0.0f);             // set colour to dark orange
                                }
                                else
                                {
                                        // all others (seeded non-checked nodes)
                                        glColor3f(0.0f,0.5f,0.0f);              // set colour to dark green
                                }

                                GetCoords( n1, Xcor, Ycor, Zcor );      // Get Coordinates of the current node
                                glVertex3f( (float)Xcor, (float)Ycor, (float)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 );
                                glColor3f(1.0f,0.5f,0.0f);      // set colour to orange
                                glVertex3f( (float)Xcor, (float)Ycor, (float)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 );
                                glColor3f(1.0f,1.0f,0.0f);      // set colour to yellow
                                glVertex3f( (float)Xcor, (float)Ycor, (float)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;
                        if (edgeChecked[e1])
                        {
                                glColor3f(0.0f,0.0f,1.0f);      // set colour to blue
                        }
                        else
                        {
                                glColor3f(0.0f,0.3f,0.5f);      // set colour to dark blue
                        }
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line.

                        GetCoords( G.target(e1), Xcor, Ycor, Zcor );    // get coordinates of target node;
                        if (edgeChecked[e1])
                        {
                                glColor3f(1.0f,1.0f,1.0f);      // set colour to white
                        }
                        else
                        {
                                glColor3f(0.5f,0.5f,0.5f);      // set colour to grey
                        }
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line.
                }
        glEnd();

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

        // Unlock Semaphore
        semaphore.Unlock();
        Sleep( 15 );

        return true;
}


//=============================================================================
// LoadContents
//
// Purpose: Loads the graph and other related base planner parameters to the the
//                  given ifstream.
//      
//                      Returns success of load.
//=============================================================================
bool PL_PRM::LoadContents( std::ifstream& infile )
{
        // Load the Graph and other base parameters
        bool success = PL_GraphBase::LoadContents( infile );
        if ( !success )
        {
                return false;
        }
        
        // Load the PRM Parameters
        infile >> radiusTol;
        infile >> initQuant >> enhanceQuant >> seedRatio;
        infile >> lazyMode >> validNodesOnly >> validEdgesOnly >> useMidPts;

        // The graph mode will depend on the PRM mode type (lazy or full).  Since
        // the graph has just been loaded, DO NOT call SetGraphMode to initialize the graph
        // mode as this may clear the just loaded graph.  Instead, use the lazyMode switch
        // to determine if the connectID list should be initialized and loaded.
        if ( !lazyMode )
        {
                // Will be using a tree graph.  Initialize the list and set the base counter.
                infile >> baseConnectID;  // this number will only be saved in a tree graph because of backwards
                                                                  // compatibility.
                if ( connectIDp != NULL )
                {
                        delete connectIDp;
                }
                connectIDp = new node_map<int>(G);
        }
        else
        {
                // using a fully connected graph.  Delete any current connectionID list.
                baseConnectID = DEFAULTBASECONNECTID;
                if ( connectIDp != NULL )
                {
                        delete connectIDp;
                        connectIDp = NULL;
                }
        }


        // Load the Node Information
        node n0;
        forall_nodes( n0, G )
        {
                int nodeID;
                infile >> nodeID;
                infile >> nodeChecked[n0] >> uniformlyAdded[n0];

                if ( !lazyMode )
                {
                        // this item is only saved in a tree graph.
                        infile >> (*connectIDp)[n0];
                }
        }

        // Error checking.  The next entry in file should be the nil id
        int id;
        infile >> id;
        if ( id != NIL_ID )
        {
                // Error has occurred.
                MessageBox( NULL, "Node and Edge information do not align with current Graph.", "ERROR", MB_OK );
                return false;
        }

        // Load the Edge information.
        edge e1;
        forall_edges( e1, G )
        {
                int edgeID;
                infile >> edgeID;
                infile >> edgeChecked[e1];
        }

        // Since we have loaded a new graph, inform the planner we want to start a new graph search.
        lastPlanningState = PRM_START;

        return true;
        
}

//=============================================================================
// SaveContents
//
// Purpose: Saves the graph and other related base planner parameters to the the
//                  given ofstream.
//      
//                      Returns success of save.
//=============================================================================
bool PL_PRM::SaveContents ( std::ofstream& outfile ) const
{
        // Save the Graph and parent parameters
        bool success = PL_GraphBase::SaveContents( outfile );
        if ( !success )
        {
                return false;
        }

        // Save the PRM parameters
        outfile << radiusTol << endl;
        outfile << initQuant << " " << enhanceQuant << " " << seedRatio << endl;
        outfile << lazyMode << " " << validNodesOnly << " " << validEdgesOnly << " " << useMidPts << endl;

        // Save the baseconnectID only if a tree graph has been saved.
        // In a fully connected graph, this number will not be saved, thus ensuring backwards compatibilty with previous
        // saved PRM files.
        if ( !lazyMode )
        {
                outfile << baseConnectID << endl;
        }

        // Save Node Information
        node n1;
        forall_nodes( n1, G )
        {
                outfile << n1->id() << " " << nodeChecked[n1] << " " << uniformlyAdded[n1];

                // Save the connectionID if a tree graph
                if ( !lazyMode )
                { 
                        outfile << " " << (*connectIDp)[n1];
                }

                // append the end of line character.
                outfile << endl;

        }

        // Add error checking to file, seperate node info from edge info by adding nil
        outfile << NIL_ID << endl;

        // Save the Edge Information
        edge e1;
        forall_edges( e1, G )
        {
                outfile << e1->id() << " " << edgeChecked[e1] << endl;
        }

        // Last Planning state will be refreshed in the load.
        
        return true;
}

//=============================================================================
// Does Planning
//
// Purpose: performs the planning task
//
//                      This function uses a state machine to determine what is the next
//                      step in the planning algorithm.  See the functions associated
//                      with each state for explanation of how it works.
//=============================================================================
bool PL_PRM::Plan ()
{
        srand( (unsigned)time( NULL ) );

        PRM_StateMachineType currentState = lastPlanningState;

        // Nodes may have been added since last plan.  Allow planner to check roadmap again
        // IMPROVE:  Is this still needed?
        if ( lastPlanningState == PRM_FAILURE )
        {
                currentState = PRM_START;
        }

        // Start the timer
        StartTimer();

        // Establish the semaphore
        Semaphore semaphore( guid );
        if( this->m_UseSemaphores == true )
        {
                semaphore.Lock();
        }
        

        // Some quick checks to determine if we really need to plan
        if ( (currentState != PRM_DONE ) && (currentState != PRM_FAILURE) ) 
        {
                if ( startNode == goalNode )
                {
                        // start and goal are the same.  Do not plan.
                        currentState = PRM_DONE;

                        // Assign a small path.
                        path.Clear();
                        path.AppendPoint( GetStartConfig() );
                }
                else if ( (IsInterfering(startNode)) || (IsInterfering(goalNode)) )
                {
                        // either the start or goal are already interfering.  Plan will
                        // automatically fail.
                        currentState = PRM_FAILURE;
                }
        }
        if( this->m_UseSemaphores == true )
        {
                semaphore.Unlock();
        }

        // Ensure the diagonal length is correct
        diagonal_squared = GetCspaceRange();
        
        // State Machine
        while (( currentState != PRM_DONE ) && ( currentState != PRM_FAILURE )) 
                // NOTE:  The PRM_FAILURE state should eventually be removed!
        {
                SuccessResultType success;

                if( this->m_UseSemaphores == true )
                {
                        semaphore.Lock();
                }
                // Determine the State function to call for this iteration
                switch ( currentState )
                {
                        case PRM_BUILD_INIT_ROADMAP :
                                success = BuildInitRoadMap();
                                break;
                        case PRM_FIND_PATH :
                                success = FindPath();
                                break;
                        case PRM_VERIFY_PATH :
                                success = VerifyPath();
                                break;
                        case PRM_TRANSLATE_PATH :
                                success = TranslatePath();
                                break;
                        case PRM_ENHANCE_ROADMAP :
                                success = EnhanceRoadMap();
                                break;
                        default :
                                // No state functions are associated with this state.
                                // Do nothing except prepare for next iteration.
                                success = PASS;
                }  // end SWITCH


                // Check if timer has expired and one of the state functions had returned early.
                if ( success == TIMER_EXPIRED )
                {
                        // Escape the state machine.
                        break;
                }

                // Determine the State Function to call for the next iteration
                switch ( currentState )
                {
                        case PRM_START :
                                // Start by building the initial roadmap.
                                currentState = PRM_BUILD_INIT_ROADMAP;  
                                break;
                        case PRM_BUILD_INIT_ROADMAP :
                                // Search current road map for a short path.
                                currentState = PRM_FIND_PATH;                   
                                break;
                        case PRM_FIND_PATH :
                                if ( success == PASS )
                                {
                                        // A path has been found, verify it has no collision.
                                        currentState = PRM_VERIFY_PATH;
                                }
                                else
                                {
                                        // No path was found.  Enhance some of the trouble areas.
                                        currentState = PRM_ENHANCE_ROADMAP;
                                        //currentState = PRM_FAILURE;           // NOTE:  Once EnhanceRoadmap is working,
                                                                                                        //                the PRM_FAILURE state should be removed.
                                }
                                break;
                        case PRM_VERIFY_PATH :
                                if ( success == PASS )
                                {
                                        // Path represented by valid nodes is successful. Translate it so the
                                        // main program can implement it.
                                        currentState = PRM_TRANSLATE_PATH;
                                }
                                else
                                {
                                        // Path was not collision free.  Need to find a new path now that the troublesome
                                        // nodes/edges have been removed.
                                        currentState = PRM_FIND_PATH;
                                }
                                break;
                        case PRM_TRANSLATE_PATH :
                                // We are done.  Escape state machine and report success.
                                currentState = PRM_DONE;
                                break;
                        case PRM_ENHANCE_ROADMAP :
                                // Try finding a path again with the new nodes and edges added.
                                currentState = PRM_FIND_PATH;
                                break;
                        default :
                                // an unexpected or invalid state has occurred.  Crash the program!
                                ASSERT( FALSE );
                } // end SWITCH.

                // Release semaphore temporary.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }
        }  // END State Machine


        // save the current state of the machine for the next time the planner is called.
        lastPlanningState = currentState;

        // Report the success of the planner.
        if ( currentState == PRM_DONE )
        {
                // planner has completed.  A path has been found and saved.
                        
                // Release semaphore permanetly.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }

                //smooth the path
                if( m_Smoother != NULL )
                {
                        this->m_Smoother->SetPath( &this->path );
                        this->m_Smoother->SetCollisionDetector( this->collisionDetector );
                        m_Smoother->Smooth();
                        this->path = m_Smoother->GetPath();
                        m_Smoother->SetPath( NULL );
                }

                return true;
        }
        else
        {
                // planner has not completed and has terminated early
                graphPath.clear();

                // Assign a small path.
                path.Clear();
                path.AppendPoint( GetStartConfig() );

                // Release semaphore permanetly.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }

                return false;
        }

}  // END Plan

//=============================================================================
// SetRadiusTol
//
// Purpose: Sets the maxRadius for connecting to close neighbours by a given tol.
//                      Setting by Tol allows the user not to worry about the size and range
//                      of the Cspace customized by the weight function.
//=============================================================================
void PL_PRM::SetRadiusTol( double tol )
{
        // Store the given tolerence
        radiusTol = tol;
        
}

//=============================================================================
// GetRadiusTol
//
// Purpose: Determines what the tol was in order to set the maxRadius.
//                      Used for updating the Dialog Window
//=============================================================================
double PL_PRM::GetRadiusTol()
{
        // Return the tol
        return radiusTol;
}

//=============================================================================
// SetPRMMode
//
// Purpose: Sets the mode dependent switches for the different variations of PRM
//=============================================================================
void PL_PRM::SetPRMMode ( BOOL& isLazy, BOOL& validNodes, BOOL& validEdges )
{
        // Mode Switches:  Lazy OFF => have to use validNodes and validEdges
        //                                 if either validNodes or validEdges are off, then will be
        //                                 running some form of lazy, therefore lazy is turned ON.

        // Figure out which of the modes a change has been requested.
        if ( lazyMode != isLazy )
        {
                // Update the dependent mode requests for non-lazy mode.
                if ( !isLazy )
                {
                        // have to use valid Nodes and valid Edges
                        validNodes = TRUE;
                        validEdges = TRUE;
                }
        }
        else 
        {
                // update the requirement for a form of lazy if valid nodes or edges have not been requested
                if ( (!validNodes) || (!validEdges) )
                {
                        isLazy = TRUE;
                }
        }

        // Check if lazy mode has been changed, thus requiring the graph mode to be changed
        if ( lazyMode != isLazy )
        {
                // Tree graph is required if lazy is turned off, and a fully connected graph is turned on.
                SetGraphMode( !isLazy );
        }

        // Update the switches with any mode changes
        lazyMode = isLazy;
        validNodesOnly = validNodes;
        validEdgesOnly = validEdges;
        
}


//=============================================================================
// GetPRMMode
//
// Purpose: Gets the mode dependent switches for the different variations of PRM
//=============================================================================
void PL_PRM::GetPRMMode ( BOOL& isLazy, BOOL& validNodes, BOOL& validEdges ) const
{
        // Return the current mode settings
        isLazy = lazyMode;
        validNodes = validNodesOnly;
        validEdges = validEdgesOnly;

}

//=============================================================================
// SetCollisionDetector
//
// Purpose: Assigns the collision detector and then updates PRM specific parameters
//                  based on the collision detector.
//=============================================================================
void PL_PRM::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        // Call Parent's Collision Detector and assign the collision detector
        PL_GraphBase::SetCollisionDetector( collisionDetector );

        // Calculate the diagonal length now that we have some DOF
        diagonal_squared = GetCspaceRange();


}

//=============================================================================
// SetStartConfig
//
// Purpose: Sets the start config and connects the node associated with it to 
//                      the current graph.
//=============================================================================
void PL_PRM::SetStartConfig( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( GetStartConfig() == config ) && ( startNode != nil ) )
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // Set the startConfig, add it to the graph, and assign the startNode to it by
        // calling the parent's SetStartConfig.
        PL_GraphBase::SetStartConfig( config );

        // Verify the startNode was initiated.
        ASSERT( startNode != nil );

        // Check if the startNode is already graph connected and connect it if it isn't.
        // StartNode could possible not be graph connected because it was just added.
        if ( G.degree( startNode ) < 1 )
        {
                // node is not connected.  Do so now.
                ConnectNode( startNode );
        }

        // Since we have added a new configuration, we want to start a fresh planning session,
        // so indicate we want to start a new session.
        lastPlanningState = PRM_START;

}

//=============================================================================
// SetGoalConfig
//
// Purpose: Sets the goal config and connects the node associated with it to 
//                      the current graph.
//=============================================================================
void PL_PRM::SetGoalConfig( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( GetGoalConfig() == config ) && ( goalNode != nil ))
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // Set the goalConfig, add it to the graph, and assign the goalNode to it by
        // calling the parent's SetGoalConfig.
        PL_GraphBase::SetGoalConfig( config );

        // Verify the goalNode was initiated.
        ASSERT( goalNode != nil );

        // Check if the goalNode is already graph connected and connect it if it isn't.
        // GoalNode could possible not be graph connected because it was just added.
        if ( G.degree( goalNode ) < 1 )
        {
                // node is not connected.  Do so now.
                ConnectNode( goalNode );
        }

        // Since we have added a new configuration, we want to start a fresh planning session,
        // so indicate we want to start a new session.
        lastPlanningState = PRM_START;

}


//=============================================================================
// SetGraphMode
//
// Purpose:  Sets the graph mode for either connect tree or fully connected graph.
//                       If set for tree mode, the current graph is cleared and the connection ID lists
//                       are intialized.  
//                       If set for fully connected graph, then the existing graph is kept, but the
//                       connection ID lists are deleted.  To switch back to tree graph, the graph
//                       will be cleared.
//=============================================================================
void PL_PRM::SetGraphMode ( const bool& treeGraph )
{
        if ( treeGraph )
        // Update Graph for a tree graph
        {
                // Clear the existing graph.
                ClearGraph();

                // Initialize the connection ID pointer and base counter
                baseConnectID = DEFAULTBASECONNECTID;
                if ( connectIDp != NULL )
                {
                        delete connectIDp;
                }
                connectIDp = new node_map<int>(G,0);
        }
        // Update the graph for a fully connected graph
        else {
                // Delete the connectionID list.  
                if ( connectIDp != NULL )
                {
                        delete connectIDp;
                        connectIDp = NULL;
                }

                baseConnectID = DEFAULTBASECONNECTID;
        }


}

//=============================================================================
// NodeInConnectionList
//
// Purpose: Checks if the given node has a connectionID that matches the given
//                      connection ID list.  Returns FALSE if connectIDp is not valid.
//=============================================================================
bool PL_PRM::NodeInConnectionList( const node& n1, const list<int>& connectIDs)
{
        if ( connectIDp == NULL )
        {
                return false;
        }

        int id;

        forall( id, connectIDs )
        {
                if ( (*connectIDp)[n1] == id )
                {
                        return true;
                }
        }

        // if still in function, node is not in given connectID list.
        return false;

}


//=============================================================================
// AddNode
//
// Purpose: Adds a uniformly distributed random configuration into the graph.
//                      Returns TRUE if the node was successfully added.
//=============================================================================
BOOL PL_PRM::AddNode()
{
        Configuration newConfig;

        bool validConfig = FALSE;
        while ( !validConfig )
        {
                newConfig = GenerateRandomConfig();

                if ( validNodesOnly )
                // Check if collision detection is requried
                {
                        // check if config is valid
                        if ( IsInterfering( newConfig ) )
                        {
                                validConfig = FALSE;

                                // add this config as a seed 
                                config_seeds.append( newConfig );
                        }
                        else
                        {
                                validConfig = TRUE;
                        }
                }
                else
                {
                        // since we do not care if invalid nodes are added, all configs are considered valid
                        validConfig = TRUE;
                }

                if ( HasTimeLimitExpired() )
                {
                        return FALSE;
                }
        }  // end while


        node newNode = G.new_node( newConfig );

        if (newNode != nil)
        {
                // Report if node has been checked for collisions by its addition type.
            nodeChecked[newNode] = validNodesOnly;

                // Node has been added uniformly
                uniformlyAdded[newNode] = TRUE;

                // Connect the new node
                ConnectNode( newNode );

                return TRUE;    // node was successfully added
        }
        else
        {
                return FALSE;   // node was not successfullt added.
        }
}

//=============================================================================
// ConnectNode
//
// Purpose: Connects the given node with edges to neighbours within the tolerance
//                  radius, calculated from the radiusTol and the diagonal length.
//                      
//                      This function calls the appropriate connect edges style function depending
//                      on the PRM implemenation selected.
//=============================================================================
void PL_PRM::ConnectNode( const node& newnode )
{
        if ( lazyMode )
        {
                // Lazy PRM selected
                //    radiusTol are squared since we are dealing with squared distances, therefore the fraction
                //        also needs to be squared:  (frac*dist)^2 = frac^2*dist^2 = frac^2*(dist_squared);
                ConnectEdgesLazy( newnode, radiusTol*radiusTol*diagonal_squared );
        }
        else
        {
                // Full PRM selected
                ConnectEdgesFull( newnode, radiusTol*radiusTol*diagonal_squared );
        }

}


//=============================================================================
// ConnectEdgesLazy
//
// Purpose: Connects the node to the graph Lazy PRM Style:  connects to all nodes 
//              within given sqauared radius.  Does not perform any type of collision 
//                      detecting when connecting edges.
//=============================================================================
void PL_PRM::ConnectEdgesLazy( const node& newnode, const double& radius_squared )
{
        // Save calculation time by determining configuration of the node once intstead of in each iteration
        Configuration nodeconfig = G.inf(newnode);

        //  Search all nodes in graph, selecting those within radius and add an edge to join them with new node
        node testnode;
        forall_nodes( testnode, G )
        {
                if ( newnode != testnode)
                {
                        double edgelength_squared = Distance( nodeconfig, G.inf(testnode) );  // returns the squared distance
                        // Check if the two nodes are within radius
                        if (  edgelength_squared <= radius_squared )  // can compare squares since a^2<b^2 if |a|<|b|
                        {
                                bool addedge = TRUE;

                                // connect edge based on its addition type
                                if ( validEdgesOnly )
                                {
                                        if ( IsInterfering( nodeconfig, G.inf(testnode) ) )
                                        {
                                                addedge = FALSE;

                                                // Add the midpoint of this potential edge as a seed, if requested
                                                if ( useMidPts )
                                                {
                                                        Configuration midpt = GetMidPoint( nodeconfig, G.inf(testnode) );
                                                        config_seeds.append( midpt );
                                                }
                                        }
                                } // end if ValidEdges

                                if ( addedge )
                                {
                                        // connect the two nodes with an edge and store the actual distance (sqrt) as the cost of the length
                                        double edgelength = sqrt( edgelength_squared );
                                        edge newedge = G.new_edge ( newnode, testnode, edgelength ); 

                                        // new edge has been collision if a validEdge was requested.
                                        edgeChecked[newedge] = validEdgesOnly;
                                }  // end if addedge
                        }  // end if within neighbourhood
                }  // end if not testnode
        }  // end for loop
} // end ConnectEdgesLazy


//=============================================================================
// ConnectEdgesFull
//
// Purpose: Connects the node to the graph Full PRM Style:  tree graph connect to 
//          closest nodes only; any other nodes that are within the given squared radius, 
//          but are already graph connected will not be connected again.
//
//                      This function will perform collision detection for each potential edge,
//                      only connecting those that do not contain a collision.
//=============================================================================
void PL_PRM::ConnectEdgesFull(const node& newnode, const double& radius_squared)
{
        // verify connectIDp is valid.
        ASSERT( connectIDp != NULL );

        // Save calculation time by determining configuration of the node once intstead of in each iteration
        Configuration nodeconfig = G.inf(newnode);

        node n1;

        // create a list of neighbour nodes to connect to within the given radius.
        node_pq<double> neighbours(G);
        forall_nodes( n1, G)
        {
                if ( newnode != n1)
                {
                        double edgelength_squared = Distance( nodeconfig, G.inf(n1) );  // returns the squared distance
                        // Check if the two nodes are within radius
                        if (  edgelength_squared <= radius_squared )  // can compare squares since a^2<b^2 if |a|<|b|
                        {
                                // node is within radius, add it as a neighbour with its distance as its priority.
                                neighbours.insert( n1, edgelength_squared );
                        }
                }
        }

        // New node is currently not connected to anyone, assign it the NIL_ID.
        (*connectIDp)[newnode] = NIL_ID;

        // Create a running list of graph connected components new node is attached to.
        list<int> connectIDs;

        // While the list of neighbour is not empty, connect the node to the closests nodes, not already graph
        // connected to.
        while ( !neighbours.empty() )
        {
                // get closest neighbour and remove it from the list
                node neighbour = neighbours.find_min();
                double dist_sq = neighbours.inf( neighbour );
                neighbours.del( neighbour );

                // connect the neighbour to the new node if it is not already graph connected.
                // A node is graph connected if its current connectID is in the running list of
                // connected graph components or has the same connectID as the new node.
                if ( ((*connectIDp)[neighbour] == NIL_ID) ||
                         ( ((*connectIDp)[neighbour] != (*connectIDp)[newnode]) &&
                           ( !NodeInConnectionList(neighbour, connectIDs) ) )
                   )
                {
                        // nodes are not aleady connected.  Connect them now if there is a collision free path.
                        if ( !IsInterfering( nodeconfig, G.inf(neighbour) ) )
                        {
                                double edgelength = sqrt( dist_sq );
                                edge newedge = G.new_edge( newnode, neighbour, edgelength );
                                edgeChecked[newedge] = TRUE;

                                // update the connectionIDs to show these two nodes are now graph connected.
                                if ( (*connectIDp)[neighbour] != NIL_ID )
                                // neighbour node has been previously connected to a graph component.
                                {
                                        if ( (*connectIDp)[newnode] == NIL_ID )
                                        {
                                                // node has not been previously connected, assign it the connect ID of its new neighbour.
                                                (*connectIDp)[newnode] = (*connectIDp)[neighbour];
                                        }
                                        else if ( (*connectIDp)[neighbour] < (*connectIDp)[newnode] )
                                        {
                                                // the neighbour node has the smaller ID.  Select this as the new ID for the two
                                                // newly connected graph components (one from the new neighbour, one from the new node).

                                                // save the old connectionID into the running list to be updated later.
                                                connectIDs.push( (*connectIDp)[newnode] );

                                                // assign the lower connection ID to the new node.
                                                (*connectIDp)[newnode] = (*connectIDp)[neighbour];
                                        }
                                        else
                                        {
                                                // the new node has the smaller ID.  Select this as the new ID for the two components.

                                                // save the old connectionID into the running list to be updated later.
                                                connectIDs.push( (*connectIDp)[neighbour] );

                                                // assign the lower connection ID to the neighbour.
                                                (*connectIDp)[neighbour] = (*connectIDp)[newnode];
                                        }
                                }  // end if neighbour != NIL_ID
                                else
                                // neighbour has not been previously connected.
                                {
                                        if ( (*connectIDp)[newnode] != NIL_ID )
                                        {
                                                // new node has been previously connected, assign the neighbour its ID.
                                                (*connectIDp)[neighbour] = (*connectIDp)[newnode];
                                        }
                                        else
                                        {
                                                // both nodes have not been previously connected.  Create a new graph connected
                                                // component by assigning these two nodes the baseConnectID.
                                                (*connectIDp)[newnode] = baseConnectID;
                                                (*connectIDp)[neighbour] = baseConnectID;

                                                // Update the base connect ID to ensure a new unique ID is available for next new node.
                                                baseConnectID++;
                                        }
                                }  // end update connectIDs.

                        } // end if interfering
                        else
                        {
                                // nodes have a collision path.  Use the midpoint between these two nodes as a seed for enhancing.
                                // Add the midpoint of this potential edge as a seed, if requested
                                if ( useMidPts )
                                {
                                        Configuration midpt = GetMidPoint( nodeconfig, G.inf(neighbour) );
                                        config_seeds.append( midpt );
                                }
                        }  // end if interfering else.

                }       // end if not already graph connected.

        } // end while !neighbours.empty().

        // Search the entire graph, if required, updating connectIDs to reflect how new node has graph connected
        // different components of the graph.
        if ( ((*connectIDp)[newnode] != NIL_ID) && ( !connectIDs.empty() ) )
        {
                // new node is connected to more than one graph connected component.  Update the connect IDs of
                // affected nodes.
                int newID = (*connectIDp)[newnode];

                forall_nodes( n1, G )
                {
                        if ( ((*connectIDp)[n1] != newID ) && (NodeInConnectionList( n1, connectIDs )) )
                        {
                                (*connectIDp)[n1] = newID;
                        }
                }
        }  // end update connectIDs of entire graph

                
        return;
}       // end ConnectEdgesFull


//=============================================================================
// BuildInitRoadMap
//
// Purpose: Adds nodes according to specified PRM implemenation until the
//                  mininium amount of nodes have been added.
//
//                      Function will exit early if the timer has expired.
//=============================================================================
SuccessResultType PL_PRM::BuildInitRoadMap()
{
        // Keep adding additional nodes and connect them until initQuant is acheived.
        for (int i = NodeCount(); i < initQuant; i++ )
        {
                // Add the nodes
                AddNode();

                // Check if Time has expired
                if ( HasTimeLimitExpired() )
                {
                        return TIMER_EXPIRED;
                }

        }  // end For Loop

        return PASS;
}

//=============================================================================
// FindPath
//
// Purpose: Performs the A* search on the current roadmap (graph) to determine
//                      a [shortest] path between start and goal nodes.  If no path is found,
//                      the function will return failure.
//
//                      Function will exit early if the timer has expired.
//=============================================================================
SuccessResultType PL_PRM::FindPath()
{

        // Going to emulate the A* Algorithm.
        node n1;                    //stores the current node

        // All storage structures are stored as private members or pointers of this class.
        
        graphPath.clear();      // Clears any previous preposal path.
                                        

        // Check if we are continuing from a previously expired search, and if not, initialize a fresh search.
        if ( lastPlanningState != PRM_FIND_PATH )
        // We are starting fresh.  Initialize the search.
        {
                // Initialize search lists.
                pred.init(G,nil);       // Associates pred edge pointers to current graph and initializes them to nil.
                dist.init(G,0);         // Associates the current cost of each of the nodes with the current graph.
                
                // Assign the cost of the first node
                dist[startNode] = 0;

                // Create the a new priority queue of open nodes.
                if ( openp != NULL )
                {
                        delete openp;
                }
                openp = new node_pq<double>(G);
                openp->clear();

                // Start Open queue
                openp->insert(startNode,0);     // priority set with 0 to ensure it is the first to be selected
        }
        else
        // We are using an exisiting search.  Leave all parameters as before.
        {
                // Indicate we have entered this loop because the previous search expired and ensure
                // we re-enter this state function fresh on the next iteration.
                lastPlanningState = PRM_TIMER_EXPIRED;

        }


        // While OPEN is not empty
        while ( !openp->empty() )
        {
                // Check to make sure timer has not expired.
                if ( HasTimeLimitExpired() )
                {
                        // Timer has expired.

                        // current state variables are saved, include the open priority queue which is not deleted.

                        // Report the timer has expired.
                        return TIMER_EXPIRED;
                }

                //Remove the lowest cost node from open queue
                n1 = openp->del_min();

                // check if goal has been found
                if ( n1 == goalNode )
                // found goal.
                {
                        // Store the current path.
                        graphPath.clear();
                        graphPath.push( n1 );

                        // Starting with the goal, go up through path, adding each of the prev nodes
                        // to the path as they are found.
                        while ( n1 != startNode )
                        {
                                node n0 = pred[n1];             // get the prev node
                                
                                // Add prev node to front of the path.
                                graphPath.push( n0 );

                                // Prepare for next iteration: the prev node will be the current node.
                                n1 = n0;
                        }

                        // No longer need the open priority queue list.  Delete it.
                        delete openp;
                        openp = NULL;

                        // A path has been successfully found.
                        return PASS;
                                        
                }
                else
                {
                        // Expand all successor nodes (all nodes that graph connected to current node)
                        // NOTE:  TO find the successor nodes, we search all nodes connected by edges (bi-diretional) to
                        //                the current node.  Eventually the prev node in the path will be selected, however, it will
                        //                never be updated as one of the potential successor nodes since the current cost of getting
                        //                to the current node, includes going through the previous node.  This means a newcost of the
                        //                actual cost to the current node plus the cost of travelling back from the current node
                        //                to the prev node will be higher than the cost of getting to the prev node by not passing through
                        //                the current node.  Cost:  cost[A] < cost[A] + cost(A to B) + cost(B back to A)!

                        node n2;        // successor node.  n1 = curr node.
                        edge e1;        // scanning edge.

                        
                        forall_inout_edges(e1, n1)
                        {
                                // find the successor node.   (since edges are in both directions, will have to examine
                                // both the target and source node of the edge.  If either is the current node, then the
                                // opposite node is the correct successor node. 

                                n2 = G.source(e1);
                                if ( n2 == n1)
                                {
                                        // edge was pointing from current node, so select the target node as the successor
                                        n2 = G.target(e1);
                                }

                                // check if new node is uninitialized
                                if ( ( pred[n2] == nil ) && ( n2 != startNode ) )
                                {
                                        // record current node as the prev node for the successor
                                        pred[n2] = n1;
                                        
                                        // Assign a cost (dist) to successor node
                                        dist[n2] = dist[n1] + G.inf(e1);  // length of path upto current node point plus cost of using edge to this node
                                                
                                        //Add this node to open priority queue based on its estimated cost for the entire path
                                        openp->insert( n2, Astar_f(n2, dist[n2]) );
                                }
                                else
                                {
                                        // check if path to this node is cheaper than the existing path
                                        double currcost = dist[n2];
                                        double newcost  = dist[n1] + G.inf(e1);

                                        // if new path is cheaper, update the parameters
                                        if ( newcost < currcost )
                                        {
                                                // record current node as the prev node for the successor
                                                pred[n2] = n1;
                                        
                                                // Assign new cost to this node
                                                dist[n2] = newcost;

                                                // Find priority for new node:  cost upto this node + estimated distance to goal. 
                                                double priority = Astar_f(n2, dist[n2]);

                                                // check if this node is in the priority queue
                                                if ( openp->member(n2) )
                                                {
                                                        // already in queue, decrease its priority
                                                        openp->decrease_p( n2, priority );
                                                }
                                                else
                                                {
                                                        // not in queue, add it
                                                        openp->insert( n2, priority );
                                                }
                                        } // end if newcost < currcost
                                } // end if uninitialized
                        } // end forall_inout_edges

                } // end if not goal

                                
        }  // end while

        // No longer need the open queue, delete it.
        delete openp;
        openp = NULL;

        // Fall through the While Loop => no path between start and goal has been found.
        return FAIL;
}

//=============================================================================
// VerifyPath
//
// Purpose: Checks the proposed path to ensure all nodes and edges are collision
//                      free.  Will delete the first node or edge it finds that contains a 
//                      collision and report failure.  If the entire path is collision free,
//                      will report success.
//
//                      Function will exit early if the timer has expired.
//=============================================================================
SuccessResultType PL_PRM::VerifyPath()
{
        // Starting from opposite ends of the path and working towards
        // the centre, check for collisions.

        if ( graphPath.empty() )
        {
                // There is no path to verify
                return FAIL;
        }

        // Check each of the nodes of the path
        node n0 = graphPath.head();
        node n1 = graphPath.tail();

        // Starting from the ends and working towards the centre, check each node for collisions
        while ( TRUE )
        {
                // Check the Timer
                if ( HasTimeLimitExpired() )
                {
                        return TIMER_EXPIRED;
                }

                if (!nodeChecked[n0])
                {
                        // Check for collision
                        if ( IsInterfering( n0 ) )
                        {
                                // Use the midpoint between the last valid node and this node as a seed if the
                                // colliding node was uniformly added.
                                if ( uniformlyAdded[n0] )
                                {
                                        Configuration midpoint = GetMidPoint( G.inf(n0), G.inf( graphPath.cyclic_pred( n0 ) ) );
                                        config_seeds.append( midpoint );
                                }

                                // Node interfers...delete it.
                                G.del_node( n0 );

                                // A node in this path collides.  Therefore path is not valid.
                                return FAIL;
                        }
                        else
                        {
                                // Node has just been checked OK!
                                nodeChecked[n0] = TRUE;
                        }

                }

                // Check if we sitting the middle node, which we just checked.
                if ( n0 == n1 )
                {
                        break;
                }

                if (!nodeChecked[n1])
                {
                        // Check for collision
                        if ( IsInterfering( n1 ) )
                        {

                                // Use the midpoint between the last valid node and this node as a seed is the problem node was uniformly added.
                                if ( uniformlyAdded[n1] )
                                {
                                        Configuration midpoint = GetMidPoint( G.inf(n1), G.inf( graphPath.cyclic_succ( n1 ) ) );
                                        config_seeds.append( midpoint );
                                }

                                // Node interfers...delete it.
                                G.del_node( n1 );

                                // A node in this path collides.  Therefore path is not valid.
                                return FAIL;
                        }
                        else
                        {
                                // Node has just been checked OK!
                                nodeChecked[n1] = TRUE;
                        }
                }

                // Move nodes towards centre
                n0 = graphPath.succ( n0 );

                // Check if we are about to cross the middle
                if ( n0 == n1 )
                {
                        break;
                }

                n1 = graphPath.pred( n1 );

        } // End While loop.

        // IF still in this function, then all nodes of path have been verifed.  Now
        // check the edges.
        // Again, starting from the ends and working towards the centre, check edge for
        // collision.

        // the two nodes marking the edge to check at the front of the path.
        node front0 = graphPath.head();
        node front1 = graphPath.succ( front0 );

        // the two nodes marking the edge to check at the back of the path.
        node back1 = graphPath.tail();
        node back0 = graphPath.pred( back1 );

        while ( TRUE )
        {
                // Check the Timer
                if ( HasTimeLimitExpired() )
                {
                        return TIMER_EXPIRED;
                }

                // Get edge at the front of the path
                edge e0 = FindEdge( front0, front1 );
                if ( e0 == nil )
                {
                        // no edge found joining the two nodes. Report failure.
                        return FAIL;
                }
                
                // Check edge for collision if it has not already been done so.
                if ( !edgeChecked[e0] )
                {
                        // Check for collision
                        if ( IsInterfering( e0 ) )
                        {
                                // Obtain the midpoint along this colliding edge and use it as a seed during enhancing if
                                // the end node was uniformly added and edge midpoints are requested.
                                if ( (useMidPts) && ( uniformlyAdded[front1] ) )
                                { 
                                        Configuration midpt = GetMidPoint( G.inf(front0), G.inf(front1) );
                                        config_seeds.append( midpt );
                                }

                                // Edge has collision...delete it.
                                G.del_edge( e0 );

                                // An edge in this path collides.  Therefore path is not valid.
                                return FAIL;
                        }
                        else
                        {
                                // Edge has just been checked OK!
                                edgeChecked[e0] = TRUE;
                        }
                }

                // Check if we are on the middle edge of the path. (both sets of nodes surrounding same edge)
                if ( front1 == back1 )
                {
                        // on middle edge.  Check complete.
                        break;
                }

                // Get edge at back of the path
                e0 = FindEdge( back0, back1 );
                if ( e0 == nil )
                {
                        // no edge found joining the two nodes. Report failure.
                        return FAIL;
                }
                
                // Check edge for collision if it has not already been done so.
                if ( !edgeChecked[e0] )
                {
                        // Check for collision
                        if ( IsInterfering( e0 ) )
                        {
                                // Obtain the midpoint along this colliding edge and use it as a seed during enhancing if
                                // the end node was uniformly added and edge midpoints are requested
                                if ( (useMidPts) && ( uniformlyAdded[back0] ) )
                                {
                                        Configuration midpt = GetMidPoint( G.inf(back1), G.inf(back0) );
                                        config_seeds.append( midpt );
                                }

                                // Edge has collision...delete it.
                                G.del_edge( e0 );

                                // An edge in this path collides.  Therefore path is not valid.
                                return FAIL;
                        }
                        else
                        {
                                // Edge has just been checked OK!
                                edgeChecked[e0] = TRUE;
                        }
                }

                // Move the nodes
                front0 = front1;
                front1 = graphPath.succ( front1 );

                // Check if we are on an edge that was just checked by the back set, indicating
                // we are in the middle of the path.
                if ( front1 == back1 )
                {
                        // Middle edge, check complete.
                        break;
                }

                back1 = back0;
                back0 = graphPath.pred( back0 );
        
        }  // End While

        // If we are still in the function, then path has been completely verified.  
        // Report success.
        return PASS;
}


//=============================================================================
// EnhanceRoadmap
//
// Purpose: Determine the troublesome areas of the roadmap (graph) and add more
//                      nodes and edges in these areas.  The next time the findpath routine is
//                      called, hopefully the new nodes and edges generated will allow for a
//                      successful path.
//
//                      Function will exit early if the timer has expired.
//=============================================================================
SuccessResultType PL_PRM::EnhanceRoadMap()
{
        // Add the requested number of enhanced nodes to roadmap.

        // Enhancing is done in two steps:  nodes added using seeds based on previosly deleted nodes and edges
        //                                                                      then additional nodes distributed uniformly across C-space.

        int uniformQuant = enhanceQuant;

        if ( !config_seeds.empty() )
        {
                // Half the new nodes will be based on seeds.  The remainder will be uniformly distributed.
                int seedQuant = double(enhanceQuant * seedRatio);
                uniformQuant = enhanceQuant - seedQuant;

                // Want to distribute an even number of enhance nodes for each seed, so continue to cyclic search
                // through the list until enhance quant has been met.  Then clear the seeds so we do not use them again.
                list_item seedindex = config_seeds.first();
                //list_item firstindex = seedindex;
                //bool gone_through_all = FALSE;

                double std_dev = 0.5 * radiusTol * sqrt( diagonal_squared );

                for ( int i = 0; i < seedQuant; i++ )
                {
                        // Get a configuration normally distributed about the seed.
                        Configuration newConfig;
                        
                        bool validConfig = FALSE;
                        while (!validConfig)
                        {
                                newConfig = GenerateRandomConfig( config_seeds.inf( seedindex ), std_dev );

                                // NOTE:  order of argurments in if statment is vital; interference checking will only be done if validNodesOnly.
                                if ( (validNodesOnly) && (IsInterfering( newConfig )) ) 
                                                                
                                {
                                        validConfig = FALSE;
                                }
                                else
                                {
                                        validConfig = TRUE;
                                }
                        }

                        node newNode = G.new_node( newConfig );

                        ASSERT ( newNode != nil );

                        // Node has been checked for collisions if validNodesOnly.
                        nodeChecked[newNode] = validNodesOnly;

                        // Node was added as part of a seed and not uniformly
                        uniformlyAdded[newNode] = FALSE;

                        // Connect the new node
                        ConnectNode( newNode );

                        // Check if Time has expired
                        if ( HasTimeLimitExpired() )
                        {
                                return TIMER_EXPIRED;
                        }

                        // Advance to next seed
                        seedindex = config_seeds.cyclic_succ( seedindex );
                        /*if ( seedindex == firstindex )
                        {
                                // gone through all the seeds at least once.
                                gone_through_all = TRUE;
                        }*/

                }       // end for loop.

                // Finished adding nodes about these seeds.  Clear list for next time.
                config_seeds.clear();           // start with a fresh seed list.
                /*if ( gone_through_all )
                {
                        // all seeds have been used.  Clear list
                        config_seeds.clear();
                }
                else
                {
                        // save any seeds that weren't used by removing those that were.
                        while ( firstindex != seedindex )
                        {
                                config_seeds.erase( firstindex );
                                firstindex = config_seeds.first();
                        }

                } */

        }  // end if-then


        // Add the remaining number of enhanceQuant nodes uniformly
        for (int i = 0; i < uniformQuant; i++ )
        {
                // Add the nodes
                AddNode();

                // Check if Time has expired
                if ( HasTimeLimitExpired() )
                {
                        return TIMER_EXPIRED;
                }

        }  // end For Loop

                        
        // Return Success.
        return PASS;
}


//=============================================================================
// Astar_f
//
// Purpose: Estimates the cost of the path based on the given node.
//                      Returns the cost of the path upto the current node (given as a parameter)
//                      and estimates the cost from current node to the goal.
//=============================================================================
double PL_PRM::Astar_f( const node& n, const double& currcost ) const
{
        return currcost + sqrt(Distance( n, goalNode ));  
}

//=============================================================================
// CopySettings
//
// Purpose: copies the settings from a different planner
//=============================================================================
void PL_PRM::CopySettings( PlannerBase* original )
{
        PL_GraphBase::CopySettings( original );
        PL_PRM* originalPrm = dynamic_cast< PL_PRM* >( original );
        if( originalPrm == NULL )
        {
                return;
        }
        this->validEdgesOnly = originalPrm->validEdgesOnly;
        this->validNodesOnly = originalPrm->validNodesOnly;
        this->seedRatio = originalPrm->seedRatio;
        this->radiusTol = originalPrm->radiusTol;
        this->useMidPts = originalPrm->useMidPts;
        this->initQuant = originalPrm->initQuant;
        this->enhanceQuant = originalPrm->enhanceQuant;
        this->lazyMode = originalPrm->lazyMode;

        // Set the graph mode based on lazyMode.
        SetGraphMode( !lazyMode );
}


void *PL_PRM::GetParameters()
{
        PL_GraphBase::GetParameters();
        //if (paramBuffer)
        //      return paramBuffer;

        char szLine[255];

        //Test size first;
        int bufferLen = 0;
        sprintf(szLine, "Validate Edges Only: %d\n", validEdgesOnly);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Validate Nodes Only: %d\n", validNodesOnly);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Use Middle Points: %d\n", useMidPts);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Use Lazy Mode: %d\n", lazyMode);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Initial Quantity: %d\n", initQuant);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Enhance Quantity: %d\n", enhanceQuant);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Seed Ratio: %f\n", seedRatio);
        bufferLen += strlen(szLine);
        sprintf(szLine, "Radius Tolerance: %f\n", radiusTol);
        bufferLen += strlen(szLine);

        //In case the buffer is too small, reallocate one.
        bufferLen += 100;
        if (paramBuffer != NULL)
                bufferLen += strlen(paramBuffer);
        if (bufferLen > sizeOfParameterBuffer)
        {
                char *pNewBuffer = (char *)malloc(bufferLen);
                if (pNewBuffer)
                {
                        memset(pNewBuffer, 0, bufferLen);
                        if (paramBuffer)
                        {
                                strcpy(pNewBuffer, paramBuffer);
                                delete paramBuffer;
                        }
                        paramBuffer = pNewBuffer;
                        sizeOfParameterBuffer = bufferLen;
                }
                else
                {
                        return paramBuffer;
                }
        }

        //This time really put them into buffer
        sprintf(szLine, "Validate Edges Only: %d\n", validEdgesOnly);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Validate Nodes Only: %d\n", validNodesOnly);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Use Middle Points: %d\n", useMidPts);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Use Lazy Mode: %d\n", lazyMode);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Initial Quantity: %d\n", initQuant);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Enhance Quantity: %d\n", enhanceQuant);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Seed Ratio: %f\n", seedRatio);
        strcat(paramBuffer, szLine);
        sprintf(szLine, "Radius Tolerance: %f\n", radiusTol);
        strcat(paramBuffer, szLine);

        return paramBuffer;
}

bool PL_PRM::SetParameters(const void *param)
{
        if (PL_GraphBase::SetParameters(param)==false)
                return false;

        istrstream paramStream(paramBuffer);
        char szLine[255];
        while (!paramStream.eof())
        {
                paramStream.getline(szLine, 255);
                _strlwr(szLine);
                char *pValue = strchr(szLine, ':');
                if (pValue == NULL)
                {
                        pValue = szLine;
                        //Trimleft of szLine
                        while(*pValue==' ')
                                pValue++;
                        //IF szLine is non-empty, there is some mistake, 
                        //ELSE it just end of the stream.
                        if (strlen(pValue) != 0)
                                return false;
                        else
                                break;
                }
                else
                {
                        pValue += 1;
                }

                if (strstr(szLine, "validate edges only"))
                {
                        validEdgesOnly = atoi(pValue);
                }
                else if (strstr(szLine, "validate nodes only"))
                {
                        validNodesOnly = atoi(pValue);
                }
                else if (strstr(szLine, "use middle points"))
                {
                        useMidPts = atoi(pValue);
                }
                else if (strstr(szLine, "use lazy mode"))
                {
                        lazyMode = atoi(pValue);
                }
                else if (strstr(szLine, "initial quantity"))
                {
                        initQuant = atoi(pValue);
                }
                else if (strstr(szLine, "enhance quantity"))
                {
                        enhanceQuant = atoi(pValue);
                }
                else if (strstr(szLine, "seed ratio"))
                {
                        seedRatio = atof(pValue);
                }
                else if (strstr(szLine, "radius tolerance"))
                {
                        radiusTol = atof(pValue);
                }
        }
        return true;
}

bool PL_PRM::ValidateParameters()
{
        return true;
}

---