planners/sandros/PL_Sandros.cpp

Go to the documentation of this file.

//Sandros Planner Implemetation,
//Start at April 20
//By Huang Yifeng 

#include "synchronization/semaphore.h"
#include <assert.h>
#include "PL_Sandros.h"

#ifndef NOGL
#include "opengl/glos.h"
#include <gl/gl.h>
#endif

#include <LEDA/node_map.h> 
#include <LEDA/node_pq.h>
#include <LEDA/node_set.h>
#include <LEDA/templates/shortest_path.t> 
#include <LEDA/list.h>
#include <math.h>

#include "math/math2.h"

#define LEDA_CHECKING_OFF

//yifeng has made some changes.

#define for_allnodes_inV(v,G)   forall_nodes(v, G)\
        if(m_nodeUVPsPt[v]==NodeInV)

#define for_allnodes_inPs(v,G)  forall_nodes(v, G)\
        if(m_nodeUVPsPt[v]==NodeInPs)

#define for_allnodes_inPsNoIsland(v,G)  forall_nodes(v, G)\
        if(m_nodeUVPsPt[v]==NodeInPs&&m_nodeInPsIsland[v]==false)


#define for_allnodes_inPt(v,G)  forall_nodes(v, G)\
        if(m_nodeUVPsPt[v]==NodeInPt)

#define forall_nodesSan(v,G)    forall_nodes(v, G)\
        if(m_nodeUVPsPt[v]==NodeInPt||m_nodeUVPsPt[v]==NodeInPs||m_nodeUVPsPt[v]==NodeInV)

// Class PL_Sandros 

//-----------------------------------------------------------------------------
// Destructor
//
// comment - Deletes any temorary data
//-----------------------------------------------------------------------------
PL_Sandros::~PL_Sandros()
{
        //this does nothing right now
}

int PL_Sandros::GetRefinedNode (int i)
{
        int k;
        if(NodeRefined!=nil&&i<dof)
        {
                k=NodeRefinedc[i];
        }
        else
        {
                k=-1;
        }

        return k;
}
//-----------------------------------------------------------------------------
// Plan
//
// comment - Called when planning is to be performed
//-----------------------------------------------------------------------------
bool PL_Sandros::Plan ()
{

        //## begin PL_Sandros::Plan%929728915.body preserve=yes
        //some early check
        plan_success=FAIL;

        dof=collisionDetector->DOF(); //local copy of the Degree Of Freedom

        // Perform early checks
        if ( startNode == goalNode )
        {
                graphPath.clear();
                TranslatePath();
                plan_success = PASS;
        }
        if ( plan_success == PASS )
        {
                return true;
        }
        
        //Initialize Sandros's Graph:
        //Ps={s}, Pt={t},U={},V={root}
        
        InitailizeGraph();
        
        PlannerBase::StartTimer();

        Semaphore semaphore( guid );
        semaphore.Lock();


        //THIS IS THE MASTER WHILE LOOP
        bool planningDone = false;
        //Do some pretest here

        
        node_list L;            //node list used to store the sequence aviliable
        m_bResolutionLimit=false;

        while( !planningDone )
        {
                //PLACE ALL PLANNING CALLS IN HERE
                //every main loop planner will search for all the sequence avilialbe 
                //for a free path, if no free path find then will do the node refinment
                //untill path find or failure report.
                m_bNoOtherSequence=false;
                do
                {
                        if(!L.empty())L.clear();        //clear L for a new sequence

                        if(SearchForSequence(L))
                        {
                                DealwithSequence(L);    //a sequence is find then use local planner judge 
                        }                                                       //whether a node is reachable
                        else
                        {
                                if(m_bResolutionLimit)  //now resolution has reach to the upbound limit and no other 
                                {                                               //sequence can be generated, report planner's fail
                                        plan_success = FAIL;
                                        return false;
                                }
                                m_bNoOtherSequence=true;
                        }
                        if(plan_success==PASS)
                        {
                                //when you actually finish planning, set this to true
                                planningDone = true;
                                SaveSandrosPath(L);
                        }
                }while(!m_bNoOtherSequence&&planningDone!=true);

                if(!planningDone)
                        ConstructGraph();       //construct the Sandros graph for search a path from Ps to Pt
                                                                //and split the node in Q into subnodes
                if(m_bResolutionLimit)  //now resolution has reach to the upbound limit and no other 
                {                                               //sequence can be generated, report planner's fail
                        plan_success = FAIL;
                        return false;
                }

                //Every once in a while, you should release the semaphores, and check to 
                //see if the time limit has expired.  If you don't release the semaphore,
                //the drawing thread will never draw anything
                semaphore.Unlock();
                //check to see if the time limit has expired
//              if( HasTimeLimitExpired() == true )
//              {
//                      return false ;
//              }
                semaphore.Lock();
        }
        //if we have in fact found a path, return a true value, otherwise, return false
        return true;
  //## end PL_Sandros::Plan%929728915.body
}

