planners/sequential/PL_Sequential.cpp

Go to the documentation of this file.

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

// PL_Sequential
#include "PL_Sequential.h"
#include "sp3dstct.h"
#include "spfiles.h"
#include "gdefs.h"
#include "sp3dgl.h"
#include <stdio.h>
#include <stdlib.h>
#include "opengl/glos.h"
#include <gl/gl.h>
#include "smoothers\SmootherBase.h"

#pragma warning( disable : 4786 )

const int UNDONE = -1 ;
//## end module%3752D78501D0.additionalDeclarations


// Class PL_Sequential 

PL_Sequential::~PL_Sequential()
{
  //## begin PL_Sequential::~PL_Sequential%.body preserve=yes
        free( orig_initial_config ) ;   
        free( orig_final_config ) ;     

        int i;
        for( i = 0; i < arm_degree; i++ )
        {
                free( lnkcps[ i ].d );
                lnkcps->d = NULL;
        }
        free( lnkcps );
        free2D( ( void** )globalpath );
        free( tqcs.tqlink );
        free( bkwdpath.d );
        free( frwdpath.d );
        free( combpath.d );
        free( mainpath.d );
        free( wall );
  //## end PL_Sequential::~PL_Sequential%.body
}

//=========================================================================================
// DrawExplicit
//
// Draws the plannmer to the output window
//
//=========================================================================================
bool PL_Sequential::DrawExplicit () const
{
/*              for (int a = 0; a < ptqlink->dimt; a++) 
                {
                        for (int b = 0; b < ptqlink->dimq; b++)
                        {
                                if ((a==pinitial.x) && (b==pinitial.y))
                                {
                                        //fprintf(stream, ";");
                                        bitmap.PutPixel(a, b, 0);
                                }
                                else if ((a==pfinal.x) && (b==pfinal.y))
                                {
                                        //fprintf(stream, ";");
                                        bitmap.PutPixel(a, b, 0);
                                }
                                else if (ptqlink->bitmap[a][b] == OBSTACLE) 
                                //if (ptqlink->bitmap[a][b] == OBSTACLE) 
                                {
                                        bitmap.PutPixel( a, b, 0 );
                                        //fprintf(stream, ";");
                                }
                                else if (ptqlink->bitmap[a][b] == FREESPACE) 
                                {
                                        bitmap.PutPixel( a, b, 1 );
                                        //fprintf(stream, "|");
                                }       
                        }               
                        //fprintf( stream, "\n" ) ;
                } 
*/
        if( this->tqcs.tqlink->bitmap == NULL )
        {
                //draw a test pattern
                //fill the pixel array with some data
                const int xRes = 128;
                const int yRes = 128;
                char pixels[ yRes ][ xRes ];
                int i;
                int j;
                for( i = 0; i < xRes; i++ )
                {
                        for( j = 0; j < yRes; j++ )
                        {
                                pixels[ j ][ i ] = i;
                        }
                }


                ::glDrawPixels
                (
                        128,    //width
                        128,    //height
                        GL_LUMINANCE,
                        GL_BYTE,
                        &pixels
                );
        }
        else
        {
                //draw a test pattern
                //fill the pixel array with some data
                const int xRes = tqcs.tqlink->dimt;
                const int yRes = tqcs.tqlink->dimq;

                ::glDrawPixels
                (
                        xRes,   //width
                        yRes,   //height
                        GL_LUMINANCE,
                        GL_BYTE,
                        tqcs.tqlink->bitmap
                );
        }
        return true;
}

//## Other Operations (implementation)
bool PL_Sequential::Plan ()
{
  //## begin PL_Sequential::Plan%928173143.body preserve=yes

        
        //create the output window
/*#ifdef PC_PROTO       
        bitmap.StartOutput() ;          //IMPROVE: remember to end output somewhere

        //this is how you draw a bitmap
        bitmap.NameDialog( "Started Planning" ) ;
        bitmap.SetResolution( 100, 100 ) ;
        for( int x = 0; x < 100; x++ )
        {
                for( int y = 0; y < 100; y++ )
                {
                        double intensity = Max( x, y ) / 100.0 ;
                        bitmap.PutPixel( x, y, intensity ) ;            
                }
        }
        bitmap.Display() ;
#endif  */

        int     i,j;
        int     blen;
        int     BT_flag, Block_flag, Narrow_flag, on_BT_max;
        int     current_link, path_link, path_final=SP_PLANNING, on_BT, linkno;
        int back_to_link;
        int     BT_Number[7][7], BT_Number_Record[7];
        TQConfig        block[100], block_last[1];

        // obtain joint limits of all links of robot
        for (linkno=0; linkno<arm_degree; linkno++) {
                original_joint_limit[linkno].status = 1;
                original_joint_limit[linkno].start = RADIAN(collisionDetector->JointMin(linkno));
                original_joint_limit[linkno].end = RADIAN(collisionDetector->JointMax(linkno));
        }
        
        // get start config 
        for( int loop = 0; loop < arm_degree; loop++ )
        {
                orig_initial_config[ loop ] = RADIAN(GetStartConfig()[ loop ]) ;
        }

        // get goal config 
        for( loop = 0; loop < arm_degree; loop++ )
        {
                orig_final_config[ loop ] = RADIAN(GetGoalConfig()[ loop ]) ;
        }
         
    // Convert all angles into look-up index            
    convert_angle_to_index();

        // planning for link0 
        allocate_space_for_link_cs(0);

        //deactivate all the frame-frame collison checking
        collisionDetector->DeactivateAllFrames() ;

        // activates frame-to-frame collision check between frame 0 and 1
        collisionDetector->ActivateFrames(0,1);  
        path_link = plan_path_for_first_link(); 
        if (path_link != SP_TQ_OK) 
        {
                return false;
        }

        //If path exists for link0, then proceeds to Main Path Search Scheme, Includes Backtracking             
        current_link = 1;   
        path_final= SP_PLANNING;
        on_BT = FALSE; 
        BT_level_test = 1;      // bt level of current backtracking. 
        BT_flag = 0;            // sign of maximum bt reached. 
        Block_flag = 0;         // sign of same blocks. 
        Narrow_flag = 0;        // sign of that goal is in narrow region 
        on_BT_max = 0;

        block_last[0].t = 0;
        block_last[0].q = 0;

        //IMPROVE: BT_Number_Record[7] and BT_Number[7][7] limiting dof
        for (i=0; i<=max_level; i++) {
            BT_Number_Record[i] = 0;
            for (j=0; j<=arm_degree; j++) {
                BT_Number[j][i] = 0;
            }
        } 

        current_link++; 

        // Main Loop of link path planning from link 2 to link n, link by link,
        // and backtracking to permitted level if neccesary     
        while ((current_link <= arm_degree) && (path_link == SP_TQ_OK)) {
                blen = 0;
                path_link = plan_path_for_nth_link(current_link,
                                &(tqcs.tqlink[current_link-1]), 
                                block, block_last, &blen, 
                                &BT_flag, &Block_flag, &Narrow_flag);

                if (path_link == SP_IF_NOT_OK){
                        return false ;
                }
                if (path_link == SP_STOPPING) {
                        return false ;
                }
                if (path_link == SP_TQ_OK) { //if path is found for current_link
                        on_BT = FALSE;
                        BT_level_test = 1;
                        current_link++;         //increment current_link 
                } 
                else // prepare for backtracking         
                { 
                    on_BT = TRUE;
                    BT_flag = 0;
                        // narrow region case 
                        if(Narrow_flag == 1) {
                                blen = 1;
                        }
                        // Determine if reached the MAXIMUM number 
                        if(BT_Number[current_link][BT_level_test]>=max_bt_no){
                                BT_Number_Record[BT_level_test] += max_bt_no;
                                BT_Number[current_link][BT_level_test] = 0;
                                BT_level_test++;
                                // Check if MAXIMUM BT LEVEL is reached 
                                if(BT_level_test > max_level){
                                        path_final = SP_FAILED;
                                        return false ;
                                }
                                BT_flag = 1;
                                blen = 1;
                        }
                } //end of else

                // If no path found for the current link, backtracking  
                while ((on_BT == TRUE) && (BT_level_test >= 1) && (BT_level_test <= max_level )) {
                        BT_Number[current_link][BT_level_test]++;
                    while(BT_Number[current_link][BT_level_test]>max_bt_no){
                                BT_Number_Record[BT_level_test] += max_bt_no;
                                BT_Number[current_link][BT_level_test] = 0;
                                BT_level_test++;
                                // Check if MAXIMUM BT LEVEL is reached 
                                if(BT_level_test>max_level){
                                    path_final = SP_FAILED;
                                        return false ;
                                        break;
                                }
                                BT_Number[current_link][BT_level_test]++;
                                BT_flag = 1;
                                blen = 1;
                        }
        
                        back_to_link = current_link - BT_level_test;

                        // There is no backtracking for link 1  
                        if (back_to_link < 2) {
                                return false;
                        }

                // Plan path for the link backtracked to        
                        path_link =     plan_path_for_nth_link(back_to_link,
                                                &(tqcs.tqlink[back_to_link-1]),
                                                block, block_last, &blen, 
                                                &BT_flag, &Block_flag, &Narrow_flag);
                        
                        if (path_link == SP_IF_NOT_OK) {
                                return false ;
                        }
                        if (path_link == SP_STOPPING) {
                                return false ;
                        }
                        if (path_link == SP_TQ_OK) {
                                BT_level_test--;
                        } 
                        else if (path_link == SP_TQ_NOT_OK) {
                                //prepare for continuous backtracking
                                BT_level_test++;
                                if(Narrow_flag == 1)
                                        blen = 1;
                        } 
                        else {
                                return false ;  //something's wrong with the SP
                        }
                        continue;
                } //end of while (for backtracting)
                
                BT_level_test = 1;
                continue;
        } //end of while (of main planner)

        // sum up backtracking numbers
        /*
        on_BT_max = 0;
        for (i=1; i<= max_level; i++) {
            for (j=1; j<=arm_degree; j++) {
                        if (BT_Number[j][i] > 0) {
                            BT_Number_Record[i] += BT_Number[j][i];
                                BT_Number_Record[0] += BT_Number_Record[i];
                                on_BT_max = i;
                        }
            }
            if (BT_Number_Record[i] > 0) {
                printf("\t%4d Times on Level %d\n",
                                BT_Number_Record[i],i);
                //fprintf(resultfile,"\t%4d Times on Level %d\n", 
                //              BT_Number_Record[i],i);
            }
        }
        */
        
        // compute global path - modified version (07/15)
        if ( path_final != SP_FAILED ) 
        {
                globaldim = lnkcps[ arm_degree - 1 ].dim + 2;
                globalpath = ( double** ) malloc2D( globaldim, arm_degree, sizeof( double ) );

                retrieve_global_path(globalpath, globaldim, orig_initial_config, orig_final_config);

                path.Clear() ;
                Configuration config ;
                config.SetNumDOF( arm_degree ) ;
                for( i = 0; i < globaldim; i++ )
                {
                        for( int j = 0; j < arm_degree; j++ )
                        {
                                config[ j ] = (globalpath[i][j]*180.0/PI);
                        }
                        path.AppendPoint( config ) ;
                }
                // for optimization 
                //smoothpath(arm_degree, globaldim, globalpath, &globaldim);

                //smooth the path
                if( this->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 );
                }
        }
        else {
                return (false);
        }
    
        return true ;
    
  //## end PL_Sequential::Plan%928173143.body
}

void PL_Sequential::initialize_parameters ()
{
  //## begin PL_Sequential::initialize_parameters%930592279.body preserve=yes

        // Assign default sequential planner debug level 
        sp_debug_level = 1; 

        // Default distance detect range 
        //distance_detect_range = 0.5;

        // Assigning default values     
        //dimx = DIMX;
        //dimy = DIMY;
        //dimz = DIMZ;

        dimt[0] = DIMT;  //resolution of tq map
        dimq[0] = DIMQ;  //resolution of tq map

        // maximum backtracking level allowed   
        max_level = MAX_BACKTRACKING_LEVEL;

        // maximum backtracking number in a level       
        max_bt_no = MAX_BACKTRACKING_NUMBER;

        COEFD_Q = dimq[0]/(2*PI);  //constant set for rad_to_index conversion
        COEFQ_D = (2*PI)/dimq[0];  //constant set for index_to_rad conversion

        for (int i=1; i<= arm_degree; i++)
        {
                dimq[i] = dimq[0];
        }
  //## end PL_Sequential::initialize_parameters%930592279.body
}

