planners/aca/PL_Juan.cpp

Go to the documentation of this file.

//## begin module%376A87F4037E.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%376A87F4037E.cm

//## begin module%376A87F4037E.cp preserve=no
//## end module%376A87F4037E.cp

//## Module: PL_Juan%376A87F4037E; Pseudo Package body
//## Source file: C:\project\mpk\code\Planners\PL_Juan.cpp

//## begin module%376A87F4037E.additionalIncludes preserve=no
//## end module%376A87F4037E.additionalIncludes

//## begin module%376A87F4037E.includes preserve=yes
#include <assert.h>
//## end module%376A87F4037E.includes

// PL_Juan
#include "synchronization/semaphore.h"
#include "PL_Juan.h"
#include "jma_switch.h"
#include "jma_aca_macros.h"             //IMPROVE: these should be c++ function calls
#include <iosfwd>
#include <iostream>

#include "opengl/glos.h"
#include <gl/gl.h>
#include "smoothers\SmootherBase.h"

#define THICKNESS 3
#define WIN_WIDTH  1000
#define WIN_HEIGHT 500
#define EPSILON  1.0e-3

#define NUMBER_OF_EMBRYOS MAX_EMBRYOS
#define BIG_NUMBER 1.0e+37

//IMPROVE: these definitions should be constants, or removed
//## end module%376A87F4037E.additionalDeclarations


// Class PL_Juan 


//=============================================================================
// PL_Juan::PL_Juan
//=============================================================================
PL_Juan::PL_Juan():
    tree( NULL ),
    m_Initialized( false )
{
}

//=============================================================================
// PL_Juan::~PL_Juan
//=============================================================================
PL_Juan::~PL_Juan()
{
    delete tree;
    tree = NULL;
}

//=============================================================================
// PL_Juan::PL_Juan
//=============================================================================
void PL_Juan::Initialize()
{
    if( !m_Initialized )
    {
        tree = new JMA_Roadmap_Tree;
        m_Initialized = true;
    }
}

//## Other Operations (implementation)
bool PL_Juan::Plan ()
{
  //## begin PL_Juan::Plan%929728915.body preserve=yes
        PlannerBase::StartTimer() ;
    Initialize();
        path.Clear() ;
//      StartOutput() ;         //IMPROVE: this is for the output window - shouldnt be here
        if( collisionDetector->IsInterfering( GetStartConfig() ) )
        {
                path.AppendPoint( GetGoalConfig() );
                path.AppendPoint( GetGoalConfig() );
                return false;
        }
        if( collisionDetector->IsInterfering( GetGoalConfig() ) )
        {
                path.AppendPoint( GetStartConfig() ) ;
                path.AppendPoint( GetStartConfig() ) ;
                return false ;
        }

        int new_path[500];
        int new_n_landmarks;
        int n_landmarks;
        int path_found;

//      init_conf.dof = startConfig ;
//      init_conf.dist = 0 ;
//      InitRoadmapTree( &tree, startConfig.DOF(), MAX_EMBRYOS, init_conf );
        Semaphore semaphore( guid );
        semaphore.Lock();
        do
    {
                if (!(path_found = Search( tree ) ) )
                {
                        //=== place a new landmark ===
                        Explore( tree );
                }
                if( HasTimeLimitExpired() == true )
                {
                        return false ;
                }
                semaphore.Unlock();
                semaphore.Lock();
    }while( ( !path_found ) && ( tree->n_nodes < MAX_LANDMARKS ) );
        semaphore.Unlock();


        if( path_found )
    {
                //GetIdsPath( tree, tree->best_father_id, ids_list, &n_landmarks );
                GetIdsPath( tree, tree->n_nodes-1, ids_list, &n_landmarks );
                SimplifyIdsTrajectory( tree, ids_list, n_landmarks, new_path, &new_n_landmarks );

                for( int i = 0; i < new_n_landmarks; i++ )
                {
                        int pointIndex = new_path[ i ] ;
                        Configuration point = tree->node[ pointIndex ].landmark.dof ;
                        path.AppendPoint( point ) ;
                }

        //Configuration embryoPoint = tree->node[ tree->best_father_id ].embryo[ tree->best_embryo_id ].dof;
        //path.AppendPoint( embryoPoint );

                path.AppendPoint( GetGoalConfig() ) ;
        }
        else
    {
                if( path.Size() == 0 )
                {
                        path.AppendPoint( GetStartConfig() ) ;
                }
                return false ;
        }
        
        //verify the outputs
    int size = path.Size();
        for( int i = 0; i < size - 1; ++i )
        {
                Configuration p1 = path[ i ] ;
                Configuration p2 = path[ i + 1 ] ;
                assert( !collisionDetector->IsInterferingLinear( p1, p2 ) ) ;
                if( collisionDetector->IsInterferingLinear( p1, p2 ) )
                {
                        assert( false ) ;
                }
        }

        //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 );
        }
        
        return true ;
  //## end PL_Juan::Plan%929728915.body
}