//when path found then translate the path into the mpk 'path' virente
void PL_Sandros::SaveSandrosPath(const node_list& L)
{
        path.Clear();
        if(!L.empty())
        {
                node n1=L.head();
                node n=n1;
                while(n!=startNode)
                {
                        path.AppendPointToBeginning (m_nodeInUPs[n]);
                        n=m_nodePs[n];
                }
                path.AppendPointToBeginning(G.inf(startNode));
                do
                {
                        Configuration c2=m_nodeInUPs[n1];
                        n1=L.succ(n1);          //find first node in V in the node_list
                        Configuration c1;
/*                      if(IsInterferingSan(c2,n1,c1))          //here use a linear path use as local planner
                        {

                                path.AppendPoint (G.inf(goalNode));
                                return;         
                        }
*/
                        path.AppendPoint(c2);
                        if(!IsInterfering(c2,G.inf(goalNode)))
                        {
                                path.AppendPoint(c2);
                                path.AppendPoint (G.inf(goalNode));
                                return;
                        }
                }while(m_nodeUVPsPt[n1]!=NodeInPt);
                path.AppendPoint (G.inf(goalNode));
        }//end if(!L.empty())
        return;
}

node PL_Sandros::GenerateNewNodeGrid(int res,const node n1,int no)
{
        node returnme=nil;
        if(no>=nextNo)
        {
        Configuration config_return;
        config_return.SetLength(dof);

        double max = collisionDetector->JointMax( res ) ;
        double min = collisionDetector->JointMin( res ) ;

        double jointvalue=min+(max-min)/seedofa*(no-1);

        config_return=G.inf(n1);
        config_return[ res ] = jointvalue;

        for(int i=res+2;i<=dof;i++)
        {
                collisionDetector->DeactivateFrames(0,i) ;
        }

        int m=0;
        while(IsInterfering(config_return))
        {
                m++;
                config_return[res]=config_return[res]+stride;
        }
        for(i=res+2;i<=dof;i++)
        {
                collisionDetector->ActivateFrames(0,i) ;
        }
        if(config_return[res]==-1)
        {
                config_return[res]=-2;
        }

        returnme=G.new_node(config_return);
        m_refine_level[returnme]=res+1;
        int temp=Tolerance/stride;
        nextNo=no+m+temp+1;
        }
        return returnme;
}

node PL_Sandros::GenerateNewNode(int res,const node n1)
{
        Configuration config_return;
        config_return.SetLength(dof);

        node returnme;
        double max = collisionDetector->JointMax( res ) ;
        double min = collisionDetector->JointMin( res ) ;
        double random = 2*( double( rand() ) / RAND_MAX ) - 1; //this is a random number between 
        
        double jointvalue=(max-min)*random;
        if ( jointvalue < min )
        {
                jointvalue += (max - min);
        }
        else if ( jointvalue > max )
        {
                jointvalue -= (max - min);
        }

        config_return=G.inf(n1);
        config_return[ res ] = jointvalue;
        returnme=G.new_node(config_return);

        return returnme;
}