void PL_Sequential::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
  //## begin PL_Sequential::SetCollisionDetector%933360621.body preserve=yes
        PL_Boolean_Output::SetCollisionDetector( collisionDetector ) ;

        arm_degree = collisionDetector->DOF() ; 
        
        //set the collision detector so that nothing collides with anything
        for( int i = 0; i <= arm_degree; i++ )
        {
                for( int j = 0; j <= arm_degree; j++ )
                {
                        collisionDetector->DeactivateFrames( i, j ) ; 
                }
        }

        tqcs.nlinks = arm_degree;

        initialize_parameters();

        orig_initial_config = (double *) malloc(arm_degree*sizeof(double));
        assert( orig_initial_config != NULL );
        orig_final_config = (double *) malloc(arm_degree*sizeof(double));
    assert( orig_final_config != NULL );
    tqcs.tqlink = (TQLinkCSpace *) calloc(arm_degree, sizeof(TQLinkCSpace));
        assert( tqcs.tqlink != NULL );
    lnkcps = (LinkPath *) calloc(arm_degree, sizeof(LinkPath)); 
    assert( lnkcps != NULL );
        wall = (unsigned char *) calloc(dimq[0], sizeof(unsigned char) ) ;
    assert( wall != NULL );
    
        for (i=0; i<dimq[0];i++) {
                wall[i] = OBSTACLE;
        }

    for (i=0; i<arm_degree; i++) {
       lnkcps[i].d = NULL;
        }
    
    // allocate memory for the global path        
        allocate_space_for_working_tq_path();     
    globalpath = NULL;

  //## end PL_Sequential::SetCollisionDetector%933360621.body
}

// Additional Declarations
  //## begin PL_Sequential%3752D78501D0.declarations preserve=yes

//=========================================================================================
//int PL_Sequential::insert()
//called by: int PL_Sequential::voro_heuristic2d()
//input:        - int xx
//                      - int yy
//                      - IntPoint **FRONT
//                      - IntPoint *PFRONT
//                      - int nbfront
//                      - short int **V
//functions: to insert latest results into array when computing heuristic map of link
//output: returns incremented nbfront
//=========================================================================================
int PL_Sequential::insert (int xx, int yy, IntPoint **FRONT, IntPoint *PFRONT, int nbfront, 
                                                   short int **V)
{
        int icur,ipere;
        IntPoint *temp;

        nbfront++; 
        FRONT[nbfront] = PFRONT++;
        FRONT[nbfront]->x = (xx); 
        FRONT[nbfront]->y = (yy); 
        icur = nbfront;
        while(icur > 1){
                ipere = icur/2;
                if(V[FRONT[icur]->x][FRONT[icur]->y] <= V[FRONT[ipere]->x][FRONT[ipere]->y]) 
                        break;
                temp = FRONT[icur];
                FRONT[icur] = FRONT[ipere];
                FRONT[ipere] = temp;
                icur = ipere;
        }
        return (nbfront);
}


//=========================================================================================
//int PL_Sequential::deletemin()
//input:        - int xx
//                      - int yy
//                      - IntPoint **FRONT
//                      - int nbfront
//                      - short int **V
//function: ??
//output: returns modified nbfront
//=========================================================================================
int PL_Sequential::deletemin (int xx, int yy, IntPoint **FRONT, int nbfront, short int **V)
{
        int icur, inew, ifilsd, ifilsg; 
        IntPoint *temp;
        
        xx = FRONT[1]->x; 
        yy = FRONT[1]->y;
        FRONT[1] = FRONT[nbfront];
        nbfront--;
        icur = 1;

        while(icur <= nbfront / 2){
                ifilsg = 2 * icur; 
                ifilsd = 2 * icur + 1;
                if(V[FRONT[ifilsg]->x][FRONT[ifilsg]->y] > V[FRONT[ifilsd]->x][FRONT[ifilsd]->y] || nbfront == ifilsg)
                        inew = ifilsg;
                else 
                        inew = ifilsd;

                if(V[FRONT[icur]->x][FRONT[icur]->y] < V[FRONT[inew]->x][FRONT[inew]->y]){
                        temp = FRONT[icur];
                        FRONT[icur] = FRONT[inew];      
                        FRONT[inew] = temp;
                        icur = inew;
                }
                else break;
        }
        return (nbfront);
}


//=========================================================================================
//int PL_Sequential::plan_path_for_first_link()
//input: none
//function: plan path for link0; lnkcps[0] includes the max. reachable free space of link0
//output: returns a flag to indicate if path for link0 can be found
//=========================================================================================
int PL_Sequential::plan_path_for_first_link( void )
{
        int     col_rc, i, j, iminable, imaxable, fminable, fmaxable, connect_case;
        int     blocked;
        unsigned char   **map;
        TQConfig        start, end;
        double          ft;
        TQConfig        *para;

        //determine if boundary of t and q exist
    tqcs.tqlink[0].t_bound = FALSE;                     // No t boundary for first link 
    if (!(joint_limit[0].status))
        tqcs.tqlink[0].q_bound = FALSE;         // Boundary for q    
    else
        tqcs.tqlink[0].q_bound = TRUE;          // No boundary for q    

        //assign map to be tq-map (bitmap) of link0
        map = tqcs.tqlink[0].bitmap;  
        
        //a dummy variable
        para = (TQConfig *) malloc (sizeof(TQConfig));
        para[0].t = 0;
        para[0].q = 0;

        //t is irrevalent for link0; compute a column of tq-map for link0
        col_rc = compute_a_column_for_link_cspace(tqcs.tqlink, 1, 0, para);
    if (col_rc == SP_STOPPING) {
                return(false);
    }
        
        free(para);  //free dummy variable

    // determine if tq-space is blocked horizontally
    blocked = false;
    for (j=0; j<tqcs.tqlink[0].dimq; j++) { 
        if ( map[0][j] == OBSTACLE) {
            blocked = true;
            break;
        }
        } 
        
        // since t is irrevalent for link0, all columns of tq-map for link0 are identical;
        // copy column for all t in order to complete tq-map for link0
        for (j=1; j<tqcs.tqlink[0].dimt; j++) {
                for (i=0; i<tqcs.tqlink[0].dimq; i++) { 
            map[j][i] = map[0][i];
                } 
        }

        // Find the reachable space range in cspace
    // look for lowest reachable bound from start
    iminable = initial_config[0];
        while ((iminable>=0) || blocked)
        {
            if (map[0][(iminable+dimq[0])%dimq[0]]==FREESPACE)
                {
                        iminable--;
                }
        else
                {
                        break;
                }
        }
    iminable++;    // Found the low reachable bound from start    

        // look for highest reachable bound from start
    imaxable = initial_config[0];
    while ((imaxable<tqcs.tqlink[0].dimq) || blocked)
        {
                if (map[0][imaxable%dimq[0]]==FREESPACE)
                {
                        imaxable++;
                }
        else 
                {
                        break;
                }
        }
    imaxable--;    // Found the high reachable bound from start    

        // look for lowest reachable bound from goal
    fminable = final_config[0];
    while ((fminable>=0) || blocked) 
        {
                if (map[0][(fminable+dimq[0])%dimq[0]]==FREESPACE)
                {
                        fminable--;
                }
        else 
                {
                        break;
                }
        }
    fminable++;    // Found the low reachable bound from goal    

        // look for highest reachable bound from goal
    fmaxable = final_config[0];
    while ((fmaxable<tqcs.tqlink[0].dimq) || blocked)
        {
                if (map[0][fmaxable%dimq[0]]==FREESPACE)
                {
                        fmaxable++;
                }
        else
                {
                        break;
                }
        }
    fmaxable--;    // Found the high reachable bound from goal    

    // The simplest case in tq-map of link0:            
    //    obstacle area
    //   *  **********************************
    //   *  ssssssssssssssssssssssssssssssssss
    // ^ *             
    // | *                              free space
    // q *  gggggggggggggggggggggggggggggggggg                      
    //   *  **********************************
    //     obstacle area
        //              t ->
 
        if (((initial_config[0]>fminable)&&(initial_config[0]<fmaxable))
                || ((final_config[0]>iminable)&&(final_config[0]<imaxable))
                || (initial_config[0]==final_config[0]))
        connect_case = 0;
        // I and F are seperated by a middle obstacle   
    else {            
        if (tqcs.tqlink[0].q_bound)
            return (false);  //no path exists for lnk1
        if ( ((iminable<=0) && (fmaxable>=(tqcs.tqlink[0].dimq-1)) ) 
                        || ((fminable <=0) && (imaxable>=(tqcs.tqlink[0].dimq-1) ) ) )
          connect_case = 1;
        else
            return (false);  //no path exists for lnk1
    }

    // build a path for each connection case        
    start.t = 0;                //initial t of path for link0
    end.t = dimt[0]-1;  //final t of path for link0

        allocate_space_for_Link_tq_path(0, dimt[0]);
        
    // determine start and end q according to the connection case    
    if (connect_case == 0) {
        if (initial_config[0] <= final_config[0]) {
            start.q = iminable;    end.q = fmaxable;
        }
        else {
            start.q = imaxable;    end.q = fminable;
        }
        lnkcps[0].dim = dimt[0];
    }
    else if (connect_case == 1) {
        if (initial_config[0] <= final_config[0]) {
            start.q = imaxable;
            end.q = fminable-dimq[0];  // mod modification for a path
        }
        else {
            start.q = iminable;
            end.q = fmaxable+dimq[0];  // mod modification for a path
        }
    }
    else {
                return (false);  
        }

    // Determine if q bound exits        
    if ((start.q==((end.q+1)%dimq[0])) || ((end.q%dimq[0])==((start.q+1)%dimq[0])))
        tqcs.tqlink[0].q_bound = FALSE;
    else
        tqcs.tqlink[0].q_bound = TRUE;

    // Determine path bound for next link path plan    
    if ( !(tqcs.tqlink[0].t_bound) && !(tqcs.tqlink[0].q_bound) )
        tqcs.tqlink[0].p_bound = FALSE;
    if ( tqcs.tqlink[0].p_bound ) {
        lnkcps[0].d[0].t = start.t;
        lnkcps[0].d[0].q = ROUND(start.q);
        lnkcps[0].d[dimq[0]-1].t = end.t;
        lnkcps[0].d[dimq[0]-1].q = ROUND(end.q);
        start.t += 1; end.t -= 1;  // Leave a column for bounding        
    }

    // generate a linear path for link0, starting from lowest reachable from start,
        // to highest reachable from goal
        for (i = start.t, ft = start.t; i <= end.t; i++, ft++) {
        lnkcps[0].d[i].t = i;  
        lnkcps[0].d[i].q = ROUND(ft*(end.q-start.q)/(double)dimt[0]+start.q);
        lnkcps[0].d[i].q = (lnkcps[0].d[i].q +dimq[0]) %dimq[0];
        // record t,q initial and final in link path        
        if (lnkcps[0].d[i].q % dimq[0] == initial_config[0]) {
            initial_tq[0].t = i;
            initial_tq[0].q = lnkcps[0].d[i].q;
                }
        if (lnkcps[0].d[i].q % dimq[0] == final_config[0]) {
            final_tq[0].t = i;
            final_tq[0].q = lnkcps[0].d[i].q;
                }
    }

        // ensure q in lnkcps[0] are incremental (i.e. relative to its previous step)
        if (sp_debug_level >0) {
                for (i=0; i< dimt[0]; i++) {
            lnkcps[0].d[i].q = lnkcps[0].d[i].q % dimq[0];
        }
        }

        //programmer outputs
        /*
    if (sp_debug_level >0) {
        printf("Initial and final tq configuration at link 1 are:\n");
        printf("\t\t(%d, %d)->(%d, %d).\n", initial_tq[0].t, initial_tq[0].q,
            final_tq[0].t, final_tq[0].q);
            write_map("DATA/link1obs.map", TQ_CSpace_Obs,
            (char **) tqcs.tqlink[0].bitmap, dimt[0], dimq[0],
            sizeof(unsigned char));
        write_linkpath("DATA/link1path.dat", TQ_CSpace_Path,
            (char *) lnkcps[0].d, dimt[0], sizeof(TQConfig) );
        write_linkpathtxt("DATA/link1path.txt", lnkcps[0].d, dimt[0]);
        write_tqif("DATA/link1if.dat", initial_tq[0], final_tq[0]);
    }
        */
        
    // prepare the dimensions for next link    
        dimt[1] = lnkcps[0].dim;     
       
    free_space_for_link_cs(0);  // free memory

    return(SP_TQ_OK); // return flag to indicate path found for link0

} // plan_path_for_first_link()    