double PL_Juan::MyDistanceFunc (const Configuration& conf1, const Configuration& conf2) const
{
  //## begin PL_Juan::MyDistanceFunc%929737297.body preserve=yes
  return collisionDetector->DistanceBetween( conf1, conf2 ) ;
  //## end PL_Juan::MyDistanceFunc%929737297.body
}

int  PL_Juan::MyRandomPath (int id_landmark, const int id_embryo, const int n_dof, const JMA_Configuration init_conf, JMA_Configuration* embryo) const
{
  //## begin PL_Juan::MyRandomPath%929737298.body preserve=yes
        Configuration config = GetStartConfig();        //this just initializes the config
        bool isConfigInCollision = collisionDetector->IsInterfering( config ) ;
        assert( isConfigInCollision == false ) ;
        Configuration current = init_conf.dof ;

        //IMPROVE: if there's a collision, find out where, then pick a point inbetween
        //create a random configuration
        for( int j = 0; j < config.DOF(); j++ )
        {
                double jointMax = collisionDetector->JointMax( j ) ;
                double jointMin = collisionDetector->JointMin( j ) ;
                double random01 = (double) switch_random()/BIGEST_RND_NUMBER ;
                double jointVal = jointMin + random01 * ( jointMax - jointMin );
                config[ j ] = jointVal ;
        }
    
        while(  collisionDetector->IsInterferingLinear( current, config ) )
        {
                //choose another configuration
                Configuration intersect = collisionDetector->GetLastIntersection() ;
                config = ( current + intersect ) / 2 ;
        }
        ( *embryo ).dof = config ;
  return(0);
  //## end PL_Juan::MyRandomPath%929737298.body
}

int PL_Juan::SimplifyIdsTrajectory (const JMA_Roadmap_Tree* tree, const int list[], int n_nodes, int new_list[], int* new_n_nodes) const
{
  //## begin PL_Juan::SimplifyIdsTrajectory%929987269.body preserve=yes
        int i,j,k;
        int simplified;
        int couting;

        for(i=0;i<n_nodes;i++)
        new_list[i] = list[i];

        i = 0;
        simplified = FALSE;
        while( i < n_nodes)
        {
                j = n_nodes-1;
                couting = FALSE;
                //while( (j > i+1)/* && !couting */)
                while( (j > i+1) )
                {
                        if (! MyPtpCollision((*tree).node[new_list[i]].landmark.dof,(*tree).node[new_list[j]].landmark.dof))
                        {
                                for(k=j; k< n_nodes; k++)
                                new_list[i+(k-j)+1] = new_list[k];
                                n_nodes = i + 1 + ( n_nodes - j );      //IMPROVE: this is not a persistant assignment
                                simplified = TRUE;
                                break;
                        }
                        j--;
                }
                i++;
        }
        ( *new_n_nodes ) = n_nodes;
        return( simplified );

  //## end PL_Juan::SimplifyIdsTrajectory%929987269.body
}

int PL_Juan::GetNAncestors (const JMA_Roadmap_Tree* tree, const int landmark_id)
{
    if( landmark_id >= tree->n_nodes )
    {
        return 0;
    }
    IJG_Assert( landmark_id < tree->n_nodes );
  
    int id = landmark_id;
    int counter = 0;
    
    while(id != 0)
    {
        IJG_Assert( id < tree->n_nodes );
        ++counter;
        id = tree->node[id].father_id;
    }
    return( counter );
}

int PL_Juan::Search (JMA_Roadmap_Tree* tree) const
{
  //## begin PL_Juan::Search%929987266.body preserve=yes
        Configuration local_minima;
  
        int numberOfNodes = (*tree).n_nodes  ;
        Configuration start = (*tree).node[ numberOfNodes - 1 ].landmark.dof ;
        MyLocalPlanner( start, local_minima );
        return(MyGoalFunc(local_minima));
  //## end PL_Juan::Search%929987266.body
}