void PL_Sandros::SetValue(int m_Grid,int m_Neighbor,int m_Tolerance)
{
        stride=m_Grid;
        ThreshHold=m_Neighbor;
        Tolerance=m_Tolerance;
}
void PL_Sandros::InitailizeGraph()
{
        Configuration config_root;
        config_root.SetLength(dof);

        for(int i=0;i<dof;i++)
        {
                config_root[i]=-1;                      //-1: mean the dof not yet specified
        }
        node root=G.new_node(config_root);
        
        m_nodePs.init(G,nil);
        m_nodePt.init(G,nil);
        m_nodeUVPsPt.init(G);
        m_nodeInUPs.init(G);
        m_refine_level.init(G);
        m_nodeInPsIsland.init(G,false);
        m_bEdgeHiden.init(G,false);
        
        m_nodeUVPsPt[startNode]=NodeInPs;
//      m_nodeUVPsPt[goalNode]=NodeInPt;
        m_nodeUVPsPt[root]=NodeInV;
        m_nodeInUPs[startNode]=G.inf(startNode);
        m_refine_level[startNode]=dof;
        m_refine_level[goalNode]=dof;
        m_refine_level[root]=0;
        

        m_quenQ=new node_pq<int>(G);    // pointer to open priority queue set to be empty
        m_quenQ1=new queue<node>;       // pointer to open queue set to be empty

        double dis=DistanceSandros(startNode,root);     //connect the node with edges
        edge e1=G.new_edge(root,startNode,dis);         //startNode to root
        edge e3=G.new_edge(startNode,root,dis);         //startNode to root
        dis=DistanceSandros(root,goalNode);
        edge e2=G.new_edge(root,goalNode,dis);          //root to goalNode
        edge e4=G.new_edge(goalNode,root,dis);          //startNode to root


//      m_quenQ->insert(root,current_res);
        m_c.init(G);
        m_c[e1]=G.inf(e1);
        m_c[e2]=G.inf(e2);
        m_c[e3]=G.inf(e3);
        m_c[e4]=G.inf(e4);

//      G.make_undirected();
}

bool PL_Sandros::Save (const char *filename) const
{
  //## begin PL_Sandros::Save%958948890.body preserve=yes
        char filenameWithExtension[ 256 ] = "" ;
        strcpy( filenameWithExtension, filename ) ;
        strcat( filenameWithExtension, ".SAN" ) ;                       //IMPROVE: store the extension elsewhere

        //open a binary file
        FILE* outfile = fopen( filenameWithExtension, "wb" ) ;
        assert( outfile != NULL ) ;

        //output the file header
        fwrite( "PSAN", 1, 4, outfile ) ;

        //OUTPUT ALL THE PLANNER SPECIFIC DATA IN HERE

        //close the binary file 
        fclose( outfile ) ;

        return false ;
  //## end PL_Sandros::Save%958948890.body
}

bool PL_Sandros::Load (const char *filename)
{
  //## begin PL_Sandros::Load%958953407.body preserve=yes
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        char filenameWithExtension[ 256 ] = "" ;
        strcpy( filenameWithExtension, filename ) ;
        strcat( filenameWithExtension, ".SAN" ) ;

        //open a binary file
        FILE* infile = fopen( filenameWithExtension, "rb" ) ;
        if( infile == NULL )
        {
                semaphore.Unlock();
                return false ;
        }

        //input the file header
        char header[ 5 ] = "" ;
        fread( header, 4, 1, infile ) ;
        header[ 4 ] = NULL ;
        assert( strcmp( header, "PSAN" ) == 0 ) ;

        //INPUT ALL THE SANDROS SPECIFIC DATA HERE

        //close the binary file 
        fclose( infile ) ;

        semaphore.Unlock();
        return true ;
  //## end PL_Sandros::Load%958953407.body
}

PL_Sandros::PL_Sandros()
{
        // Clear the graph and initialize the node maps and open list
        ClearGraph();
        NodeRefined=nil;
        nextNo=1;
        plan_success=FAIL;
        seedofa=0;
        stride=5;
        current_res=0;
        m_bResolutionLimit=false;
        ThreshHold=16;
        Tolerance=9;
        TRACE("PL_Sandros Default Constructor completed.\n");
}