//=========================================================================================
//int PL_Sequential::path_path_for_nth_link()
//input:        - int lnkno
//                      - TQLinkCSpace *ptqlink
//                      - TQConfig *pblock
//                      - TQConfig *pblock_last
//                      - int *bl (not used)
//                      - int *BT_Flag
//                      - int *block_flag
//                      - int *narrow_flag
//functions: find plan for nth link
//output: returns a flag to indicate if path is found for linkn
//=========================================================================================
int PL_Sequential::plan_path_for_nth_link(      int lnkno, 
                                TQLinkCSpace *ptqlink, 
                                TQConfig *pblock,
                                TQConfig *pblock_last, 
                                int *bl, 
                                int *BT_Flag, 
                                int *block_flag,
                                int *narrow_flag)
{
        int     tq_rc, i, j ;
        int lnkno1, path_found; 
        int     ipos, fpos;
        IntPoint        pinitial, pfinal;
        TQConfig        tqi, tqf;
        int blockwidth = 0;
        
        // activate frame_to_frame collision detection
        for (i = 0; i < lnkno; i++)
        {
                collisionDetector->ActivateFrames(i, lnkno);
        }
        
    lnkno1 = lnkno - 1 ;        //array indexing convention

        // determine the initial TxQ position in TxQ space        
    tqi.t = pinitial.x = initial_tq[lnkno-2].t;
    tqi.q = pinitial.y = initial_config[lnkno1]; 

    // determine the final TxQ position in TxQ space        
    tqf.t = pfinal.x = final_tq[lnkno-2].t;
    tqf.q = pfinal.y = final_config[lnkno1];

        // Allocate space for link c-space    
        allocate_space_for_link_cs(lnkno1);

        //compute obstacle map for link n
        tq_rc = compute_cspace_obstacle_map(lnkno);
        if (tq_rc == SP_STOPPING) {
                free_space_for_link_cs(lnkno1);
                return(tq_rc);
        }
        
        if (*bl) //in backtracking
        {
                blockwidth = *bl;
                const int RESERVED_SIZE = 2;

                for (i = 0; i < blockwidth; i++) 
                {
                        for (int k = pblock[i].t - blockwidth; k < pblock[i].t + blockwidth; k++) 
                                {
                                        for (j = pblock[i].q - blockwidth; j < pblock[i].q + blockwidth; j++)
                                        {
                                                if ((ABS(tqi.t - k) > RESERVED_SIZE)
                                                        && (ABS(tqi.q - j) > RESERVED_SIZE)
                                                && (ABS(tqf.t - k) > RESERVED_SIZE)
                                                        && (ABS(tqf.q - j) > RESERVED_SIZE))
                                                {
                                                                ptqlink->bitmap[(k+dimt[lnkno1]) % dimt[lnkno1]]
                                                                        [(j+dimq[lnkno1]) % dimq[lnkno1]] 
                                                                        = OBSTACLE;
                                                }
                                        }
                                }
                }
                        
                // Collision Check
                if (ptqlink->bitmap[pinitial.x][pinitial.y] == OBSTACLE) {
                        pblock[0].t = lnkcps[lnkno1-1].d[pinitial.x].t;
                        pblock[0].q = lnkcps[lnkno1-1].d[pinitial.x].q;
                        *bl = 1;
                        free_space_for_link_cs(lnkno1);
                        return(SP_TQ_NOT_OK);   //requires further backtracking
                }

                // test if goal is in obstacle    
                if (ptqlink->bitmap[pfinal.x][pfinal.y] == OBSTACLE) {
                pblock[0].t = lnkcps[lnkno1-1].d[pfinal.x].t;
                    pblock[0].q = lnkcps[lnkno1-1].d[pfinal.x].q;
                        *bl = 1;
                        free_space_for_link_cs(lnkno1);
                        return(SP_TQ_NOT_OK);  //requires further backtracking
                }
        }
        else 
        {
                // Collision Check
            // test if start is in obstacle   
                if (ptqlink->bitmap[pinitial.x][pinitial.y] == OBSTACLE) {
                        free_space_for_link_cs(lnkno1);
                        return(SP_IF_NOT_OK);   
                }

                // test if goal is in obstacle    
                if (ptqlink->bitmap[pfinal.x][pfinal.y] == OBSTACLE) {
                free_space_for_link_cs(lnkno1);
                        return(SP_IF_NOT_OK);
                }
        }
        
                //GILL: added for debugging (07/15)
/*#ifdef PC_PROTO               
                bitmap.SetResolution( ptqlink->dimt, ptqlink->dimq );
                bitmap.Clear();
                char namedialog[ 300 ];
                sprintf( namedialog, "obstacle map for link %d", lnkno ) ;
                bitmap.NameDialog( namedialog ) ;
                bitmap.Display();

                for (int a = 0; a < ptqlink->dimt; a++) 
                {
                        for (int b = 0; b < ptqlink->dimq; b++)
                        {
                                if ((a==pinitial.x) && (b==pinitial.y))
                                {
                                        //fprintf(stream, ";");
                                        bitmap.PutPixel(a, b, 0);
                                }
                                else if ((a==pfinal.x) && (b==pfinal.y))
                                {
                                        //fprintf(stream, ";");
                                        bitmap.PutPixel(a, b, 0);
                                }
                                else if (ptqlink->bitmap[a][b] == OBSTACLE) 
                                //if (ptqlink->bitmap[a][b] == OBSTACLE) 
                                {
                                        bitmap.PutPixel( a, b, 0 );
                                        //fprintf(stream, ";");
                                }
                                else if (ptqlink->bitmap[a][b] == FREESPACE) 
                                {
                                        bitmap.PutPixel( a, b, 1 );
                                        //fprintf(stream, "|");
                                }       
                        }               
                        //fprintf( stream, "\n" ) ;
                } 
                //fclose(stream);  
                //end of Gill outputs
                bitmap.Display();
#endif          
*/
    // determine whether bounds exist for t, q parameters        
    if (joint_limit[lnkno1].status)
        ptqlink->q_bound = TRUE;
    else
        ptqlink->q_bound = FALSE;

    if ((tqcs.tqlink[lnkno-2].p_bound) || (tqcs.tqlink[lnkno-2].q_bound))
        ptqlink->t_bound = TRUE;
    else
        ptqlink->t_bound = FALSE;

    if (ptqlink->t_bound || ptqlink->q_bound)
        ptqlink->p_bound = TRUE;

    // Block both end of the TxQ space if t bound exist        
    if (ptqlink->t_bound) {
        for (j=0; j<dimq[lnkno1]; j++)
            ptqlink->bitmap[0][j]
                = ptqlink->bitmap[dimt[lnkno1]-1][j] = OBSTACLE;
    }

    // Find the main path of the whole path, ie. from initial to final  
        path_found = find_main_path(lnkno, &pinitial, &pfinal,
                                ptqlink, &mainpath, narrow_flag);

    if (!path_found) {
                if((pblock_last[0].t == mainpath.d[0].t) && (pblock_last[0].q == mainpath.d[0].q))
                {
                        *block_flag = 1;
                        pblock_last[0].t = 0;
                        pblock_last[0].q = 0;
                } 
                else 
                {
                        *block_flag = 0;
                        pblock_last[0].t = mainpath.d[0].t;
                        pblock_last[0].q = mainpath.d[0].q;
                }
                if (lnkno > 2) { // Note, there is no backtracking for link 1 and 2 
         for (i = 0 ; i < MIN(mainpath.dim, 99) ; i++) {
                           pblock[i].t = lnkcps[lnkno1-1].d[mainpath.d[i].t].t;
               pblock[i].q = lnkcps[lnkno1-1].d[mainpath.d[i].t].q;
                        }
                }
 
                *bl = MIN(mainpath.dim, 99); // store block width 

            free_space_for_link_cs(lnkno1);
   
                return(SP_TQ_NOT_OK);  //flag to indicate back tracking
        }
        
    // Find the forward extention path, ie. final to  ??????    
    if (lnkno != arm_degree)
        find_forward_extension(lnkno, &pinitial, &pfinal, ptqlink, &frwdpath);
    else
        frwdpath.dim = 0;

    // Find the backward extention path, ie. initial to  ?????? 
    if (lnkno != arm_degree)
        find_backward_extension(lnkno, &pinitial, &pfinal, ptqlink, &bkwdpath);
    else
        bkwdpath.dim = 0;

    // Connect the three parts of path into a whole path        
    combine_three_paths(&combpath, &mainpath, &frwdpath, &bkwdpath, &ipos, &fpos);

        if (NO_PATH_COMPRESS) {
        if (ptqlink->p_bound && (lnkno != arm_degree)) {
            dimt[lnkno] = combpath.dim+2;
            allocate_space_for_Link_tq_path(lnkno1, dimt[lnkno]);  // Allocate memory for link TQ path     
            reparameterize_path(ptqlink->p_bound, &combpath, &(lnkcps[lnkno1]), 
                                &ipos, &fpos, lnkno);
        }
        else {
            if (ptqlink->p_bound)
                        {
                                dimt[lnkno] = combpath.dim+2;
                        }
            else
            {
                                dimt[lnkno] = combpath.dim;
                        }
            allocate_space_for_Link_tq_path(lnkno1, dimt[lnkno]);
            reparameterize_path(ptqlink->p_bound, &combpath, &(lnkcps[lnkno1]), 
                                &ipos, &fpos, lnkno);
        }
    }
    else {
                dimt[lnkno] = dimt[0];
                allocate_space_for_Link_tq_path(lnkno1, dimt[lnkno]);  // Allocate memory for link TQ path     
        // Reparameterize the whole path to pre-defined dimension  
        reparameterize_path(ptqlink->p_bound, &combpath, &(lnkcps[lnkno1]), 
                        &ipos, &fpos, lnkno);
    }

    // mark the initial and final position on the path        
    initial_tq[lnkno1].t = ipos;
    initial_tq[lnkno1].q = lnkcps[lnkno1].d[ipos].q;

    final_tq[lnkno1].t = fpos;
    final_tq[lnkno1].q = lnkcps[lnkno1].d[fpos].q;
    *bl = 0;

    dimt[lnkno] = lnkcps[lnkno1].dim;  // dimension of t for current link

        // ensure q in lnkcps[n] is incremental (i.e. relative to its previous step)
        if (sp_debug_level > 0) {
                for (i=0; i<dimt[lnkno]; i++) {
            lnkcps[lnkno1].d[i].q = lnkcps[lnkno1].d[i].q % dimq[lnkno1];
                }                       
        }

        // programmer outputs
        //outputs heuristic map, distance map, and voronoi map to file 
        /*
        if (sp_debug_level>0) {
                sprintf(fname,"link%dheur.map", lnkno);
                write_map(fname, TQ_CSpace_Heuristic_Map, ptqlink->potential, dimt[lnkno1], dimq[lnkno1], sizeof(short int) );
                sprintf(fname,"link%dvoro.map", lnkno);
                write_map(fname, TQ_CSpace_Voronoi_Map, ptqlink->voro, dimt[lnkno1], dimq[lnkno1], sizeof(unsigned char) );
        }

    // generate tq space distance bitmap file name. 
        sprintf(fname,"link%1ddis.map",lnkno);
    write_distance_map(fname, TQ_CSpace_Distance_Map, (short int **) ptqlink->distmap, dimt[lnkno1], dimq[lnkno1], sizeof(short int) );
        
        if (sp_debug_level>0) {
        sprintf(fname,"DATA/link%dobs.map", lnkno);
        write_map(fname, TQ_CSpace_Obs, ptqlink->bitmap,
            dimt[lnkno1], dimq[lnkno1], sizeof(unsigned char) );
        sprintf(fname, "DATA/link%1dpath.dat", lnkno);
        write_linkpath(fname, TQ_CSpace_Path,
             (char *) lnkcps[lnkno1].d, dimt[lnkno], sizeof(TQConfig) );
        sprintf(fname, "DATA/link%1dpath.txt", lnkno);
        write_linkpathtxt(fname, lnkcps[lnkno1].d, dimt[lnkno]);
    }
        */
        
    free_space_for_link_cs(lnkno1);  // free memory

    return(SP_TQ_OK);  // return flag to indicate path found for link n

} // plan_path_for_nth_link()    