int PL_Juan::MyLocalPlanner (const Configuration& start_conf, Configuration& local_minima) const
{
  //## begin PL_Juan::MyLocalPlanner%929987263.body preserve=yes
        int collision=FALSE;
        collision = MyPtpCollision(start_conf, GetGoalConfig() );

        if(!collision)
        {
                local_minima = GetGoalConfig();
        }
        else 
        {
                local_minima = start_conf ;
        }
        return(0);
  //## end PL_Juan::MyLocalPlanner%929987263.body
}

int PL_Juan::MyGoalFunc (const Configuration& config) const
{
  //## begin PL_Juan::MyGoalFunc%929987264.body preserve=yes
        if( config != GetGoalConfig() )
        {
                return false ;
        }
        return TRUE ;
        //IMPROVE: this should use the bool class
  //## end PL_Juan::MyGoalFunc%929987264.body
}

int PL_Juan::MyPtpCollision (const Configuration& start_conf, const Configuration& local_minima) const
{
  //## begin PL_Juan::MyPtpCollision%929987265.body preserve=yes
        int steps = 100 ;

        //IMPROVE: use the built in linear collision checker

        //assign the start and goal configurations
        Configuration s = start_conf ;
        Configuration g = local_minima ;
        Configuration offset = g - s ; //IMPROVE: joint limits mean that it won't be as simple as this
        Configuration current = start_conf ;

        if( collisionDetector->IsInterferingLinear( s, g ) )
        {
                return true ;
        }
        else
        {
                return false ;
        }
  //IMPROVE: all these functions should simply call the c function, that way less compilation is needed
  //## end PL_Juan::MyPtpCollision%929987265.body
}

void PL_Juan::InitRoadmapTree (JMA_Roadmap_Tree* tree, const int n_dof, const int n_embryos, const JMA_Configuration init_conf) const
{
    int i;
    double dist;
    
    tree->N_dof = n_dof;
    tree->n_nodes = 1;
    tree->max_dist = 0.0;
    tree->last_max_dist = -1.0;
    tree->best_father_id = 0;
    
    tree->node[0].N_embryos = n_embryos;
    tree->node[0].father_id = NIL;
    
    tree->node[0].landmark.dof = init_conf.dof;         //can't just assign JMA_Configurations
    tree->node[0].landmark.dist = init_conf.dist;
    
    dist = 0.0;
    for( i = 0; i < tree->node[0].N_embryos; ++i )
    {
        GenerateNewEmbryo(tree,0,i);
        
        if( tree->node[0].embryo[i].dist > dist )
        {
            tree->best_embryo_id = i;
            dist = tree->node[0].embryo[i].dist;
        }
    }
    tree->max_dist = dist;
}

void PL_Juan::Explore (JMA_Roadmap_Tree* tree) const
{
  //## begin PL_Juan::Explore%929987268.body preserve=yes
  int i;
  
  /*=== place a new landmark ===*/
  (*tree).node[(*tree).n_nodes].landmark = (*tree).node[(*tree).best_father_id].embryo[(*tree).best_embryo_id];
  (*tree).node[(*tree).n_nodes].father_id = (*tree).best_father_id;
  
   
  /*=== create embryos for the new landmark ===*/
  
  (*tree).node[(*tree).n_nodes].N_embryos = NUMBER_OF_EMBRYOS;
  
  for(i=0; i< NUMBER_OF_EMBRYOS; i++)
      GenerateNewEmbryo(tree,(*tree).n_nodes,i);
  
  /*=== replace the embryo that became the new landmark ===*/
  
  GenerateNewEmbryo(tree,(*tree).best_father_id,(*tree).best_embryo_id);

  UpdateRoadmapTree(tree);
  (*tree).n_nodes++;


  //## end PL_Juan::Explore%929987268.body
}

void PL_Juan::GenerateNewEmbryo (JMA_Roadmap_Tree* tree, const int id_landmark, const int id_embryo) const
{
  //## begin PL_Juan::GenerateNewEmbryo%929987270.body preserve=yes
        //(*tree).node[id_landmark].landmark.dof //IMPROVE: this is not assigned anything yet
        ((*tree).node[id_landmark].embryo[id_embryo]).dof = GetStartConfig();   //IMPROVE: this makes the DOF equal - should go elsewhere - constructor?
        int N_Dof = (*tree).N_dof ;
        MyRandomPath( id_landmark, id_embryo, N_Dof, (*tree).node[id_landmark].landmark, &((*tree).node[id_landmark].embryo[id_embryo]) );
        
        ComputeEmbryoDistance(tree,&((*tree).node[id_landmark].embryo[id_embryo]));
  //## end PL_Juan::GenerateNewEmbryo%929987270.body
}