void PL_Sandros::SetStartConfig( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( startNode != nil ) && ( GetStartConfig() == config ))
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // A new configuration has been requested.  Need to clear the current graph and search parameters
        ClearGraph();

        // 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 );

        // Make sure the start node is valid
        if ( IsInterfering( GetStartConfig() ) )
        {
                // Remove invalid start from graph.
                G.del_node( startNode );
                startNode = nil;

                // Start Interfers, there will be no valid path possible.
                plan_success = FAIL;
                return;
        }

        // Assume the current goal config is correct, but still need to add it to new graph 
        // and assign the goal node.
        PL_GraphBase::SetGoalConfig( GetGoalConfig() );

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

        if ( IsInterfering( GetGoalConfig() ) )
        {
                // Remove the invalid goal from graph
                G.del_node( goalNode );
                goalNode = nil;

                // Goal Interfers, there will be no valid path possible.
                plan_success = FAIL;
                return;
        }
}
void PL_Sandros::SetGoalConfig ( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( goalNode != nil ) && ( GetGoalConfig() == config ))
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // calling the parent's SetGoalConfig.
        PL_GraphBase::SetGoalConfig( config );


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

        // Ensure the new goal is valid
        if ( IsInterfering( GetGoalConfig() ) )
        {
                G.del_node( goalNode );
                goalNode = nil;

                // Goal interferes.  There is no point in performing a search.  Exit early
                plan_success = FAIL;
                return;
        }
}

void PL_Sandros::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        PL_GraphBase::SetCollisionDetector( collisionDetector );
}

edge PL_Sandros::FindEdgeBetweenTwoNodes(const node& n1,const node& n2)
{
        edge e1;
        forall_adj_edges(e1, n1) 
        {
                node n3=G.target(e1);
                if(n3==n2)
                        return e1;
        }
        return nil;
}

void PL_Sandros::DealwithSequence(const node_list& L)
{
        if(!L.empty())
        {
                node n1=L.head();
                do
                {
                        Configuration c2=m_nodeInUPs[n1];
                        node nn=n1;
                        n1=L.succ(n1);          //find first node in V in the node_list
                        Configuration c1;
                        if(n1!=nil)
                        {
                                if(IsInterferingSan(c2,n1,c1))          //here use a linear path use as local planner
                                {
                                        node n2=L.pred(n1);
                                        edge e1=FindEdgeBetweenTwoNodes(n1,n2);         //cut the edge
                                        edge e2=FindEdgeBetweenTwoNodes(n2,n1);         //cut the edge
                                        G.hide_edge(e1);
                                        m_bEdgeHiden[e1]=true;
                                        G.hide_edge(e2);
                                        m_bEdgeHiden[e2]=true;
                                        int nnnnn=G.number_of_edges();
                                        if(!m_quenQ->member(n1))
                                        {
                                                m_quenQ->insert(n1,m_refine_level[n1]);
                                                m_quenQ1->append(n1);
                                        }
                                        if(!m_quenQ->member(nn))
                                        {
                                                m_quenQ->insert(nn,m_refine_level[nn]);
                                                m_quenQ1->append(nn);
                                        }
                                        return;         
                                }
                                else
                                {
                                        m_nodeUVPsPt[n1]=NodeInPs;                      //find a node in v reachable from Ps
                                        m_nodeInUPs[n1]=c1;
                                        m_nodePs[n1]=L.pred(n1);
                                        node n2=L.pred(n1);
                                        edge e1=FindEdgeBetweenTwoNodes(n1,n2);
                                        G.del_edge(e1);
                                        edge e2=FindEdgeBetweenTwoNodes(n2,n1);
                                        G.del_edge(e2);
                                        if(!IsInterfering(c1,G.inf(goalNode)))
                                        {
                                                plan_success=PASS;
                                                return;
                                        }
                                }
                        }
//                      n4=L.succ(n1);
                }while(n1!=nil);//m_nodeUVPsPt[n4]!=NodeInPt
        }//end if(!L.empty())
        return;
}

void PL_Sandros::ClearGraph()
{
        PL_GraphBase::ClearGraph();
}