//=========================================================================================
//int PL_Sequential::compute_a_column_for_link_cspace()
//intputs:      - TQLinkCSpace *tqlcs
//                      - int lnkno
//                      - TQConfig *para
//functions: compute a column of tq-map for link n, where t is fixed, while q varies
//               in the range of link's upper and lower limits
//output: flag to indicate success  (not necessary ?)
//=========================================================================================
int PL_Sequential::compute_a_column_for_link_cspace(TQLinkCSpace        *tqlcs,
                                                                                                    int         lnkno, 
                                                                                                    int         col, 
                                                                                                        TQConfig        *para)
                                                                        
{
        int is, ie, lnkno1, k;
        unsigned char   *row;
        Configuration config;
        config.SetNumDOF( arm_degree ) ;
        
        lnkno1 = lnkno - 1;             // array indexing convention

        row = tqlcs->bitmap[col];       // assign row to be linkn tq-map at fixed t

    if (joint_limit[lnkno1].status) {
                is = joint_limit[lnkno1].start+1;    // linkn's lower limit    
        ie = joint_limit[lnkno1].end;        // linkn's upper limit    
                 
        is = MAX(0,is); 
                ie = MIN(tqlcs->dimq, ie); 

                for (int i = 0; i < is; i++) {  //mark the unreachable in cspace obstacle
            row[i] = OBSTACLE;
                }
        for (i = ie; i < tqlcs->dimq; i++) { 
                        row[i] = OBSTACLE;
                }

                for( i = is; i < ie; i++ )
                {
                        row[ i ] = FREESPACE ;          //IAN added this for initialization purposes
                }
        }
    else {
        is = 0;
        ie = tqlcs->dimq;
    }

        //ian added this for initialization purposes
        config = GetStartConfig();

        // copies the previous joints variables for collision detection
        for( int j = 0; j < lnkno1; j++ )
        {
                config[ j ] = (index_to_rad(para[j].q))*180/PI ;                
        }

        // at the instance of t (at given configs of link n-1...0), determine if the present
        // config of link n is colliding with an obstacle
        k = is;
    while ( k < ie) 
        {
                double joint1Config = index_to_rad( k ) * 180.0 / PI ;  //IMPROVE: use a function to convert to degrees
                config[lnkno1] = joint1Config; 
                if ( !collisionDetector->IsInterfering( config ) ){
            row[k] = FREESPACE;
                }
        else {
            row[k] = OBSTACLE;
                }
                k++;
    } 

        return(SP_TQ_OK);  // return a flag to indicate success

} // compute_a_column_for_link_cspace() 


//=========================================================================================
//int PL_Sequential::compute_cspace_obstacle_map()
//intput:       - int lnkno
//function: computes tq obstacle map for link n
//output: flag to indicate success
//=========================================================================================
int PL_Sequential::compute_cspace_obstacle_map(int lnkno)
{
        int     tq_rc=SP_TQ_OK, lnkno1;
        TQConfig    para[MAX_DEGREE];
        
    lnkno1 = lnkno - 1;        // array indexing convention
        
        // For every column of tq-map    
    for (int i = 0; i < dimt[lnkno1]; i++) {
        // find q for link n-1 
            para[lnkno1-1].q = lnkcps[lnkno1-1].d[i].q;
        para[lnkno1-1].t = lnkcps[lnkno1-1].d[i].t;
        
                // find q for all previous links link n-2...0    
                for (int j=lnkno1-2; j>=0; j--) {
                   para[j].t = lnkcps[j].d[ para[j+1].t].t; 
                   para[j].q = lnkcps[j].d[ para[j+1].t].q;
                }

                // Obtain the absolute angles 
                //for (int k=1; k<lnkno1; k++) {
                //  para[k].q = (para[k].q+para[k-1].q+dimq[lnkno1]/2)%dimq[lnkno1];
                //}
                
                // compute each column of the tq-map        
                tq_rc = compute_a_column_for_link_cspace(&(tqcs.tqlink[lnkno1]),
              lnkno, i, para); 
                if (tq_rc == SP_STOPPING) {
                        break;
                }
    }

    return(tq_rc);  // returns flag to indicate success

} // compute_cspace_obstacle_map()      


//=========================================================================================
//void PL_Sequential::convert_angle_to_index()
//input: none
//function: convert link configs into lookup table
//=========================================================================================
void PL_Sequential::convert_angle_to_index(void)
{
    // For each angle, plus 180 offset. the 0.5 is for the roundup      
    for (int i=0; i<arm_degree; i++) {
      initial_config[i] = rad_to_index(orig_initial_config[i]);
      final_config[i]   = rad_to_index(orig_final_config[i]);
      joint_limit[i].status = original_joint_limit[i].status;
      joint_limit[i].start = rad_to_index(original_joint_limit[i].start);
      joint_limit[i].end = rad_to_index(original_joint_limit[i].end);
    }
} // convert_angle_to_index()   


//=========================================================================================
//int PL_Sequential::rad_to_index()
//input: double angle
//function:     convert link config (in radian) into an index 
//output: corresponding index 
//note: modify this function if robot has prismatic joints
//=========================================================================================
int PL_Sequential::rad_to_index( double angle ) 
{
    return((int)(COEFD_Q*(angle+PI)+0.5));
} 


//=========================================================================================
//double PL_Sequential::index_to_rad()
//input: int index
//function: convert an index into the corresponding link config (in radian)
//output: corresponding link config
//=========================================================================================
double PL_Sequential::index_to_rad( int index )
{
     return((double)(COEFQ_D*index - PI));
} 


//=========================================================================================
//void PL_Sequential::modify_cspace_obstacle_map()
//note: 
//=========================================================================================
void PL_Sequential::modify_cspace_obstacle_map(TQLinkCSpace     *ptqlink, 
                                TQConfig        *tqi,
                                TQConfig        *tqf, 
                                TQConfig        *pblock,
                int             len, 
                                int             L1, 
                                int             *BT_FLAG, 
                                int             *narrow_flag)

{
/*
    int i, j, k, pblock_extral;
        int blockwidth;
        int d_max, d_max1, d_count_max, d_count, L1_half;
    int d_max_t[100];
    int initial_t, final_t;


        const int RESERVED_SIZE = 2;

    // the resolution of tq space is reduced to 4 degree 
    blockwidth = dimq[L1]/32;

    pblock_extral = 0;

        // place a virtual obstacle into tq map of (n-2)th link or
        // (n-1)th link if max bt is reached or goal is in narrow
        // region respectively.
        //

        if(*BT_FLAG == 1 || *narrow_flag == 1) 
        {
                if(*BT_FLAG) 
                {
                        *BT_FLAG = 0;
                }
            
                if(*narrow_flag)
                {
                *narrow_flag = 0;
                }

            L1_half = (final_tq[L1].t - initial_tq[L1].t)/2;
            d_count = 1;

            d_max_t[d_count] = initial_tq[L1].t + 2;

            initial_t = initial_tq[L1].t + 2;
            final_t = final_tq[L1].t - 2;

            d_max = ptqlink->distmap[lnkcps[L1].d[initial_t].t][lnkcps[L1].d[initial_t].q];

            for(j = initial_t + 1; j <= final_t; j++) 
                {
                d_max1 = ptqlink->distmap[lnkcps[L1].d[j].t][lnkcps[L1].d[j].q];
                if (d_max1 > d_max)
                        {
                                d_max = d_max1;
                                d_count = 1;
                                d_max_t[d_count] = j;
                } 
                else if (d_max1 == d_max) 
                        {
                                d_count ++;
                                d_max_t[d_count] = j;
                }
            }

            d_count_max = d_max_t[1];

            for (j=1; j<d_count; j++) {
              if ((initial_t + L1_half - d_max_t[j]) >= (d_max_t[j+1] - L1_half - initial_t))
                                d_count_max = d_max_t[j+1]; 
            }

            len = 1;

            pblock[0].t = lnkcps[L1].d[d_count_max].t;
            pblock[0].q = lnkcps[L1].d[d_count_max].q;
        } // end of outer if

    for (i = 0; i < MIN(len, dimt[L1]/30); i++)
        {
                for (k = pblock[i].t + dimt[L1] - blockwidth; 
                k < pblock[i].t + dimt[L1] + blockwidth; k++)
                {
                        for (j = pblock[i].q + dimq[L1] - blockwidth;
                j < pblock[i].q + dimq[L1] + blockwidth; j++)
                                {
                                        if ( (ABS(tqi->t + dimt[L1] - k) > RESERVED_SIZE)
                                        && (ABS(tqi->q + dimq[L1] - j) > RESERVED_SIZE)
                                && (ABS(tqf->t + dimt[L1] - k) > RESERVED_SIZE)
                                        && (ABS(tqf->q + dimq[L1] - j) > RESERVED_SIZE) )
                                        {
                                                ptqlink->bitmap[(k+dimt[L1])%dimt[L1]][(j+dimq[L1])%dimq[L1]] = OBSTACLE;
                                        }
                                }
                
                }
            
    }
        */
} // modify_cspace_obstacle_map