void PL_Juan::GetIdsPath( const JMA_Roadmap_Tree* tree, const int landmark_id, int list_ids[], int* n_landmarks)
{
    int counter = GetNAncestors( tree, landmark_id );
    *n_landmarks = counter + 1;
    
    int id = landmark_id;
    while( counter > 0 )
    { 
        list_ids[ counter ] = id;
        id = tree->node[ id].father_id;
        --counter;
    }
    if( landmark_id >= tree->n_nodes )
    {
        list_ids[ counter ] = tree->best_father_id;
    }
    else
    {
        list_ids[ counter ] = id;
    }    
}

void PL_Juan::UpdateRoadmapTree (JMA_Roadmap_Tree* tree) const
{
  //## begin PL_Juan::UpdateRoadmapTree%929987271.body preserve=yes
  int i, j;
  double distance;

  (*tree).last_max_dist = (*tree).max_dist;
  (*tree).max_dist = 0.0;
  
  for(i = 0 ; i < (*tree).n_nodes; i++)
    for(j = 0; j < (*tree).node[i].N_embryos; j++)
      {
        /*==== update the distance of embryo(i,j) w.r.t. the new landmark ====*/

        distance = MyDistanceFunc((*tree).node[i].embryo[j].dof,
        (*tree).node[(*tree).n_nodes].landmark.dof); 

        if (distance < (*tree).node[i].embryo[j].dist)
          (*tree).node[i].embryo[j].dist = distance;

        /*==== meanwhile we take advantage of this cycle and 
               find the embryo with the bigest distance !!  ====*/

        if ((*tree).max_dist < (*tree).node[i].embryo[j].dist)
          {
            (*tree).max_dist = (*tree).node[i].embryo[j].dist;
            (*tree).best_father_id = i;
            (*tree).best_embryo_id = j;
          }
      }
  //## end PL_Juan::UpdateRoadmapTree%929987271.body
}

void PL_Juan::ComputeEmbryoDistance (JMA_Roadmap_Tree* tree, JMA_Configuration* embryo) const
{
  //## begin PL_Juan::ComputeEmbryoDistance%929987273.body preserve=yes
  int i;
  double distance;
   
  (*embryo).dist = BIG_NUMBER;

  for(i=0; i < (*tree).n_nodes; i++)
    {
      /*==== compute distance w.r.t. landmark i ====*/
      distance = MyDistanceFunc((*embryo).dof,(*tree).node[i].landmark.dof); 

      if (distance < (*embryo).dist)
        (*embryo).dist = distance;
    }
  //## end PL_Juan::ComputeEmbryoDistance%929987273.body
}