bool PL_Sandros::DrawExplicit () const
{
  //## begin PL_Sandros::DrawExplicit%964739313.body preserve=yes
#ifndef NOGL
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        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 );

        //INSERT ALL YOUR DRAWING CODE HERE
        glBegin( GL_POLYGON );
                glColor4f( 0.0f, 0.0f, 0.0f, 1.0f );    //black
                glVertex3f( 0.0f, 0.0f, 0.0f );
                glColor4f( 1.0f, 0.0f, 0.0f, 1.0f );    //red
                glVertex3f( 0.0f, 5.0f, 0.0f );
                glColor4f( 0.0f, 1.0f, 0.0f, 1.0f );    //green
                glVertex3f( 5.0f, 5.0f, 0.0f );
                glColor4f( 0.0f, 0.0f, 1.0f, 1.0f );    //blue
                glVertex3f( 5.0f, 0.0f, 0.0f );
        glEnd();

        if( lightingState != 0x00 )
        {
                glEnable( GL_LIGHTING );
        }
        semaphore.Unlock();
#endif
        return true;
  //## end PL_Sandros::DrawExplicit%964739313.body
}

node PL_Sandros::FindNextNodeToRefined()
{
        node n1=m_quenQ->find_min();
        int res=m_quenQ->prio(n1);
        node n2;
        bool findmin=false;
        queue<node> Q;
        n2=nil;
        do{
                n2=m_quenQ1->pop();
                if(m_quenQ->prio(n2)==res)
                        findmin=true;
                else
                        Q.append(n2);
        }while(!findmin);
        while(!m_quenQ1->empty())
        {
                Q.append(m_quenQ1->pop());
        }
        while(!Q.empty())
        {
                m_quenQ1->append(Q.pop());
        }


/*      do
        {
                n2=m_quenQ1->pop();
                if(m_quenQ->prio(n2)<dof)
                        findmin=true;
        }while(!findmin&&!m_quenQ1->empty ());
*/
        return n2;
}