//=========================================================================================
//PL_Sequential::find_main_path()
//inputs:       - int lnkno
//                      - IntPoint *tqinitial
//                      - IntPoint *tqfinal
//                      - TQLinkCSpace *ptqlink
//                      - LinkPath *result
//                      - int *narrow_flag
//functions: find main path for link n by computing distance map, voronoi map, and
//                       heuristic map                          
//outputs: flag to indicate if a path is found for link n
//=========================================================================================
PL_Sequential::find_main_path(  int                             lnkno, 
                                                                IntPoint                *tqinitial, 
                                                                IntPoint                *tqfinal, 
                                                                TQLinkCSpace    *ptqlink, 
                                                                LinkPath                *result, 
                                                                int                             *narrow_flag)
{
        int     i, j, k, wallpos, mid, t1, t2, width, lnkno1, found; 
        unsigned char   *wtmp;

        lnkno1 = lnkno - 1;  //array indexing convention
        
        // use a wall to block the possible circle in main path 
        if ((tqinitial->x !=0 ) && (tqfinal->x != 0))
            wallpos =0;
        else
            wallpos = dimt[lnkno1]-1;
        
        wtmp = ptqlink->bitmap[wallpos];
        ptqlink->bitmap[wallpos] = wall;

        // Computing the distance map and voronoi diagram in TxQ cspace 
        distance2d(ptqlink->bitmap, dimt[lnkno1], dimq[lnkno1],
                        ptqlink->distmap, ptqlink->voro);

        // Check if final position is valid; if not return final position to replan     
        if ((ptqlink->distmap[tqfinal->x][tqfinal->y] < 1 ) 
                || (ptqlink->distmap[tqfinal->x][tqfinal->y] == MAXSHORTINT)) 
        {
                result->d[0].t = tqfinal->x;
                result->d[0].q = tqfinal->y;
                result->dim = 1;
                // remove the block wall                
                ptqlink->bitmap[wallpos] = wtmp;
                return(false);
        }

        // compute the heuristics in the TQ space       
        voro_heuristic2d(ptqlink->bitmap, dimt[lnkno1],
                        dimq[lnkno1], *tqfinal,
                        ptqlink->distmap,  ptqlink->voro, 
                        ptqlink->potential, narrow_flag);

        // check if a path can be found, i.e., heuristic has reached the
        // intial position, if not find the place where the block could
        // occured, return for replanning

        int startpot = 0;

        startpot = ptqlink->potential[tqinitial->x][tqinitial->y]; // Potential at I 

        if ((startpot < 1) || (startpot >= MAXSHORTINT)) {
            if((tqinitial->x == tqfinal->x) && (tqinitial->y == tqfinal->y)) {
                        result->d[0].t = tqinitial->x;
                        result->d[0].q = tqinitial->y;
                        result->dim = 1;
                        ptqlink->bitmap[wallpos] = wtmp;
                        return(true);  
            } // end of if

                else if (tqinitial->x != tqfinal->x)
                {
                        found = false; 
                
                        for (i = tqinitial->x + 2; i < tqfinal->x - 2; i++) 
                        {
                                width = (i - tqinitial->x) / 2;
                                width = MAX(1, width);
                                width = MIN(dimq[lnkno1]/10, width);

                                mid = tqinitial->y + (tqfinal->y - tqinitial->y) * (i - tqinitial->x)
                                / (tqfinal->x - tqinitial->x);
                                j = k = mid;
                                
                                while ((k >= 0) || (j < dimq[lnkno1])) 
                                {
                                        t1 = i;
                                        if ((k >= 0) && ( (mid - k) <= width))
                                        {
                                                if ((ptqlink->potential[t1][k] > 0)
                                                        && (ptqlink->potential[t1][k] < MAXSHORTINT)) 
                                                {
                                                        found = true;
                                                        result->d[0].t = t1;
                                                        result->d[0].q = k;
                                                        result->dim = 1;
                                                        break;
                                                }
                                        }
                                        k--;
                                        t2 = i;
                                        if ((j < dimq[lnkno1]) && !found && ((j - mid) <= width))
                                        {
                                                if ((ptqlink->potential[t2][j] > 0)
                                                        &&(ptqlink->potential[t2][j] < MAXSHORTINT)) 
                                                {
                                                  found = true;
                                                  result->d[0].t = t2;
                                                  result->d[0].q = j; 
                                                  result->dim = 1; 
                                                  break;
                                                }
                                        }
                                        j++;
                                }  //end of while
                                if (found) break;
                        } //end of for

                        // locate the pixel most close to initial position 
                        // which is reachable from goal inside for loop,      
                        // stored at result->d[0]                             
                        i = result->d[0].t-1;
                        j = result->d[0].q;
                        k = result->dim;
                
                        // determine the block width from the pixel found  
                        // inside above for loop to the left. k is the width  
                        // finally determined and stored in result->dim below 

                        //while (((ptqlink->bitmap[i][j] == OBSTACLE) && (k < MAXBLOCKLENGTH)) 
                        //      || ( k < MAXBLOCKLENGTH / 4)) 
                        while ((((ptqlink->potential[i][j] < 0) || (ptqlink->potential[i][j] == MAXSHORTINT))
                                && (k < MAXBLOCKLENGTH)) 
                                || ( k < MAXBLOCKLENGTH / 4)) 
                        {
                                result->d[k].t = i;
                                result->d[k].q = j;
                                i--; k++;
                        }
                        
                        result->dim = k;

                        // remove the block wall                
                        ptqlink->bitmap[wallpos] = wtmp;        
                        
                        return(false);

                }  //end of else if

                else //tqinitial->x == tqfinal->x
                {
                        found = false;
                        int pos1, pos2, pos3, pos4;
                        int cur_x = tqinitial->x;
                        int cur_y = ROUND((tqinitial->y + tqfinal->y) / 2);

                        for (i = 0; ((cur_x + i) < dimt[lnkno1]) 
                                && ((cur_x - i) >= 0) 
                                && ((cur_y + i) < dimq[lnkno1]) 
                                && ((cur_y - i) >= 0); i++) 
                        {
                                pos1 = (cur_x + i + dimt[lnkno1]) % dimt[lnkno1];
                                pos2 = (cur_x - i + dimt[lnkno1]) % dimt[lnkno1];
                                pos3 = (cur_y + i + dimq[lnkno1]) % dimq[lnkno1];
                                pos4 = (cur_y - i + dimq[lnkno1]) % dimq[lnkno1];

                                if ((ptqlink->potential[pos1][pos3] > 0)
                                        && (ptqlink->potential[pos1][pos3] < MAXSHORTINT))
                                {
                                        found = true;
                                        result->d[0].t = pos1;
                                        result->d[0].q = pos3; 
                                        result->dim = 1; 
                                        break;
                                }
                                
                                if ((ptqlink->potential[pos2][pos3] > 0)
                                        && (ptqlink->potential[pos2][pos3] < MAXSHORTINT)) 
                                {
                                        found = true;
                                        result->d[0].t = pos2;
                                        result->d[0].q = pos3; 
                                        result->dim = 1; 
                                        break;
                                }
                        
                                if ((ptqlink->potential[pos1][pos4] > 0)
                                        && (ptqlink->potential[pos1][pos4] < MAXSHORTINT)) 
                                {
                                        found = true;
                                        result->d[0].t = pos1;
                                        result->d[0].q = pos4; 
                                        result->dim = 1; 
                                        break;
                                }

                                if ((ptqlink->potential[pos2][pos4] > 0)
                                        && (ptqlink->potential[pos2][pos4] < MAXSHORTINT)) 
                                {
                                        found = true;
                                        result->d[0].t = pos2;
                                        result->d[0].q = pos4; 
                                        result->dim = 1; 
                                        break;
                                }

                                if ((ptqlink->potential[pos1][cur_y] > 0)
                                        && (ptqlink->potential[pos1][cur_y] < MAXSHORTINT)) 
                                {
                                        found = true;
                                        result->d[0].t = pos1;
                                        result->d[0].q = cur_y; 
                                        result->dim = 1; 
                                        break;
                                }
                                if ((ptqlink->potential[pos2][cur_y] > 0)
                                        && (ptqlink->potential[pos2][cur_y] < MAXSHORTINT)) 
                                {
                                        found = true;
                                        result->d[0].t = pos2;
                                        result->d[0].q = cur_y; 
                                        result->dim = 1; 
                                        break;
                                }
                                if (found) {
                                        break;  
                                }
                        }
                        if (!found) 
                        {
                                result->d[0].t = tqinitial->x;
                                result->d[0].q = tqinitial->y;
                                result->dim = 1;
                        }
                        
                        // remove the block wall                
                        ptqlink->bitmap[wallpos] = wtmp;        
                        
                        return(false);
                        
                } //end of else

}//end of outer if

        // Find path by following heuristic     
        find_path_by_heuristic(result, ptqlink->potential,
                dimt[lnkno1], dimq[lnkno1], tqinitial, tqfinal);
        
        // remove the block wall                
        ptqlink->bitmap[wallpos] = wtmp;

        return(true);

} // find_main_path()   


//=========================================================================================
//PL_Sequential::find_forward_extension()
//inputs:       - int lnkno
//                      - IntPoint *tqinitial
//                      - IntPoint *tqfinal
//                      - TQLinkCSpace *ptqlink
//                  - LinkPath *result
//function: computes extension from goal to collision-free cspace of link n 
//=========================================================================================
void PL_Sequential::find_forward_extension(int  lnkno, 
                                                                IntPoint        *tqinitial, 
                                                                IntPoint        *tqfinal, 
                                                                TQLinkCSpace    *ptqlink, 
                                                                LinkPath        *result)
{
        int     i, j, lnkno1, changesign, found, t, q;
        unsigned char   *wtmp;
        IntPoint        tqp1, fstart;
        int narrow_flag1 = 0;

        lnkno1 = lnkno - 1 ;    // array indexing convension
        
        // Block the possible circle in forward path    
        wtmp = ptqlink->bitmap[(tqinitial->x+1)%dimt[lnkno1]];
        ptqlink->bitmap[(tqinitial->x+1)%dimt[lnkno1]] = wall;

        // Computing the distance map and voronoi diagram in TxQ cspace 
        distance2d(ptqlink->bitmap, dimt[lnkno1], dimq[lnkno1],
                        ptqlink->distmap, ptqlink->voro);
  
        // compute the heuristics in the TQ space       
        voro_heuristic2d(ptqlink->bitmap, dimt[lnkno1], 
                        dimq[lnkno1], *tqfinal,
                        ptqlink->distmap,  ptqlink->voro, 
                        ptqlink->potential, &narrow_flag1);

        // remove the block             
        ptqlink->bitmap[(tqinitial->x+1)%dimt[lnkno1]] = wtmp;

        // determine the forward end            
        //if (ptqlink->t_bound)
                tqp1.x = dimt[lnkno1]-1;
        //else
        //      tqp1.x = tqinitial->x;

        if (tqinitial->y < tqfinal->y) {
                changesign = -1;
                if (joint_limit[lnkno-1].status)
                        tqp1.y = joint_limit[lnkno-1].end;
                else
                        tqp1.y = dimq[lnkno1]-1;
        }
        else {
                changesign = 1;
                if (joint_limit[lnkno-1].status) 
                        tqp1.y = joint_limit[lnkno-1].start;
                else
                        tqp1.y = 0;
        }

        // determine the false start point              
        found = FALSE;
        i=0;    
        do {
                for (j = i; j >= 0; j--) {
                        t = (tqp1.x - j + dimt[lnkno1]) % dimt[lnkno1];
                        q = (tqp1.y + ((i-j)*changesign) + dimq[lnkno1]) % dimq[lnkno1];
                        if ( ( ptqlink->potential[t][q] > SAFETY_DISTANCE)
                                && ( ptqlink->potential[t][q] != MAXSHORTINT))
                        {       
                                found = TRUE; 
                                break; 
                        }
                }
                i++;
        } while (!found);

        fstart.x = t; fstart.y = q;

        // Find path by following heuristic     
        find_path_by_heuristic(result, ptqlink->potential,
                        dimt[lnkno1], dimq[lnkno1], tqfinal, &fstart);
        
} // find_forward_extension()   



//=========================================================================================
//PL_Sequential::find_backward_extension()
//inputs:       - int lnkno
//                      - IntPoint *tqinitial
//                      - IntPoint *tqfinal
//                      - TQLinkCSpace *ptqlink
//                  - LinkPath *result
//function: computes extension from start to collision-free cspace of link n 
//=========================================================================================
void PL_Sequential::find_backward_extension(    int                     lnkno, 
                                                                IntPoint        *tqinitial, 
                                                                IntPoint        *tqfinal, 
                                                                TQLinkCSpace    *ptqlink, 
                                                                LinkPath        *result)
{
        int     i, j, lnkno1, changesign, found, t, q;
        unsigned char   *wtmp;
        IntPoint        tqp1, fstart;
        int narrow_flag2 = 0;

        lnkno1 = lnkno - 1 ;
        
        // Block the posible circle in forward path     
        wtmp = ptqlink->bitmap[(tqfinal->x+dimt[lnkno1]-1)%dimt[lnkno1]];
        ptqlink->bitmap[(tqfinal->x+dimt[lnkno1]-1)%dimt[lnkno1]] = wall;

        // Computing the distance map and voronoi diagram in TxQ cspace 
        distance2d(ptqlink->bitmap, dimt[lnkno1], dimq[lnkno1],
                        ptqlink->distmap, ptqlink->voro);
  
        // compute the heuristics in the TQ space       
        voro_heuristic2d(ptqlink->bitmap, dimt[lnkno1], 
                        dimq[lnkno1], *tqinitial, 
                        ptqlink->distmap,  ptqlink->voro, 
                        ptqlink->potential, &narrow_flag2);

        // remove the block             
        ptqlink->bitmap[(tqfinal->x+dimt[lnkno1]-1)%dimt[lnkno1]] = wtmp;

        // determine the backward end           
        //if (ptqlink->t_bound)
                tqp1.x = 0;
        //else
        //      tqp1.x = tqfinal->x;

        if (tqinitial->y < tqfinal->y) {
                changesign = 1;
                if (joint_limit[lnkno1].status)
                        tqp1.y = joint_limit[lnkno1].start;
                else
                        tqp1.y = 0;
        }
        else {
                changesign = -1;
                if (joint_limit[lnkno1].status)
                        tqp1.y = joint_limit[lnkno1].end;
                else
                        tqp1.y = dimq[lnkno1]-1;
        }

        // determine the false start point              
        found = FALSE;
        i=0;    
        do {
                for (j=0; j<=i; j++) {
                        t = (tqp1.x + j)%dimt[lnkno1];
                        q = (tqp1.y + (i-j)*changesign +dimq[lnkno1])%dimq[lnkno1];
                        if ( ( ptqlink->potential[t][q] > SAFETY_DISTANCE)
                                && ( ptqlink->potential[t][q] != MAXSHORTINT))
                                { found = TRUE; break; }
                }
                i++;
        } while (!found);

        fstart.x = t; fstart.y = q;

        // Find path by following heuristic     
        find_path_by_heuristic(result, ptqlink->potential,
                        dimt[lnkno1], dimq[lnkno1], &fstart, tqinitial);
        
} // find_backward_extension()  