void PL_Juan::CopyTrajectoryToPath ()
{
  //## begin PL_Juan::CopyTrajectoryToPath%929987275.body preserve=yes
//  int i;
//  for(i=0; i<n_landmarks-1; i++)
//    {
//      {
//      switch_draw_line((*tree).node[ids_path[i]].landmark.dof[0],
//                (*tree).node[ids_path[i]].landmark.dof[1],
//                (*tree).node[ids_path[i+1]].landmark.dof[0],
//                (*tree).node[ids_path[i+1]].landmark.dof[1],
//                THICKNESS);
  //    }
//      switch_draw_line((*tree).node[ids_path[n_landmarks-1]].landmark.dof[0],
//              (*tree).node[ids_path[n_landmarks-1]].landmark.dof[1],
//              goal_conf[0],
//              goal_conf[1],
//              THICKNESS);
//    }
  //## end PL_Juan::CopyTrajectoryToPath%929987275.body
}

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

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

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

        //output the tree structure
        fwrite( &( tree->N_dof ),                       sizeof( tree->N_dof ),                  1, outfile ) ;
        fwrite( &( tree->n_nodes ),                     sizeof( tree->n_nodes ),        1, outfile ) ;
        fwrite( &( tree->max_dist ),        sizeof( tree->max_dist ),           1, outfile ) ;
        fwrite( &( tree->last_max_dist ),       sizeof( tree->last_max_dist ),  1, outfile ) ;
        fwrite( &( tree->best_father_id ),      sizeof( tree->best_father_id ), 1, outfile ) ;
        fwrite( &( tree->best_embryo_id ),      sizeof( tree->best_embryo_id ), 1, outfile ) ;

        //output all the landmarks
        for( int i = 0; i < tree->n_nodes; i++ )
        {
                JMA_Node node = tree->node[ i ] ;
                fwrite( &node.N_embryos,                        sizeof( node.N_embryos ),                       1, outfile ) ;
                fwrite( &node.father_id,                        sizeof( node.father_id ),                       1, outfile ) ;

                //output the landmark configuration
                int j = 0 ;
                for( j = 0; j < tree->N_dof; j++ )
                {
                        fwrite( &node.landmark.dof[ j ],        sizeof( node.landmark.dof[ j ] ),       1, outfile ) ;
                }
                fwrite( &node.landmark.dist,    sizeof( node.landmark.dist ),   1, outfile ) ;

                //output the embryos
                int k = 0 ;
                for( k = 0; k < node.N_embryos; k++ )
                {
                        JMA_Configuration embryo = node.embryo[ k ] ;
                        for( j = 0; j < tree->N_dof; j++ )
                        {
                                fwrite( &embryo.dof[ j ],       sizeof( embryo.dof[ j ] ),      1, outfile ) ;
                        }
                        fwrite( &embryo.dist,   sizeof( embryo.dist ),  1, outfile ) ;
                }
        }

        //close the binary file 
        fclose( outfile ) ;

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