void PL_Sandros::ConstructGraph()
{
        if(!m_quenQ->empty())
        { 
                G.restore_all_edges();
                node n1=FindNextNodeToRefined();
                int res;
                if(n1!=nil)
                {
                        res=m_quenQ->prio(n1);
                        current_res=res;
                }
                if(res==(dof)||n1==nil)
                {
                        m_bResolutionLimit=true;
                        return;
                }
                int recordstride,recordtolerance,recordthreshhold;
                if(dof>=4&&res==0)
                {
                        recordstride=stride;
                        recordtolerance=Tolerance;
                        recordthreshhold=ThreshHold;
                        stride=7;
                        Tolerance=13;
                        ThreshHold=25;
                }
                NodeRefined=n1;
                NodeRefinedc=G.inf(n1);
                edge ee;
                forall_edges(ee,G)
                {
                        if(m_bEdgeHiden[ee])
                        {
                                if((n1==G.source(ee))||(n1==G.target(ee)))
                                        m_bEdgeHiden[ee]=false;
                                else
                                        G.hide_edge(ee);
                        }
                }

                node n6;
                if(m_nodeUVPsPt[n1]==NodeInPs)  //if the node to be refined is reachable 
                                                                                //from start point
                {
                        Configuration cres,cres1;
                        cres=m_nodeInUPs[n1];
                        double jointvalue=cres[res];
                        cres1=G.inf(n1);
                        cres1[res]=jointvalue;
                        node newn=G.new_node(cres1);
                        m_nodeUVPsPt[newn]=NodeInPs;
                        m_nodeInPsIsland[newn]=true;
                        node v;
                        for_allnodes_inPs(v,G)
                        {
                                if(m_nodePs[v]==n1)
                                {
                                        m_nodePs[v]=newn;
                                }
                        }
                        m_nodePs[newn]=m_nodePs[n1];
                        m_nodeInUPs[newn]=m_nodeInUPs[n1];
                        m_refine_level[newn]=dof;
                        node x=m_nodePs[newn];

                        if(!m_quenQ->member(x))
                        {
                                m_quenQ->insert(x,m_refine_level[x]);
                                m_quenQ1->append(x);
                        }
                        n6=newn;
                }
                
                node n2;
                bool firstnode=true;
                double max = collisionDetector->JointMax( res ) ;
                double min = collisionDetector->JointMin( res ) ;
                seedofa=(max-min)/stride+2;
                node prev;
                nextNo=1;
                for(int i=1;i<=seedofa;i++)
                {
                        n2=GenerateNewNodeGrid(res,n1,i);
                        if(n2!=nil)
                        {
                                if(!firstnode)
                                {
                                        if(IsNeighber(prev,n2))
                                        {

                                                double dis=DistanceSandros(prev,n2);
                                                edge e1=G.new_edge(n2,prev,dis);
                                                e1=G.new_edge(prev,n2,dis);
                                        }
                                }
                                m_nodeUVPsPt[n2]=NodeInV;
                                node v1;
                                forall_adj_nodes(v1, n1) 
                                {
                                        if(IsNeighber(v1,n2))
                                        {
                                                double dis=DistanceSandros(v1,n2);
                                                edge e1=G.new_edge(n2,v1,dis);
                                                edge e2=G.new_edge(v1,n2,dis);
                                                if(m_nodeUVPsPt[v1]==NodeInPs)
                                                {
                                                        Configuration temp=m_nodeInUPs[v1];
                                                        Configuration cdd;
                                                        if(!IsNeighberSan(temp,n2))             
                                                        {
                                                                G.hide_edge(e1);
                                                                G.hide_edge(e2);
                                                        }
/*                                                      else if(IsInterferingSan(temp,n2,cdd))
                                                        {
                                                                G.hide_edge(e1);
                                                                G.hide_edge(e2);
                                                        }
*/
                                                }

                                        }
                                }
                                node n5;
                                if(m_nodeUVPsPt[n1]==NodeInPs)  //if the node to be refined is reachable 
                                {
                                        for_allnodes_inPs(n5,G)
                                        {
                                                if(m_nodePs[n5]==n6)
                                                {
                                                        if(IsNeighber(n5,n2))
                                                        {
                                                                double dis=DistanceSandros(n5,n2);
                                                                edge e1=G.new_edge(n2,n5,dis);
                                                                edge e2=G.new_edge(n5,n2,dis);
                                                                Configuration temp=m_nodeInUPs[n5];
                                                                Configuration cdd;
                                                                if(!IsNeighberSan(temp,n2))             
                                                                {
                                                                        G.hide_edge(e1);
                                                                        G.hide_edge(e2);
                                                                }
/*                                                              else if(IsInterferingSan(temp,n2,cdd))
                                                                {
                                                                        G.hide_edge(e1);
                                                                        G.hide_edge(e2);
                                                                }
*/
                                                        }
                                                }
                                        }
                                        node n7=m_nodePs[n6];
                                        if(IsNeighber(n7,n2))
                                        {
                                                double dis=DistanceSandros(n7,n2);
                                                edge e1=G.new_edge(n7,n2,dis);
                                                edge e2=G.new_edge(n2,n7,dis);
                                                Configuration temp=m_nodeInUPs[n7];
                                                Configuration cdd;
                                                if(!IsNeighberSan(temp,n2))             
                                                {
                                                        G.hide_edge(e1);
                                                        G.hide_edge(e2);
                                                }
/*                                              else if(IsInterferingSan(temp,n2,cdd))
                                                {
                                                        G.hide_edge(e1);
                                                        G.hide_edge(e2);
                                                }
*/
                                        }

                                }
                                prev=n2;
                                firstnode=false;
                        }
                }
                m_quenQ->del(n1);
                G.del_node(n1);
                if(dof>=4&&res==0)
                {
                        stride=recordstride;
                        Tolerance=recordtolerance;
                        ThreshHold=recordthreshhold;
                }
        }//end if(!m_quenQ.empty())
        else
        {
                m_bResolutionLimit=true;
                return;
        }
        
        int m=G.number_of_nodes();
        int n=G.number_of_edges();

        m_c.init(G);
        edge e2;
        forall_edges(e2,G)
        {
                m_c[e2]=G.inf(e2);
        }

        return;
}