//=========================================================================================
//PL_Sequential::combine_three_paths()
//inputs:       - LinkPath *combpath
//                      - LinkPath *mainpath
//                      - LinkPath *frwdpath
//                      - LinkPath *bkwdpath
//                      - int *iposition
//                      - int *fposition
//function: combine backward-extended, main, and forward-extended paths for link n
//=========================================================================================
void PL_Sequential::combine_three_paths(        LinkPath                *combpath, 
                                                        LinkPath                *mainpath, 
                                                        LinkPath                *frwdpath, 
                                                        LinkPath                *bkwdpath,
                                                        int                             *iposition, 
                                                        int                             *fposition)
{
        int     dim, i;
   
        dim = 0;

        // Backward path        
        for (i=0; i<bkwdpath->dim-1; i++) {
                combpath->d[dim].t = bkwdpath->d[i].t;
                combpath->d[dim].q = bkwdpath->d[i].q;
                dim++;
        }

        *iposition = dim;       // record initial position index        

        // Main path, ignore duplicate  
        for (i=0; i<mainpath->dim; i++) {
                combpath->d[dim].t = mainpath->d[i].t;
                combpath->d[dim].q = mainpath->d[i].q;
                dim++;
        }

        *fposition = dim-1;     // record final position index          

        // Forward path 
        for (i=frwdpath->dim-2; i>=0; i--) {
                combpath->d[dim].t = frwdpath->d[i].t;
                combpath->d[dim].q = frwdpath->d[i].q;
                dim++;
        }

        combpath->dim = dim;

        // Check path length limit      
        if (combpath->dim > combpath->maxdim)
                printf("***** Warning, Combined path limit over.*****\n");

} //combine_three_paths()       



//=========================================================================================
//void PL_Sequential::distance2d()
//inputs:       - unsigned char **in
//                      - int dimx
//                      - int dimy
//                      - short int **V
//                      - unsigned char **voro
//function: computes distance map and voronoi map 
//=========================================================================================
void PL_Sequential::distance2d(unsigned char **in,      int dimx, int dimy,     short int **V, 
                                                           unsigned char **voro)
{
        int i, dimx1, dimy1;
        register int x,y;
        Int2DPoint *FRONT,*NEXT,*TEMP;
        Int2DPoint *MFRONT, *MNEXT;
        int nbfront = 0, nbnext = 0 , nbskele = 0;
        Int2DPoint ***pere,*pepere; 
        Int2DPoint *Mpepere; 
        int px, py;

        dimx1 = dimx - 1;       // array indexing convension
        dimy1 = dimy - 1;       // All (x-1)%dimx and (y-1)%dimy are replaced by
                                                // (x+dimx1)%dimx and (y+dimy1)%dimy               

        // Cells in the current distance-wave front will be stored in FRONT, the 
        // next-front cells (neighbors of FRONT's cells with distance greater by 1) 
        // will be stored in NEXT 

        MFRONT= FRONT = (Int2DPoint *)malloc(100*(dimx+dimy)*sizeof(Int2DPoint));
        MNEXT= NEXT  = (Int2DPoint *)malloc(100*(dimx+dimy)*sizeof(Int2DPoint));

        // To detect collision of two "distinct" L1-distance fronts (which determine 
        // a voronoi cell), the (x,y) coordinates of every "source" cell (one whose 
        // L1 distance is 1) are recorded in pepere[]. Then, every cell encountered 
        // by the wave originated at this cell gets a pointer (stored in pere[][]) 
        // to the cell in pere[] containing the (x,y) coordinates of the source cell 
        // (note that pepere[] stores only source cells).
        //
        Mpepere= pepere = (Int2DPoint *)malloc(100*(dimx+dimy)*sizeof(Int2DPoint));
        pere = (Int2DPoint ***)malloc2D(dimx,dimy,sizeof(Int2DPoint *));

        // Put '1' in any obstacle-free workspace cell that has a 1-neighbor which
        // is an obstacle cell, store its (x,y) coordinates in pepere[], put a pointer
        // to this pepere[] cell in pere[][].
        
        for(x=0;x<dimx;++x) {
                for(y=0;y<dimy;++y) {
                        if(in[x][y] == OBSTACLE)
                                {
                                  V[x][y] = 0;
                                  continue;
                                }
                        if(in[(x+1)%dimx][y] == OBSTACLE || 
                           in[(x+dimx1)%dimx][y] == OBSTACLE ||
                           in[x][(y+1)%dimy] == OBSTACLE ||
                           in[x][(y+dimy1)%dimy] == OBSTACLE) 
                        {
                                V[x][y] = 1; 
                                FRONT[nbfront].x = pepere->x = x;
                                FRONT[nbfront].y = pepere->y = y; 
                                nbfront++;
                pere[x][y] = pepere++; 
                        } 
                        else {
                                V[x][y] = MAXSHORTINT;
                                // free cells whose L1 distance * is > 1 
                        }
                }
        } //end of outer for

        // There are nbfront cells marked by '1' in V[][], their coordinates were 
        // chronologically stored in FRONT[].

        for(i = 0; i < nbfront; ++i){
                x = FRONT[i].x;
                y = FRONT[i].y;

            if(in[(x+1)%dimx][y] == OBSTACLE ) 
                pere[(x+1)%dimx][y] = pere[x][y];

                if(in[(x+dimx1)%dimx][y] == OBSTACLE ) 
            pere[(x+dimx1)%dimx][y] = pere[x][y];

            if(in[x][(y+1)%dimy] == OBSTACLE ) 
                pere[x][(y+1)%dimy] = pere[x][y];

                if(in[x][(y+dimy1)%dimy] == OBSTACLE ) 
            pere[x][(y+dimy1)%dimy] = pere[x][y];

            if(in[(x+1)%dimx][(y+1)%dimy] == OBSTACLE ) 
            pere[(x+1)%dimx][(y+1)%dimy] = pere[x][y];

            if(in[(x+1)%dimx][(y+dimy1)%dimy] == OBSTACLE ) 
                pere[(x+1)%dimx][(y+dimy1)%dimy] = pere[x][y];

                if(in[(x+dimx1)%dimx][(y+1)%dimy] == OBSTACLE ) 
                        pere[(x+dimx1)%dimx][(y+1)%dimy] = pere[x][y];

            if(in[(x+dimx1)%dimx][(y+dimy1)%dimy] == OBSTACLE ) 
                pere[(x+dimx1)%dimx][(y+dimy1)%dimy] = pere[x][y];
        }

        // This is the main loop. Each of FRONT's cells, (x,y), acts as a wave source.
        // Its 1-neighbors are explored, the ones which are obstacle-free and unmarked
        // are marked by V[x][y]+1 and added to NEXT. The coordinates of their
        // original source are recorded in pere[][]. If a neighbor is already marked,
        // the neighbor's source cell is compared to the source of the current cell.
        // If the two sources are "sufficiently" apart, the neighbor is added to the
        // voronoi diagram. This process is repeated on NEXT's cells
 
        for (x = 0; x < dimx; x++)   // initialize the voronoi diagram  
        {
                for (y = 0; y < dimy; y++)
                {
                        voro[x][y] = 0;
                }
        }
        
        while(nbfront > 0) {
        nbnext = 0;
   
                for(i = 0; i < nbfront; ++i) 
                {
                  x = FRONT[i].x;
          y = FRONT[i].y;

                  px = pere[x][y]->x;
          py = pere[x][y]->y;

                  if(V[(x+1)%dimx][y] >= MAXSHORTINT )  {
                V[(x+1)%dimx][y] = V[x][y] + 1;
                nbnext++;
                NEXT[nbnext-1].x = (x+1)%dimx;
                NEXT[nbnext-1].y = y;
            pere[(x+1)%dimx][y] = pere[x][y]; 
                  }
                  else if(ABS(pere[(x+1)%dimx][y]->x - px) + ABS(pere[(x+1)%dimx][y]->y - py)> 3) {
                        voro[(x+1)%dimx][y] = 1; 
                        nbskele++;
                  }
          if(V[(x+dimx1)%dimx][y] >= MAXSHORTINT )  {
                V[(x+dimx1)%dimx][y] = V[x][y] + 1;
                nbnext++;
                NEXT[nbnext-1].x = (x+dimx1)%dimx;
                NEXT[nbnext-1].y = y;
                pere[(x+dimx1)%dimx][y] = pere[x][y];
                  } 
                  else if(ABS(pere[(x+dimx1)%dimx][y]->x - px) + ABS(pere[(x+dimx1)%dimx][y]->y - py)> 3) {
                        voro[(x+dimx1)%dimx][y] = 1; 
                        nbskele++;
                  }
                  if(V[x][(y+1)%dimy] >= MAXSHORTINT )  {
                V[x][(y+1)%dimy] = V[x][y] + 1;
                nbnext++;
                NEXT[nbnext-1].x = x;
                NEXT[nbnext-1].y = (y+1)%dimy;
                pere[x][(y+1)%dimy] = pere[x][y];
                  } 
                  else if(ABS(pere[x][(y+1)%dimy]->x - px) + ABS(pere[x][(y+1)%dimy]->y - py)> 3) { 
                        voro[x][(y+1)%dimy] = 1;
                        nbskele++;
                  }

                  if(V[x][(y+dimy1)%dimy] >= MAXSHORTINT )  {
                V[x][(y+dimy1)%dimy] = V[x][y] + 1;
                nbnext++;
                NEXT[nbnext-1].x = x;
                NEXT[nbnext-1].y = (y+dimy1)%dimy;
                pere[x][(y+dimy1)%dimy] = pere[x][y];
                  } 
                  else if(ABS(pere[x][(y+dimy1)%dimy]->x - px) + ABS(pere[x][(y+dimy1)%dimy]->y - py)> 3) {
                        voro[x][(y+dimy1)%dimy] = 1;
                        nbskele++;
                  }
                }

                TEMP = FRONT;
                FRONT = NEXT;
                NEXT = TEMP;

                nbfront = nbnext;
        }

        free2D( ( void** )pere );
        free( Mpepere );
        free( MNEXT );
        free( MFRONT );

} // distance2d() 