void PL_Juan::SetStartConfig (const Configuration& configuration)
{
    Initialize();
        Configuration oldStart = GetStartConfig();
        PL_Boolean_Output::SetStartConfig( configuration ) ;
        if( configuration != oldStart )
        {
                init_conf.dof = configuration ;
                init_conf.dist = 0 ;
                InitRoadmapTree( tree, GetStartConfig().DOF(), MAX_EMBRYOS, init_conf );
        }
}

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

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

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

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

        //input the tree structure
        size_t itemsRead ;
        itemsRead =  fread( &tree->N_dof,                   sizeof( tree->N_dof ),                      1, infile ) ;
        assert( itemsRead == 1 ) ;
        itemsRead =  fread( &tree->n_nodes,                 sizeof( tree->n_nodes ),        1, infile ) ;
        assert( itemsRead == 1 ) ;
        itemsRead =  fread( &tree->max_dist,        sizeof( tree->max_dist ),           1, infile ) ;
        assert( itemsRead == 1 ) ;
        itemsRead =  fread( &tree->last_max_dist,       sizeof( tree->last_max_dist ),  1, infile ) ;
        assert( itemsRead == 1 ) ;
        itemsRead =  fread( &tree->best_father_id,  sizeof( tree->best_father_id ),     1, infile ) ;
        assert( itemsRead == 1 ) ;
        itemsRead =  fread( &tree->best_embryo_id,  sizeof( tree->best_embryo_id ),     1, infile ) ;
        assert( itemsRead == 1 ) ;

        //input all the landmarks
        for( int i = 0; i < tree->n_nodes; i++ )
        {
                JMA_Node& node = tree->node[ i ] ;
                itemsRead =  fread( &node.N_embryos,                    sizeof( node.N_embryos ),                       1, infile ) ;
                assert( itemsRead == 1 ) ;
                itemsRead =  fread( &node.father_id,                    sizeof( node.father_id ),                       1, infile ) ;
                assert( itemsRead == 1 ) ;
                

                //input the landmark configuration
                int j = 0 ;
                node.landmark.dof.SetLength( tree->N_dof ) ;
                for( j = 0; j < tree->N_dof; ++j )
                {
                        double dof = 0 ;
                        fread( &dof,    sizeof( dof ),  1, infile ) ;
                        node.landmark.dof[ j ] = dof ;
                }
                itemsRead =  fread( &node.landmark.dist,        sizeof( node.landmark.dist ),   1, infile ) ;
                assert( itemsRead == 1 ) ;
                //input the embryos
                int k = 0 ;
                for( k = 0; k < node.N_embryos; k++ )
                {
                        JMA_Configuration& embryo = node.embryo[ k ] ;
                        node.embryo[ k ].dof.SetLength( tree->N_dof ) ;
                        for( j = 0; j < tree->N_dof; j++ )
                        {
                                double dof = 0 ;
                                size_t itemsRead = fread( &dof, sizeof( dof ),  1, infile ) ;
                                assert( itemsRead == 1 ) ;
                                embryo.dof[ j ] = dof ;
                        }
                        itemsRead =  fread( &embryo.dist,       sizeof( embryo.dist ),  1, infile ) ;
                        assert( itemsRead == 1 ) ;
                }
        }

        //close the binary file 
        fclose( infile ) ;

        SetStartConfig( tree->node[ 0 ].landmark.dof );
        semaphore.Unlock();
        return true ;
  //## end PL_Juan::Load%958953407.body
}

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

        glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

        //disable lighting
        BOOL lightingState = glIsEnabled( GL_LIGHTING );
        glDisable( GL_LIGHTING );

        //disable Z buffering too
        BOOL zbufferState = glIsEnabled( GL_DEPTH_TEST );
        glDisable( GL_DEPTH_TEST );

        //enable smooth shading too
        glShadeModel( GL_SMOOTH );
        

        int i = 0;
        glColor4d( 1.0, 1.0, 1.0, 1.0 );
        glPointSize( 3.0 );

        int dof = GetGoalConfig().DOF();

        glColor3f( 1.0f, 1.0f, 0.0f );
        glBegin( GL_POINTS );
                double g0 = 0;
                double g1 = 0;
                double g2 = 0;
                if( dof > 0 )
                {
                        g0 = GetGoalConfig()[ 0 ];
                }
                if( dof > 1 )
                {
                        g1 = GetGoalConfig()[ 1 ];
                }
                if( dof > 2 )
                {
                        g2 = GetGoalConfig()[ 2 ];
                }
                glVertex3f( g0, g1, g2 );
        glEnd();

        for( i = 0; i < tree->n_nodes; i++ )
        {
                JMA_Node node = tree->node[ i ];
                JMA_Configuration current = node.landmark;
                double v0 = 0; 
                double v1 = 0; 
                double v2 = 0; 

                v0 = current.dof[ 0 ];
                v1 = current.dof[ 1 ];
                if( dof > 2 )
                {
                        v2 = current.dof[ 2 ];
                }

                glBegin( GL_POINTS );
                        glVertex3f( v0, v1, 0 );
                glEnd();

                int parent = node.father_id;
                if( parent == -1 )
                {
                }
                else
                {
                        JMA_Node parentNode = tree->node[ parent ];
                        JMA_Configuration parentConfig = parentNode.landmark;
                        double p0 = 0;
                        double p1 = 0;
                        double p2 = 0;

                        p0 = parentConfig.dof[ 0 ];
                        p1 = parentConfig.dof[ 1 ];
                        if( dof > 2 )
                        {
                                p2 = parentConfig.dof[ 2 ];
                        }

                        glColor4d( 1.0, 1.0, 1.0, 1.0 );
                        glBegin( GL_LINES );
                                glVertex3f( v0, v1, v2 );
                                glVertex3f( p0, p1, p2 );
                        glEnd();
                }

                int j = 0;
                for( j = 0; j < node.N_embryos; j++ )
                {
                        JMA_Configuration embryoConfig = node.embryo[ j ];
                        double e0 = 0;
                        double e1 = 0;
                        double e2 = 0;

                        e0 = embryoConfig.dof[ 0 ];
                        e1 = embryoConfig.dof[ 1 ];
                        if( dof > 2 )
                        {
                                e2 = embryoConfig.dof[ 2 ];
                        }

                        //draw the line for the embryo node
                        glBegin( GL_LINES );
                                glColor4d( 1.0, 1.0, 0.0, 0.5 );
                                glVertex3f( e0, e1, e2 );
                                glColor4d( 1.0, 0.0, 0.0, 0.5 );
                                glVertex3f( v0, v1, v2 );
                        glEnd();

                }
        }
        if( lightingState != 0x00 )
        {
                glEnable( GL_LIGHTING );
        }

        //disable Z buffering too
        if( zbufferState )
        {
                glEnable( GL_DEPTH_TEST );
        }
        else
        {
                glDisable( GL_DEPTH_TEST );
        }

        semaphore.Unlock();
        return true;
  //## end PL_Juan::DrawExplicit%964739313.body
}

// Additional Declarations
  //## begin PL_Juan%376A87F4037E.declarations preserve=yes
  //## end PL_Juan%376A87F4037E.declarations

//## begin module%376A87F4037E.epilog preserve=yes
//## end module%376A87F4037E.epilog

---