bool PL_Sandros::IsNeighber(const node& n1,const node& n2)
{
        Configuration c1,c2;
        c1=G.inf(n1);
        c2=G.inf(n2);
        double dis=0;
//      if(m_nodeUVPsPt[n1]==NodeInPs)
//      c1=m_nodeInUPs[n1];
        for(int i=0;i<dof;i++)
        {
                double d1=c1[i];
                double d2=c2[i];
                if(c1[i]!=-1&&c2[i]!=-1)
                {
                        dis+=abs(c1[i]-c2[i]);
//                      if(abs(c1[i]-c2[i])>=ThreshHold)
//                              return false;
                }
        }
//      if(dis>=Tolerance&&dis<=ThreshHold)
        if(dis<=ThreshHold)
                return true;
        else 
                return false;
}

bool PL_Sandros::IsNeighberSan(const Configuration& c1,const node& n2)
{
        Configuration c2;
        c2=G.inf(n2);
        double dis=0;
//      if(m_nodeUVPsPt[n1]==NodeInPs)
//      c1=m_nodeInUPs[n1];
        for(int i=0;i<dof;i++)
        {
                double d1=c1[i];
                double d2=c2[i];
                if(c1[i]!=-1&&c2[i]!=-1)
                {
                        dis+=abs(c1[i]-c2[i]);
//                      if(abs(c1[i]-c2[i])>=ThreshHold)
//                              return false;
                }
        }
//      if(dis>=Tolerance&&dis<=ThreshHold)
        if(dis<=ThreshHold)
                return true;
        else 
                return false;
}


bool PL_Sandros::IsInterferingSan(const Configuration& c1,const node& n2,Configuration& p)
{
        Configuration c2;
        c2=G.inf(n2);
        for(int i=0;i<dof;i++)
        {
                if(c2[i]==-1)
                        c2[i]=c1[i];
        }
        p=c2;
        return IsInterfering(c1,c2);
}

//defination of the edge longth between the two nodes
double PL_Sandros::DistanceSandros(const node& n1,const node& n2)
{
        Configuration c1,c2;
        c1=G.inf(n1);
        c2=G.inf(n2);
        double returndis=0;
        for(int i=0;i<dof;i++)
        {
                if(c1[i]==-1)
                {
                        c1[i]=c2[i];
                }
                if(c2[i]==-1)
                {
                        c2[i]=c1[i];
                }
                returndis+=abs(c1[i]-c2[i]);
        }
        if(returndis<1)
                returndis=1;
        return returndis;
}

//find a sequence from graph of sandros 
//and return the node_list and the result
bool PL_Sandros::SearchForSequence(node_list& L)
{
        node n1;
        double shortest,test;
        shortest=0;
        test=0;
        node_array<edge>        pred;
        pred.init(G);

        node shortest_start,shortest_goal;

        bool b_pathfound=false;

        for_allnodes_inPsNoIsland(n1,G)
        {
                if(!b_pathfound)
                {
                        test=DIJKSTRA_T(G,n1,goalNode,m_c,pred);        //find shortest path from n1 to n2
                        if(pred[goalNode]!=nil)                                 //path found
                        {
                                if(shortest==0||shortest>=test)
                                {
                                        shortest=test;
                                        shortest_start=n1;
                                        shortest_goal=goalNode;
                                }       
                                b_pathfound=true;
                                goto ttt;
                        }// end if(pred[n2]!=nil)
                }
        }//endif for_allnodes_inPs

ttt:
        if(b_pathfound==false)  return false;
        else{                                                   //store the sequence for future use
                node next_node;
                node previous_node=shortest_goal;
                L.push(shortest_goal);
                test=DIJKSTRA_T(G,shortest_start,shortest_goal,m_c,pred);
                edge e1=pred[shortest_goal];
                do                                      //extract all the nodes along the shortest path
                {
                        next_node=G.source(e1);
                        if(next_node==previous_node)
                                next_node=G.target(e1);
                        e1=pred[next_node];
                        L.push(next_node);
                        Configuration cx=G.inf(next_node);
                        for(int kk=0;kk<dof;kk++)
                        {
                                int hh=cx[kk];
                        }
                }while(pred[next_node]!=nil&&m_nodeUVPsPt[next_node]!=NodeInPs);
                return true;
        }//end else
}

---