//=========================================================================================
//void PL_Sequential::voro_heuristic2d()
//inputs:       - unsigned char **in
//                      - int dimx
//                      - int dimy
//                      - IntPoint goal
//                      - short int **V
//                      - unsigned char **voro
//                      - short int     **heurpot
//                      - int *flag
//function: constructs heuristic map corresponding to voronoi map
//=========================================================================================
void PL_Sequential::voro_heuristic2d(unsigned char              **in, 
                                                                         int                            dimx, 
                                                                         int                            dimy, 
                                                                         IntPoint                       goal, 
                                                                         short int                      **V, 
                                                                         unsigned char          **voro, 
                                                                         short int                      **heurpot, 
                                                                         int                            *flag)
{
        int k, l, dimx1, dimy1;
        int x, y, i;
        int nbvoid, nbfront, nbsavofront, nbsavonext;
        IntPoint **FRONT, *PFRONT, *SAVOFRONT, *SAVONEXT, *TEMP;
        IntPoint **MFRONT, *MPFRONT, *MSAVOFRONT, *MSAVONEXT; 
        int xnew,ynew;
        short int value;
        short int Vmax;
        int narrow_flag = 0;

        dimx1 = dimx - 1;       // array indexing convension
        dimy1 = dimy - 1;

        //allocates memory for local arrays
        MFRONT = FRONT = (IntPoint **)calloc(50*(dimx+dimy),sizeof(IntPoint *));
        MPFRONT = PFRONT = (IntPoint *)calloc(50*(dimx+dimy),sizeof(IntPoint));

        MSAVOFRONT = SAVOFRONT = (IntPoint *)calloc(50*(dimx+dimy),sizeof(IntPoint));
        MSAVONEXT = SAVONEXT = (IntPoint *)calloc(50*(dimx+dimy),sizeof(IntPoint));

    // First mark by 'MAXSHORTINT' the obstacle cells in heupot[][]
        // and count the number of free cells 
        nbvoid = 0;
        for ( x=0 ; x < dimx ; ++x ) {
                for ( y=0 ; y < dimy ; ++y ) {
                        if(in[x][y] == OBSTACLE) {
                                heurpot[x][y] = MAXSHORTINT;
                        }
                else {
                                heurpot[x][y] = UNDONE;
                                nbvoid++;
                        }
                }
        }

        // assign goal in heuristic map to be 0
        heurpot[goal.x][goal.y] = 0;
        *flag = narrow_flag; 

    // proceed from the goal along increasing L1 distance from the 
    // obstacles until the voronoi diagram is met 
        x = goal.x;
        y = goal.y;

        if(voro[x][y] == 0) {
            narrow_flag = 1;
        }

        // try to connect the goal to the voronoi diagram.
        while(voro[x][y] == 0) {
                Vmax = V[x][y];

                xnew = x;
                ynew = y;
 
                for ( k = dimx-1 ; k <= dimx+1 ; ++k){
                    for ( l = dimy-1 ; l <= dimy+1 ; ++l){
                                if ( Vmax < V[(x+k)%dimx][(y+l)%dimy] ) {
                                    Vmax = V[(x+k)%dimx][(y+l)%dimy];
                                    xnew = (x+k)%dimx;
                                    ynew = (y+l)%dimy;
                                }
                    }
                }
                if ((xnew == x) && (ynew == y)) {
                    if(narrow_flag == 1) {
                        *flag = narrow_flag;  
                    }                           
                }
                voro[x][y] = 1;         // record path goal->voronoi 
                x = xnew; y = ynew; 
        } //end of while
    
        FRONT[1] = PFRONT++;
        FRONT[1]->x = goal.x;
        FRONT[1]->y = goal.y;
        nbfront = 1;

        // Compute heurpot[][] along the voronoi diagram. Starting at the goal,
        // "expand" the visited voronoi cell with the largest L1 distance from the 
        // obstacles. This heuristic creates a (discontinuous) saddle at the narrowest
        // region in the workspace, thus eliminating the most likely local minimum
        // once the actual robot is guided "along" this potential. 
        // Remark: It seems that this heuristic can be greatly improved. It is possible
        // to store the voronoi cells in several disjoint heaps, one for each 
        // workspace obstacle, and then to "expand" cells obstacle by obstacle. 
        
        // If the goal is in a narrow region, then it won't be connected to the
        // voronoi diagram. Then, none of the voronoi cells will be assigned a
        // potential value. 

        x = goal.x;
        y = goal.y;
 
        nbsavofront = 0;
        while(nbfront > 0) {
                SAVOFRONT[nbsavofront].x = FRONT[1]->x; // Save all the voronoi cells for the next paragraph 
                SAVOFRONT[nbsavofront].y = FRONT[1]->y;
                nbsavofront++;
            nbfront = deletemin(x, y, FRONT, nbfront, V); // (+,+) Get (x,y) with the largest L1 distance 
                value = heurpot[x][y] + 1;
        // Add to the heap all unmarked neighbors on the voronoi diagram 
                for (k = dimx-1 ; k <= dimx+1 ; ++k) {
                for (l = dimy-1 ; l <= dimy+1 ; ++l) {                          
                if ( voro[(x+k)%dimx][(y+l)%dimy] == 0 ) 
                                        continue;
                if ( heurpot[(x+k)%dimx][(y+l)%dimy] == UNDONE ) {
                                        heurpot[(x+k)%dimx][(y+l)%dimy] = value;
                                nbfront = insert((x+k)%dimx, (y+l)%dimy, FRONT, PFRONT, nbfront, V);
                                }
                        }
                }
        } // end of while 

        // follows the expansion from the voronoi diagram to the remaining free
        // workspace. We proceed by simultounasly advancing an L1-distance "wave" 
        // from the voronoi diagram. All the voronoi cells are stored in SAVOFRONT. 

        // In the case of the narrow region, the following while loop will still
        // try to expand the wave from goal and assign the potential values to 
        // the neighbors of the goal until the whole free space where the goal is
        // is explored. If the initial is in the same free space as the goal is,
        // a path exists. Otherwise, backtracking is needed.
        
        while (nbsavofront) {
                nbsavonext = 0;
        // Assign cost to the 1-neighbors of all the nbsavofront 
        // cells stored in SAVOFRONT. 
                for (i = 0 ; i < nbsavofront ; ++i) {

                        x = SAVOFRONT[i].x;
                        y = SAVOFRONT[i].y;

                        value = heurpot[x][y] + 1;

                        if(heurpot[(x+1)%dimx][y] == UNDONE){
                                nbsavonext++;
                                heurpot[(x+1)%dimx][y] = value;
                                SAVONEXT[nbsavonext-1].x = (x+1)%dimx;
                                SAVONEXT[nbsavonext-1].y = y;
                        }

                if(heurpot[(x+dimx1)%dimx][y] == UNDONE){
                                nbsavonext++;
                                heurpot[(x+dimx1)%dimx][y] = value;
                                SAVONEXT[nbsavonext-1].x = (x+dimx1)%dimx;
                                SAVONEXT[nbsavonext-1].y = y;
                }

                if(heurpot[x][(y+1)%dimy] == UNDONE){
                                nbsavonext++;
                                heurpot[x][(y+1)%dimy] = value;
                                SAVONEXT[nbsavonext-1].x = x;
                                SAVONEXT[nbsavonext-1].y = (y+1)%dimy;
                }

                if(heurpot[x][(y+dimy1)%dimy] == UNDONE){
                                nbsavonext++;
                                heurpot[x][(y+dimy1)%dimy] = value;
                                SAVONEXT[nbsavonext-1].x = x;
                                SAVONEXT[nbsavonext-1].y = (y+dimy1)%dimy;
                }
                } // "expanded" all the cells in SAVOFRONT 

                TEMP = SAVOFRONT;
                SAVOFRONT = SAVONEXT;
                SAVONEXT = TEMP;

                nbsavofront = nbsavonext;
        } // visited all the free workspace cells 

        // Free all memory allocated 
        free(MFRONT);
        free(MPFRONT);
        free(MSAVOFRONT);
        free(MSAVONEXT); 

} // voro_heuristic() 



//=========================================================================================
//PL_Sequential::find_path_by_heuristic()
//inputs:       - LinkPath *result
//                      - short int **heurpot
//                      - int dimx
//                      - int dimy
//                      - IntPoint *start
//                      - IntPoint *goal
//function: find main path for link n along heuristic map
//=========================================================================================
void PL_Sequential::find_path_by_heuristic(     LinkPath                *result, 
                                                                short int       **heurpot, 
                                                                int                     dimx, 
                                                                int                     dimy, 
                                                                IntPoint                *start, 
                                                                IntPoint                *goal)
{
        int     value, x, y, xnew, ynew, i, found;

        // Start from the initial position      
        x = start->x;
        y = start->y;
        value = heurpot[x][y];
        result->d[0].t = start->x;
        result->d[0].q = start->y;
        i = 1;

        // Search for the path by following the gradient of heurpot
        while ((heurpot[x][y] > 1) && (i < result->maxdim)) {
                for ( xnew = x+dimx-1 ; xnew <= x+dimx+1 ; xnew++ ) {
                        found = FALSE;
                        for ( ynew = y + dimy - 1 ; ynew <= y + dimy + 1; ynew++ )
                        {
                                if (heurpot[xnew % dimx][ynew % dimy] < value) {
                                        //if a neighbor's heuristics lower, find a new point    
                                        result->d[i].t = x = xnew % dimx;
                                        result->d[i].q = y = ynew % dimy;
                                        i++;
                                        found = TRUE;
                                        // prepare for next following   
                                        value = heurpot[xnew % dimx][ynew % dimy];
                                        break;
                                }
                        }       
                        if (found) break;
                }
        }

        // Check maximum dimension overflow in result path      
        if (i >= result->maxdim)
                printf("**** Warning, Path is not correct, overflow ! ****\n");

        // Add the final position on the path   
        if (heurpot[x][y] > 0) {
                result->d[i].t = goal->x;
                result->d[i].q = goal->y;
                i++;
        }

        // record dimension of mainpath
        result->dim = i;  

} // find_path_by_heuristic()   



//=========================================================================================
//void PL_Sequential::retrieve_global_path()
//inputs:       - double **globalpath
//                      - int pathlen
//                      - double *startdof
//                      - double *goaldof
//function: converts link paths from t-q array into absolute angles in radian
//=========================================================================================
void PL_Sequential::retrieve_global_path(double **globalpath, int pathlen,
                                                                                 double *startdof, double *goaldof)
{
        int     i, j, lnkno1;
        TQConfig        para[MAX_DEGREE];
        
        lnkno1 = arm_degree - 1;                // array indexing convension
        
        // First config is start node 
        for (j = 0; j < arm_degree; j++)
            globalpath[0][j] = startdof[j];

        // For every column of TxQ map  
        for (i = 0; i < pathlen-2; i++) {
                // retrieve q for link (n - 1)  
                para[lnkno1].q = lnkcps[lnkno1].d[i].q;
                para[lnkno1].t = lnkcps[lnkno1].d[i].t;

                // retrieve q for link (n - 2)...1      
                for (j = lnkno1-1; j >= 0; j--) {
                        para[j].t = lnkcps[j].d[ para[j+1].t].t; 
                        para[j].q = lnkcps[j].d[ para[j+1].t].q;
                }

                // convert index to angle and store into global path    
                for (j = 0; j < arm_degree; j++) {
                        globalpath[i+1][j] = index_to_rad(para[j].q); 
                }
        }

        // append goal to last point of globalpath 
        for (j = 0; j < arm_degree; j++) 
        {       
                globalpath[pathlen-1][j] = goaldof[j];
        }
            
} //retrieve_global_path()



//=========================================================================================
//void PL_Sequential::reparameterize_path()
//inputs:       - int bound
//                      - LinkPath *whole_path
//                      - LinkPath *result
//                      - int *ipos
//                      - int *fpos
//                      - int L1
//function: reparameterize path of link n in terms of t of link (n+1)
//=========================================================================================
void PL_Sequential::reparameterize_path(int             bound,
                                                                        LinkPath        *whole_path, 
                                                                        LinkPath        *result, 
                                                                        int             *ipos, 
                                                                        int             *fpos, 
                                                                        int             L1)
{
        double  lold, lnew;
        int     i, j;

        if (bound)
                lnew = dimt[L1]-3;
        else
                lnew = dimt[L1]-1;
        lold = whole_path->dim-1;

        if (bound) {
                (*ipos)++;      // shift 1 for the initial and final position
                (*fpos)++;
                result->d[0].t = whole_path->d[0].t;
                result->d[0].q = whole_path->d[0].q;
                result->d[dimt[L1]-1].t = whole_path->d[whole_path->dim-1].t;
                result->d[dimt[L1]-1].q = whole_path->d[whole_path->dim-1].q;
                for (i=1; i<dimt[L1]-1; i++) {
                        if (NO_PATH_COMPRESS) j = i-1;
                        else j = ROUND( lold*(i-1)/lnew);
                        result->d[i].t = whole_path->d[j].t;
                        result->d[i].q = whole_path->d[j].q;
                }
        }
        else
                for (i =0; i<dimt[L1]; i++) {
                        if (NO_PATH_COMPRESS) j = i;
                        else j = ROUND(lold * (i-1) / lnew);
                        result->d[i].t = whole_path->d[j].t;
                        result->d[i].q = whole_path->d[j].q;
                }

        result->dim = dimt[L1];

} //reparameterize_path()       



//=========================================================================================
//void PL_Sequential::allocate_space_for_Link_tq_path()
//input:        - int link1
//                      - int length
//function: initializes constants for link path; allocates memory for link path array
//=========================================================================================
void PL_Sequential::allocate_space_for_Link_tq_path(int link1, int length)

{
        lnkcps[link1].maxdim = length;
        lnkcps[link1].dim = length;
        lnkcps[link1].d = (TQConfig *) malloc (length * sizeof(TQConfig));
        assert( lnkcps[link1].d != NULL );
} //allocate_space_for_Link_tq_path()


//=========================================================================================
//void PL_Sequential::free_space_for_link_cs()
//input: int L
//function: free space allocated for link n's obstacle map, distance map, voronoi map, and
//                  potential map
//=========================================================================================
void PL_Sequential::free_space_for_link_cs(int L)

{
        // Obstacle map 
        if (tqcs.tqlink[L].bitmap)
                free2D((void **)tqcs.tqlink[L].bitmap);

        // Distance map 
        if (tqcs.tqlink[L].distmap)
                free2D((void **)tqcs.tqlink[L].distmap );

        // Voronoi map  
        if (tqcs.tqlink[L].voro)
                free2D((void **)tqcs.tqlink[L].voro );

        // Potential map        
        if (tqcs.tqlink[L].potential)
                free2D((void **)tqcs.tqlink[L].potential );
} //free_space_for_link_cs()



//=========================================================================================
//void PL_Sequential::allocate_space_for_link_cs()
//input: int L
//function: allocate space for link n's obstacle map, distance map, voronoi map, and
//                  potential map
//=========================================================================================
void PL_Sequential::allocate_space_for_link_cs(int L)
{

        tqcs.tqlink[L].dimt = dimt[L];
        tqcs.tqlink[L].dimq = dimq[L];

        // Obstacle map 
        tqcs.tqlink[L].bitmap = (unsigned char **)
                        malloc2D(dimt[L], dimq[L], sizeof(unsigned char));
        assert( tqcs.tqlink[L].bitmap != NULL );

        tqcs.tqlink[L].distmap = (short int **)
                malloc2D(dimt[L], dimq[L], sizeof(short int));
        assert( tqcs.tqlink[L].distmap != NULL );

        // Voronoi map  
        tqcs.tqlink[L].voro = (unsigned char **)
                malloc2D(dimt[L], dimq[L], sizeof(unsigned char));
        assert( tqcs.tqlink[L].voro != NULL );
        
        // Potential map        
        tqcs.tqlink[L].potential = (short int **)
                malloc2D(dimt[L], dimq[L], sizeof(short int));
        assert( tqcs.tqlink[L].potential != NULL );
        
} //allocate_space_for_link_cs()



//=========================================================================================
//void PL_Sequential::allocate_space_for_working_tq_path()
//input: none
//function: allocates space for links' backward extention, main, forward extention, and
//                  combined paths
//=========================================================================================
void PL_Sequential::allocate_space_for_working_tq_path()
{
        // allocate space for working pathes in each link planning stage    
        bkwdpath.maxdim = MAX_PART_PATH;
        bkwdpath.d = (TQConfig *)malloc(bkwdpath.maxdim*sizeof(TQConfig));
        assert( bkwdpath.d != NULL );

        frwdpath.maxdim = MAX_PART_PATH;
        frwdpath.d = (TQConfig *)malloc(frwdpath.maxdim*sizeof(TQConfig));
        assert( frwdpath.d != NULL );

        mainpath.maxdim = MAX_PART_PATH;
        mainpath.d = (TQConfig *)malloc(mainpath.maxdim*sizeof(TQConfig));
        assert( mainpath.d != NULL );

        combpath.maxdim = MAX_CS_PATH_LEN;
        combpath.d = (TQConfig *)malloc(combpath.maxdim*sizeof(TQConfig));
        assert( combpath.d != NULL );
} //allocate_space_for_working_tq_path()



//=========================================================================================
//char** PL_Sequential::malloc2D ()
//inputs:       - int dy
//                      - int dz
//                      - int oc
//function: allocates 2D arrays
//=========================================================================================
char** PL_Sequential::malloc2D (int dy,int dz,int oc)
{

        char *bv;
        char **v;
        char *p_bv;
        char **p_v;

        bv = ( char* )malloc( dz * dy * sizeof( char ) * oc );
        assert( bv != NULL );
        v = ( char** )malloc( dy * sizeof( char* ) );
        assert ( v != NULL );
        
        p_bv = bv;
        p_v = v;

        for( p_bv = bv; p_bv - bv < dz * dy * oc; p_bv += dz * oc )
        {
                *p_v++ = p_bv;
        }

        return(v);
} // malloc2D() 



//=========================================================================================
//void PL_Sequential::write_map()
//inputs:       - char *flnm
//                      - FileType filetype
//                      - char **map
//                      - int dimx
//                      - int dimy
//                      - int bitsize
//function: writes 2D array (map) to file (never used)
//=========================================================================================
void PL_Sequential::write_map(char *flnm, FileType filetype,
                char **map, int dimx, int dimy, int bitsize)
{

        FileDescript    fdt;
        int     i;
        FILE *fout;

        fout = fopen(flnm, "w");
        assert( fout != NULL );

        fdt.ftype = filetype;
        i=0;
        while ( (fdt.comment[i]=flnm[i]) != '\0' ) i++;

        fdt.octet = bitsize;
        fdt.dimx = dimx;
        fdt.dimy = dimy;
        fdt.dimz = 0;

        fwrite(&fdt, sizeof(FileDescript), 1, fout);
        fwrite(&(map[0][0]), bitsize, dimx*dimy, fout);

        fclose(fout);

} //write_map()



//=========================================================================================
//void PL_Sequential::write_global_path()
//inputs:       - char *flnm
//                      - int length
//                      - int degree
//                      - double **gpath
//functions: outputs globalpath to file (never used)
//=========================================================================================
void PL_Sequential::write_global_path(char *flnm, int length, int degree, double **gpath)
{
        int     i, j;
        FILE    *fout;

    fout=fopen(flnm, "w");
        assert( fout != NULL );

    fprintf(fout, "%d\n",degree);
    fprintf(fout, "%d\n",length);

    for (i = 0; i < length; i++) {
        fprintf(fout, "  Step %3d:\t", i); 
        for (j = 0; j < degree; j++) {
            fprintf(fout, "%8.3f  ",gpath[i][j]);
        }
        fprintf(fout,"\n");
    }
    fprintf(fout,"\n");
    fclose(fout);
} // write_global_path()


        
//=========================================================================================
//int PL_Sequential::smoothpath()
//inputs:       - int nb_degree
//                      - int intervals
//                      - double **dofpath
//                      - int *pathlen
//functions: optimizes and shortens globalpath (never used)
//note: MUST modify line_free() before use!
//=========================================================================================
int PL_Sequential::smoothpath(int nb_degree, int intervals, double **dofpath, int *pathlen)
{
        int path_len, prev_path_len; 
        int pt, radius, delta;
        int intervals2;
        int pt1, pt2 ;
        double factor;

        double  **gpath;         //Storage for the generated straght line 

    gpath = (double **) malloc2D(*pathlen, arm_degree,
                                sizeof(double) );
        assert( gpath != NULL );

    factor = 1.0;
    path_len = *pathlen;
    prev_path_len=path_len;
    
        // Attempt subdivisions until delta is 2, in which case we are back
    // at the unsmoothed path.
 
    do {    // while (delta > 2)    

        delta = intervals / 3;
        pt = delta;

        // Do every segment on the path   
        do {    // while (pt < path_len)    
            
                        // radius is the segment len of subdivision,
            radius=(int)((factor*path_len)/2);
            pt1=MAX(0,pt-radius);
            pt2=MIN(path_len-1,pt+radius);
        
                    intervals2 = pt2-pt1;

            // If a straight line segment [pt1,pt2] is collision
            // free, replace the portion of the unsmoothed path
            // from pt1 to pt2 by this line segment.

                        //WARNING: must modify line_free() before use
            if (line_free(dofpath, intervals2, pt1, pt2, gpath, &path_len))
                        {
                path_len = shift_dofpath(dofpath,path_len, gpath, intervals2,
                                                        pt1,pt2);
            }

            pt += delta;
        } while (pt < path_len);

        factor /= 2;
        intervals /= 2;

    } while (delta > 2);

    if ((sp_debug_level > 0) && ( prev_path_len != path_len))
        printf("Global Path shorten from %d to %d\n",
                prev_path_len, path_len);

    free(gpath);
        
    *pathlen = path_len;

    return(path_len);

} //smoothpath()



//=========================================================================================
//void PL_Sequential::write_linkpath()
//inputs:       - char *flnm
//                      - FileType filetype
//                      - char *map
//                      - int dim
//                      - int bitsize
//function: outputs link path to file (never used)
//=========================================================================================
void PL_Sequential::write_linkpath(char *flnm, FileType filetype,
                    char *map, int dim, int bitsize)

{
FileDescript    fdt;
int     i;
FILE    *fout;

    fout = fopen(flnm, "w");
    if ( fout==0) {
        printf("Inside write_linkpath()\n");
        printf("File Open Failed for file %s.\n", flnm);
                assert(false);
    }
 
    fdt.ftype = filetype;
    i=0;
    while ( (fdt.comment[i]=flnm[i]) != '\0' ) i++;
    fdt.octet = bitsize;
    fdt.dimx = dim;
    fdt.dimy = 1;
    fdt.dimz = 0;

    fwrite(&fdt, sizeof(FileDescript), 1, fout);
    fwrite(&(map[0]), bitsize, dim, fout);

    fclose(fout);
} // write_linkpath()    



//=========================================================================================
//void PL_Sequential::write_linkpathtxt()
//inputs:       - char *flnm
//                      - TQConfig *map
//                      - int dim
//function: write text output about link path to file (never used)
//=========================================================================================
void PL_Sequential::write_linkpathtxt(char *flnm, TQConfig *map, int dim)
{
    int i;
    FILE    *fout;


    fout = fopen(flnm, "w");
    if ( fout==0 ) {
        printf("Inside write_linkpathtxt()\n");
        printf("File Open Failed for file %s.\n", flnm);
                assert(false);
    }

    fprintf(fout,"\n\t*** Link %s TxQ Config Path ***\n\n", flnm);
    fprintf(fout,"Index\tT\tQ\t\n");

    for (i=0; i <dim; i++) 
        fprintf(fout,"%d\t%d\t%d\t\n",i,map[i].t, map[i].q);

    fclose(fout);

} //write_linkpathtxt()



//=========================================================================================
//void PL_Sequential::write_tqif()
//inputs:       - char *flnm
//                      - TQConfig tqi
//                      - TQConfig tqf
//function:     outputs starts and goals of links (never used)
//=========================================================================================
void PL_Sequential::write_tqif(char *flnm, TQConfig tqi, TQConfig tqf)
{
        FILE *fout;

    fout = fopen(flnm, "w");
    if ( fout==0 ) {
        printf("Inside write_tqif()\n");
        printf("File Open Failed for file %s.\n", flnm);
                assert(false);
    }

    fwrite(&tqi, sizeof(TQConfig), 1, fout);
    fwrite(&tqf, sizeof(TQConfig), 1, fout);
    fclose(fout);

}  //write_tqif()




//=========================================================================================
//int PL_Sequential::line_free()
//inputs:       - double **dofpath
//                      - int intervals
//                      - int start
//                      - int end
//                      - double **tmppath
//                      - int *path_len
//function: ???
//note: used ONLY if we wish to optimize path
//=========================================================================================
int PL_Sequential::line_free(double **dofpath, int intervals, int start, int end,
                double  **tmppath, int *path_len)
{
        return(true); //dummy return
} // line_free() 



//=========================================================================================
//int PL_Sequential::shift_dofpath()
//inputs:       - double **dofpath
//                      - int pathlen
//                      - double **gpath
//                      - int npathlen
//                      - int pt1
//                      - int pt2
//function: ???
//note: used ONLY if we wish to optimize path
//=========================================================================================
int     PL_Sequential::shift_dofpath(double             **dofpath, 
                                  int                   pathlen, 
                                  double                **gpath, 
                                  int                   npathlen, 
                                  int                   pt1, 
                                  int                   pt2)
{
        int     i, iold, j, pathdiff;
        double diff, min_motion;

        // A minimum motion is required when testing if the neighbor
        // points on the new path are distinct
         
        min_motion = 0.9*(2*PI)/dimq[0];

        // Copy the straight line over the path         
        iold = 1; 
        pathdiff = 0;
        for (i = 1; i < npathlen; i++) {        // Skip first one       
                diff = qdist(dofpath[pt1+iold-1], gpath[i]);
                // if the new point not the same as last one, copy it
                // otherwise, skip it
                 
                if (diff > min_motion) {
                        for (j=0; j<arm_degree; j++) {
                                dofpath[iold+pt1][j] = gpath[i][j];
                        }
                        iold++;
                }
                else {
                        pathdiff++;
                }
        }

        // Shift old path       
        pathdiff = pt2-pt1 - iold;

        if (pathdiff > 0) {
                for (i=pt2; i< pathlen; i++ ) { // Skip first one       
                        for (j=0; j<arm_degree; j++)
                                dofpath[i-pathdiff][j] = dofpath[i][j];
                }
                pathlen = pathlen- pathdiff;
        }

        return(pathlen);
} // shift_dofpath() 



//=========================================================================================
//void PL_Sequential::free2D()
//input: void **pp
//function: free 2D arrays
//=========================================================================================
void PL_Sequential::free2D(void **pp)
{
        if( pp == NULL )
        {
                return;         //tried to free nothing
        }
        if( pp[ 0 ] != NULL ) 
        {
                free(pp[0]);
                pp[0] = NULL;
        }
        free(pp);
        pp = NULL;
} //free2D



//=========================================================================================
//double PL_Sequential::qdist()
//input:        - double *q1
//                      - double *q2
//function: determines distance between two q's
//note: used ONLY with smooth_path()
//=========================================================================================
double PL_Sequential::qdist(double      *q1, double     *q2)
{
int     i;
double  dist;

        dist = 0.0;
        for (i=0; i<arm_degree; i++)
                dist = dist + ABS(q1[i]-q2[i]);

        return(dist);

} //qdist()

  //## end PL_Sequential%3752D78501D0.declarations
//## begin module%3752D78501D0.epilog preserve=yes
//## end module%3752D78501D0.epilog

---